change kval for rockers before and after moving. handle microstep temporal changes

3 files changed, 46 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 8ddc2f84d..ef7352ec3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -33,7 +33,7 @@
 #include "StateMachines/Printing/PrintingSTM.h"
 
 char HomingToken[NUM_OF_MOTORS][36+1]={0};
-
+uint8_t Microstepkeep[NUM_OF_MOTORS];
 /********************************************************************************
  * Motor Homing
  ********************************************************************************/
@@ -41,6 +41,12 @@ uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
 {
     MessageContainer responseContainer;
     MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
+    if((deviceID == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)||(deviceID == HARDWARE_MOTOR_TYPE__MOTO_RLOADING))
+    {
+        MotorSetMicroStep(deviceID, Microstepkeep[deviceID]);
+        //MotorSetKvalRun(deviceID, MotorsCfg[deviceID].kvalrun);
+        Task_sleep(10);
+    }
 
     responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
     responseContainer.continuous = true;
@@ -95,6 +101,14 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
                 }
                 else
                 {
+                    if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING))
+                    {
+                        Microstepkeep[MotorId] = MotorsCfg[MotorId].microstep;
+                        MotorSetMicroStep(MotorId, 1);
+                        //MotorSetKvalRun(MotorId, 35);
+                        Task_sleep(10);
+                    }
+
                     if ( Motor_Id_to_LS_IdUp[MotorId] != MAX_GPI)
                     {
                         MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdDown[MotorId], MotorHomingRequestCallback,10000);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
index 80db2a4e6..50b02a634 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
@@ -69,6 +69,7 @@ uint32_t DispenserHomingTime[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0};
 double   InitialDispenserPressure = INITIAL_DISPENSER_PRESSURE;
 uint32_t InitialDispenserTimeout = INITIAL_DISPENSER_TIMEOUT_LIMIT;
 uint32_t InitialDispenserTimeLag = INITIAL_DISPENSER_TIMEOUT;
+uint8_t MicrostepDispenserkeep[MAX_SYSTEM_DISPENSERS];
 
 void IDS_Dispenser_SetBackLashValues(double   initialdispenserpressure,   uint32_t initialdispensertimeout,   uint32_t initialdispensertimelag)
 {
@@ -84,7 +85,8 @@ uint32_t IDS_HomeDispenserBackMoveCallback(uint32_t deviceID, uint32_t ReadValue
     if ((GetDispenserPressure(DispenserId)>=InitialDispenserPressure)||(DispenserHomingTime[DispenserId]>InitialDispenserTimeout))
     {
         MotorStop(deviceID,Hard_Hiz);
-        MotorSetMicroStep(deviceID, MotorsCfg[deviceID].microstep);
+
+        MotorSetMicroStep(deviceID, MicrostepDispenserkeep[DispenserId]);
         HomingActive[DispenserId]= false;
         Report("End backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
         if (SafeRemoveControlCallback(DispenserHomingControlId[DispenserId], IDS_HomeDispenserBackMoveCallback )==OK)
@@ -147,7 +149,8 @@ uint32_t IDS_HomeDispenser (uint32_t deviceID, uint32_t speed , callback_fptr ca
         //open dispenser valve dispenser to midtank direction
         Control3WayValvesWithCallback ((Valves_t)deviceID, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
         //Valve_Set((Valves_t) request->index, MidTank_Dispenser);
-        MotorSetMicroStep(deviceID, 1);
+        MicrostepDispenserkeep[deviceID] = MotorsCfg[MotorId].microstep;
+        MotorSetMicroStep(MotorId, 1);
         SysCtlDelay(180000);
         //open dry air valve in the dispenser
         Valve_Set(IDS_Id_to_AirValve[deviceID], Atm_MidTank_ON);
@@ -192,7 +195,8 @@ uint32_t IDS_StopHomeDispenser (uint32_t deviceID)
     Disable_MidTank_Pressure_Reading(deviceID);
     Valve_Set(IDS_Id_to_AirValve[deviceID], Atm_MidTank_OFF);
     Control3WayValvesWithCallback ((Valves_t)deviceID, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
-    MotorSetMicroStep(deviceID, MotorsCfg[deviceID].microstep);
+
+    MotorSetMicroStep(MotorId, MicrostepDispenserkeep[deviceID]);
     return OK;
 }
 
@@ -232,7 +236,9 @@ uint32_t IDS_EmptyDispenser (uint32_t deviceID, uint32_t speed , callback_fptr c
         //open dispenser valve dispenser to midtank direction
         Control3WayValvesWithCallback ((Valves_t)deviceID, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
         //Valve_Set((Valves_t) request->index, Dispenser_Mixer);
-        MotorSetMicroStep(deviceID, 1);
+        MicrostepDispenserkeep[deviceID] = MotorsCfg[MotorId].microstep;
+
+        MotorSetMicroStep(MotorId, 1);
         SysCtlDelay(180000);
         //open dry air valve in the dispenser
         //Valve_Set(IDS_Id_to_AirValve[deviceID], Atm_MidTank_ON);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 74a07e99c..53ab0e8a2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -200,6 +200,11 @@
     }
     uint8_t CallbackCounter = 0;
     uint8_t TimeoutsCounter = 0;
+    uint8_t LeftMicrostepKeep = 0;
+    uint8_t LeftkvalKeep = 0;
+    uint8_t RightMicrostepKeep = 0;
+    uint8_t RightkvalKeep = 0;
+
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
     {
         Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
@@ -222,6 +227,13 @@
                 else
                 {
                     LoadStages++;
+                    if (LoadStages == THREAD_LOAD_LIFT_ROCKERS)
+                    {
+                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, RightMicrostepKeep);
+                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, LeftMicrostepKeep);
+                        //MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, RightkvalKeep);
+                        //MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, LeftkvalKeep);
+                    }
                     if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
                     {
                         //ThreadLoadStateMachine(LoadStages);
@@ -270,6 +282,15 @@
     //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        LeftMicrostepKeep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep;
+        LeftkvalKeep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].kvalrun;
+        RightMicrostepKeep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
+        RightkvalKeep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1);
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
+        //MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 35);
+        //MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 35);
+        Task_sleep(10);
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
         CallbackCounter++;