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

3 files changed, 43 insertions, 23 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 3a7045a6e..c912bca21 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -85,5 +85,8 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 
 void SetOriginMotorSpeed(float process_speed);
 
+uint32_t ThreadPrepare_Tension (int DancerId, double tension);
+
+
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 2c8db8b6c..01c70d42f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -116,6 +116,7 @@
     bool InitCalled = false;
     uint32_t LoadArmRounds;
 
+    int CurrentlyLoaded = 0;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
     uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue);
@@ -210,7 +211,7 @@
         {
             if(Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)
             {
-                WHS_Set_SetPoint_Q_value(headairflow/2);
+                WHS_Set_SetPoint_Q_value(headairflow*2/3);
             }
             else
             {
@@ -236,6 +237,7 @@
         //storeLoadArmParameters();
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
         SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+        MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
 
         return OK;
     }
@@ -461,23 +463,35 @@
     uint32_t Thread_Load_Close_Dancers(void)
     //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
     {
+        uint32_t  current = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Dancers");
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
-        CallbackCounter++;
-        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);
 
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
-        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
-        status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension);
-        if (Is_PP_Machine() == true) //PP machine - new LTFU
+        status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,&current);
+
+        ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
+/*        if (current!=windertension)
         {
             CallbackCounter++;
-            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500);
-            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000);
-            status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension);
-        }
+            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);
+            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
+            status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension);
+        }*/
+        if (Is_PP_Machine() == true) //PP machine - new LTFU
+        {
+            ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
+            /*status |= MCU_E2PromRead(EEPROM_PULLER_TENSION_POSITION,&current);
+            if (current!=pullertension)
+            {
+                CallbackCounter++;
+                MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500);
+                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000);
+                status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension);
+            }*/
 
+        }
         return OK;
     }
     uint32_t Thread_Load_Close_Lids(void)
@@ -507,7 +521,8 @@
         {
             Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
             Trigger_Head_Actuators_Control(ACTOT, LOW,false);
-            Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+            //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+            HeadCard_Actuators_Relocate();
         }
         return OK;
     }
@@ -515,6 +530,7 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating");
         ActivateHeadMagnet();
+        HeadCard_Actuators_Relocate();
 
         if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
         {
@@ -624,6 +640,7 @@
             {
                 return OK;
             }
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded);
             if (numberOfCycles<LoadArmRounds)
             {
                 MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
@@ -632,7 +649,6 @@
             else
             {
                 MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
-                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
         }
@@ -641,7 +657,7 @@
             TryAgain = true;
             SecondTry = false;
             Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
-            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded);
             TimeoutsCounter = 0;
             CallbackCounter = 0;
             load.color = fastBILNK;
@@ -672,7 +688,6 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Dryer_Loading");
 
-        int CurrentlyLoaded = 0;
         if (dryerbufferlength)
             LoadArmRounds = (int)dryerbufferlength;
         if (LoadArmRounds <= 2)
@@ -810,7 +825,8 @@
         }
         Screw_Dir = 1-Screw_Dir;
         // movement of the screw while unloading the thread from the dryer to the spool
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000);
+        AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF);  //handle alarm detection and operation
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000);
 
         return OK;
     }
@@ -832,6 +848,7 @@
             Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,numberOfCycles,LoadArmRounds,RpMessage,0,0);
             if (numberOfCycles<LoadArmRounds)
             {
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
                 MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                        numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 10000);
             }
@@ -852,9 +869,9 @@
             if (abs(temp -DrierPrevLocation)<200)
             {
                 if (temp<DrierPrevLocation)
-                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
-                else
                     direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                else
+                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
                 MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction);
 
             }
@@ -1239,15 +1256,15 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
 
     NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-    if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
-    {
-        NormalizedError = (-1*NormalizedError);
-    }
     MotorControlConfig[index].m_mesuredParam = NormalizedError;
     MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                           &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
+    if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+    {
+        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+    }
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    //if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index e1f887303..39ff91558 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -145,7 +145,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
-    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2);
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }