Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

5 files changed, 47 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index 1140027c2..ed0deadee 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -609,7 +609,10 @@ void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId)//Mov
         speed_calc = 0x3FF;
     temp |= speed_calc<<8;
 
-    MillisecWriteToMotor(_motorId, temp, 4, NULL);
+    if (_motorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
+        MillisecSetScrewSpeed(temp, 4, NULL);
+    else
+        MillisecWriteToMotor(_motorId, temp, 4, NULL);
 }
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index e1f8a593a..6f234e26a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -77,6 +77,7 @@ uint32_t DrawerFansStatus = 0;
 
 bool watchdogCriticalAlarm = false;
 
+MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
@@ -209,6 +210,29 @@ int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 
     return OK;
 }
+
+int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
+{
+    //==========================
+    MSBacklog[MsecLogindex]=Data;
+    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MsecLogindex++;
+    if (MsecLogindex>=199)
+        MsecLogindex = 0;
+    //==========================
+
+    ScrewSetMaxSpeedPending.Callback = Callback;
+    ScrewSetMaxSpeedPending.Data = Data;
+    ScrewSetMaxSpeedPending.Length = Length;
+    ScrewSetMaxSpeedPending.DataRequired = false;
+    if (ScrewSetMaxSpeedPending.Active == false)
+    {
+        MotorActivity++;
+        ScrewSetMaxSpeedPending.Active = true;
+    }
+
+    return OK;
+}
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (MotorId >= NUM_OF_MOTORS) return -1;
@@ -332,7 +356,17 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
-            if (ScrewMovePending.Active == true)
+
+            if (ScrewSetMaxSpeedPending.Active == true)
+            {
+                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
+                MotorActivity--;
+                ScrewSetMaxSpeedPending.Active = false;
+                if (ScrewSetMaxSpeedPending.Callback)
+                    ScrewSetMaxSpeedPending.Callback(Motor_i,0);
+
+            }
+            else if (ScrewMovePending.Active == true)
             {
                 MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
                 MotorActivity--;
@@ -438,7 +472,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
         {
-            Read_MidTank_Pressure_Sensor(i);
+            Read_MidTank_Pressure_Sensor(Disp_i);
         }
         for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index fe46018dc..517716c08 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -16,6 +16,7 @@
 typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback);
+int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback);
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 int MillisecFlushMsgQ(TimerMotors_t MotorId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 644918394..934467277 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -249,7 +249,7 @@ void DiagnosticTenMsecCollection(void)
     DiagnosticLoadDancer(WINDER_DANCER,Control_Read_Dancer_Position(WINDER_DANCER, 0,0));
     DiagnosticLoadDancer(POOLER_DANCER,Control_Read_Dancer_Position(POOLER_DANCER, 0,0));
     DiagnosticLoadDancer(FEEDER_DANCER,Control_Read_Dancer_Position(FEEDER_DANCER, 0,0));
-//    DiagnosticLoadSpeedSensor(getSensorSpeedData());
+    DiagnosticLoadSpeedSensor(getSensorSpeedData());
 
 }
 void Diagnostic100msecCollection(void)
@@ -261,7 +261,7 @@ void Diagnostic100msecCollection(void)
 //    dancer1angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(WINDER_DANCER, 0,0);
 //    dancer2angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(POOLER_DANCER, 0,0);
 //    dancer3angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(FEEDER_DANCER, 0,0);
-    DiagnosticLoadSpeedSensor(getSensorSpeedData());
+
     //if (JobIsActive())
     {
 /*        DiagnosticLoadMotor(FEEDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)); ThreadGetMotorSpeed
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 0272255a5..c7d5998c6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -317,6 +317,9 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     if (ThreadControlActive == false)
         return OK;
+    if (PrepareState == true)
+        return OK;
+
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -584,6 +587,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
         LOG_ERROR (dyeingspeed," job speed zero");
         return ERROR;
     }
+    LOG_ERROR (dyeingspeed," ThreadPreSegmentState");
 
     SetOriginMotorSpeed(process_speed);
     ThreadControlActive = true;