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

1 files changed, 77 insertions, 24 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 0bc2d04de..8792470d2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -37,23 +37,23 @@
 
 /*    typedef enum
     {
-        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
-        THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
-        THREAD_LOAD_CLOSE_ROCKERS,
-        THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
-        THREAD_LOAD_CLOSE_LIDS,
-        THREAD_LOAD_RESUME_HEATING,
-        THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
-        THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
+        0 THREAD_LOAD_INIT,
+        1 THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
+        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
+        3 THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
+        4 THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
+        5 THREAD_LOAD_LIFT_DANCERS,
+        6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
+        7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
+        8 THREAD_LOAD_CLOSE_ROCKERS,
+        9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
+        10 THREAD_LOAD_CLOSE_LIDS,
+        11 THREAD_LOAD_RESUME_HEATING,
+        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+        13 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                     //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
-        THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
-        THREAD_LOAD_END
+        14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
+        15 THREAD_LOAD_END
     }THREAD_LOAD_STAGES_ENUM;*/
     THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
     ProcessParameters ProcessParametersClear,ProcessParametersRecover;
@@ -104,7 +104,7 @@
         memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
         //NumberOfDrierLoaderCycles = loadLoadArmParameters();
         LoadStages++;
-        //ThreadLoadStateMachine(LoadStages);
+        ThreadLoadStateMachine(LoadStages);
 
         return OK;
     }
@@ -146,14 +146,14 @@
     {
        // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
 
-        NumberOfDrierLoaderCycles--;
+        /*NumberOfDrierLoaderCycles--;
         if (NumberOfDrierLoaderCycles)
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                             MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
         }
-        else
+        else*/
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             //storeLoadArmParameters();
@@ -177,13 +177,16 @@
     }
     uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
     {
+        uint32_t numberOfSteps = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         if (LoadArmInfo.LoadArmRounds != 0xFF)
         {
-            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+            //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
+            numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0);
             NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds;
             status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
-                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
+                                            numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
         }
         else //number of circles is not known - compare to position of the motor to verify location at the stopper
         {
@@ -297,7 +300,7 @@
         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,4000);
         CallbackCounter++;
-        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,20000);
+        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,10000);
 
         return OK;
     }
@@ -340,18 +343,68 @@
         MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000);
         return OK;
     }
+
+    uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
+    {
+        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
+        if(ControlId != 0xFF)
+        {
+            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
+            ControlId = 0xFF;
+        }
+        if (CallbackCounter)
+        {
+            CallbackCounter--;
+            if (CallbackCounter == 0)
+            {
+                LoadStages++;
+                if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+                {
+                    //ThreadLoadStateMachine(LoadStages);
+                }
+            }
+        }
+        else
+            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
+        return OK;
+    }
+
     uint32_t Thread_Load_Dryer_Loading(void)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         LoadArmInfo.LoadArmRounds = 20;
         uint32_t numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
-        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
+        SetOriginMotorSpeed(15);
+//        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
+//        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
+
         numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+        MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
+        MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime;
+        MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp;
+        MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue;
+        MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon;
+        MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype;
+        MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue =  MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_integral = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true;
+        MotorControlConfig[FEEDER_MOTOR].m_isReady = true;
+        MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_preError = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize);
+
         ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         CallbackCounter++;
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                               numberOfSteps,    Thread_Load_HomingCallback, 100000);
+                               numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
 
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmInfo.LoadArmBackLash = 0;