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

3 files changed, 101 insertions, 13 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 230d06601..e927ae436 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -9,6 +9,7 @@
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 #include "PMR/Hardware/HardwareWinder.pb-c.h"
 #include "PMR/Hardware/HardwarePidControl.pb-c.h"
+#include "PMR/Hardware/HardwareConfiguration.pb-c.h"
 #include "PMR/Printing/JobSpool.pb-c.h"
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
@@ -73,7 +74,7 @@ extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
-uint32_t MotorsConfigMessage(HardwareMotor * request);
+uint32_t MotorsConfigMessage( HardwareConfiguration* HW_request);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadInitialTestStub(HardwareMotor * request);
 uint32_t MotorPidRequestMessage(HardwarePidControl* request);
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)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 529e4d260..78203b236 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -40,11 +40,20 @@ int32_t BreakSensordebouncetimemilli = BREAK_SENSOR_LIMIT;
 HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};
 
 //********************************************************************************************************************
-uint32_t MotorsConfigMessage(HardwareMotor * request)
+uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
 {
     uint32_t status = PASSED;
-    TimerMotors_t Motor_i;
-    Motor_i = request->hardwaremotortype;
+    TimerMotors_t Motor_i,MotorId;
+    MOTDRIVER_TYPE DriverType;
+    HardwareMotor *request;
+    if (HWrequest == NULL)
+        return ERROR;
+    for (MotorId = 0; MotorId < HWrequest->n_motors ; MotorId++)
+    {
+        request = HWrequest->motors[MotorId];
+        if (request == NULL)
+            return ERROR;
+        Motor_i = request->hardwaremotortype;
         MotorsCfg[Motor_i].configword = request->configword;
         MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype;
         MotorsCfg[Motor_i].minfrequency = request->minfrequency;
@@ -80,13 +89,21 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
         MotorsCfg[Motor_i].fnslpdec = request->fnslpdec;
         MotorsCfg[Motor_i].fsspd = request->fsspd;
 
-        status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
+        MotorsCfg[Motor_i].gatecfg1 = request->gatecfg1;
+        MotorsCfg[Motor_i].gatecfg2 = request->gatecfg2;
+        MotorsCfg[Motor_i].tvalhold = request->tvalhold;
+        MotorsCfg[Motor_i].tvalrun  = request->tvalrun ;
+        MotorsCfg[Motor_i].tvalacc  = request->tvalacc ;
+        MotorsCfg[Motor_i].tvaldec  = request->tvaldec ;
+        MotorsCfg[Motor_i].tfast    = request->tfast   ;
+        MotorsCfg[Motor_i].tonmin   = request->tonmin  ;
+        MotorsCfg[Motor_i].toffmin  = request->toffmin ;
+
+            status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
 //        if (Motor_i == MOTOR_RDRIVING)
 //            ThreadInitialTestStub(request);
-
+    }
         return status;
-//    }
-//    else return Motor_i;
 
 }
 uint32_t MotorPidRequestMessage(HardwarePidControl* request)