Version 1.4.2.0 - infrastructure for IFS, improved flash handling, thread unloading

1 files changed, 75 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 66d6baed5..9ef7f3d0d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -62,6 +62,7 @@
     uint32_t ControlId = 0xFF;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
+    uint32_t Thread_Load_Dryer_UnLoading(void);
 
     typedef struct
     {
@@ -465,6 +466,74 @@
         REPORT_MSG(LoadStages,"Loading Ended");
         return OK;
     }
+    uint32_t Thread_Load_Dryer_UnLoading(void)
+    {
+        REPORT_MSG(LoadStages, "Thread UnLoad State Machine step");
+        LoadArmInfo.LoadArmRounds = 0;
+        uint32_t numberOfSteps = 0;
+        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
+        SetOriginMotorSpeed(20);
+//        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[POOLER_MOTOR].m_params.MAX = 1;
+        MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
+        MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
+        MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
+        MotorControlConfig[POOLER_MOTOR].m_params.Ki = MotorsControl[POOLER_MOTOR].integraltime;
+        MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp;
+        MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue;
+        MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon;
+        MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype;
+        MotorControlConfig[POOLER_MOTOR].m_ingnoreValue =  MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0;
+        MotorControlConfig[POOLER_MOTOR].m_integral = 0;
+        MotorControlConfig[POOLER_MOTOR].m_isEnabled = true;
+        MotorControlConfig[POOLER_MOTOR].m_isReady = true;
+        MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0;
+        MotorControlConfig[POOLER_MOTOR].m_preError = 0;
+        MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
+
+        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),FEEDER_DANCER,POOLER_MOTOR);
+////////////////////////
+        MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
+        MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
+        MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime;
+        MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain;
+        MotorControlConfig[WINDER_MOTOR].m_params.Ki = MotorsControl[WINDER_MOTOR].integraltime;
+        MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp;
+        MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue;
+        MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon;
+        MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype;
+        MotorControlConfig[WINDER_MOTOR].m_ingnoreValue =  MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0;
+        MotorControlConfig[WINDER_MOTOR].m_integral = 0;
+        MotorControlConfig[WINDER_MOTOR].m_isEnabled = true;
+        MotorControlConfig[WINDER_MOTOR].m_isReady = true;
+        MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0;
+        MotorControlConfig[WINDER_MOTOR].m_preError = 0;
+        MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
+
+        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),FEEDER_DANCER,WINDER_MOTOR);
+
+////////////////////////
+
+        CallbackCounter++;
+        //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+        //                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
+        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
+                                        MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+
+        //Keep Notation How Many Rotations In The Dryer
+        //LoadArmInfo.LoadArmBackLash = 0;
+        LoadArmInfo.LoadArmRounds = 0xFF;
+        FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
+        return OK;
+    }
 
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
     {
@@ -481,7 +550,8 @@
             break;
         case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY
             //LoadStages++;
-            Thread_Load_Set_Load_Arm_To_Start_Position();
+            //Thread_Load_Set_Load_Arm_To_Start_Position();
+            Thread_Load_Dryer_UnLoading();
             break;
         case THREAD_LOAD_CENTER_HEAD_ROCKERS:
             //LoadStages++;
@@ -570,11 +640,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         return 0xFFFFFFFF;
     }
     index = IfIndex&0xFF;
-    /*if (index == POOLER_MOTOR)
+    if (index == POOLER_MOTOR)
     {
         //pooler dancer is right sided: data is opposite
         TranslatedReadValue = (-1*TranslatedReadValue);
-    }*/
+    }
     //TranslatedReadValue = 0;//test
     MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
     MotorSamplePointer[index]++;
@@ -589,10 +659,10 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     MotorControlConfig[index].m_mesuredParam = NormalizedError;
     MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((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
+    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)
     {