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

5 files changed, 36 insertions, 12 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 34395b3e6..58ade0802 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -40,6 +40,7 @@
         THREAD_LOAD_INIT,
         THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
         THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
+        THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
         THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
         THREAD_LOAD_LIFT_DANCERS,
         THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
@@ -201,11 +202,20 @@
             LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
         return OK;
     }
+    uint32_t Thread_Load_Center_Head_Rockers(void)
+    {
+        CallbackCounter++;
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
+        CallbackCounter++;
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000);
+        return OK;
+    }
     uint32_t Thread_Load_Open_Covers(void)
     {
         //Open Dyeing Head Cover And Dryer Lid
         //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
         //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
+        DeActivateHeadMagnet();
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
@@ -279,6 +289,8 @@
     }
     uint32_t Thread_Load_Resume_Heating(void)
     {
+        ActivateHeadMagnet();
+
         if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
         {
             LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
@@ -352,6 +364,10 @@
             //LoadStages++;
             Thread_Load_Set_Load_Arm_To_Start_Position();
             break;
+        case THREAD_LOAD_CENTER_HEAD_ROCKERS:
+            //LoadStages++;
+            Thread_Load_Center_Head_Rockers();
+            break;
         case THREAD_LOAD_OPEN_COVERS:  //OPEN DYEING HEAD COVER AND DRYER LID
             //LoadStages++;
             Thread_Load_Open_Covers();
@@ -416,7 +432,6 @@ uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue)
     //double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
-    char Message[60];
 
     DancerId = ThreadMotorIdToDancerId[index];
     if (ReadValue < 10)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index bbb05e727..becdf054a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -217,7 +217,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
+            //REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
 
         }
     }
@@ -226,7 +226,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
             ScrewNumberOfSteps++;
-            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
+            //REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
         }
     }
     if (WinderMotorSpeedRollOver)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 511a4ef93..56b229ea8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -43,4 +43,7 @@ uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId);
 uint32_t Winder_End(void);
 
+void ThreadLoadPollRequest(MessageContainer* requestContainer);
+void ThreadLoadRequest(MessageContainer* requestContainer);
+
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index f525e4a04..04a0026bd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -85,6 +85,9 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     memcpy (&MotorsControl[Motor_i],request,sizeof(HardwarePidControl));
     if (MotorsControl[Motor_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
         MotorsControl[Motor_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+#ifdef TEST_LONGER_PID_THREAD
+    MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
+#endif
     for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index c48eec522..1cb854f87 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -305,7 +305,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
 }
 //double eNormalizedError[100];
 //int    TranslatedreadValue[100];
-#define MAX_THREAD_CONTROL_LOG 100
+/*#define MAX_THREAD_CONTROL_LOG 100
 double calculatedError[MAX_THREAD_CONTROL_LOG+1];
 double NormError[MAX_THREAD_CONTROL_LOG+1];
 double mIntegral[MAX_THREAD_CONTROL_LOG+1];
@@ -313,7 +313,7 @@ int    MotorId[MAX_THREAD_CONTROL_LOG+1];
 int    readValue[MAX_THREAD_CONTROL_LOG+1];
 int    AveragereadValue[MAX_THREAD_CONTROL_LOG+1];
 int    calculatedspeed[MAX_THREAD_CONTROL_LOG+1];
-int    timestamp[MAX_THREAD_CONTROL_LOG+1];
+int    timestamp[MAX_THREAD_CONTROL_LOG+1];*/
 int controlIndex = 0;
 bool keepdata = true;
 /*int32_t KeepReadValue = 0;
@@ -342,7 +342,7 @@ void testDancersControl()
 }*/
 bool dancerinvalid = false;
 int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
-char Message[60];
+char TMessage[60];
 uint16_t BreakSensorCounter = 0;
 uint16_t BreakSensorLatchCounter = 0;
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
@@ -358,7 +358,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
-    char Message[60];
 
     if (ThreadControlActive == false)
         return OK;
@@ -403,6 +402,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
             MotorSamplePointer[index] = 0;
+#ifdef TEST_LONGER_PID_THREAD
+        else // test: handle tension once in pvinputfilterfactormode milliseconds
+            return OK;
+#endif
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
@@ -447,13 +450,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
         {
             keepdata = false;
-            usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
+            usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
             //JobAbortedByUser = true;
             ThreadControlActive = false;
             //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
             JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
-            SendJobProgress(0.0,0,false, Message);
-            //EndState(CurrentJob,Message );
+            SendJobProgress(0.0,0,false, TMessage);
+            //EndState(CurrentJob,TMessage );
             SegmentReady(Module_Thread,ModuleFail);
             AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE,true);
             LOG_ERROR (DancerId, "Dancer Failure");
@@ -475,7 +478,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
         {
-            if (keepdata == true)
+            /*if (keepdata == true)
             {
                  calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
                     MotorId[controlIndex] = index;
@@ -488,7 +491,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                     timestamp[controlIndex] = msec_millisecondCounter;
                  if (controlIndex++>=MAX_THREAD_CONTROL_LOG)
                      controlIndex = 0;
-            }
+            }*/
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
         }