Update Actuator + ControlLowLoop

Stub: stubManager.Run<ProgressResponse>("ProgressRequest" ,0xFEAD, 0xCAF101);
//Actuators: ID, power, Direction (01 = UP)

4 files changed, 21 insertions, 16 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
index d31d0479a..cc4b34857 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
@@ -324,28 +324,33 @@ uint32_t HeadCard_Valve_Control(bool Direction)
 }
 */
 uint32_t ActControlId = 0xFF;
-ACTUATORS Global_Act_ID;
 
-uint32_t HeadCard_ActuatorsCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
+char Global_Act_Cmd_flag = 0; //Sync - prevent reset from previous command
+
+
+uint32_t HeadCard_ActuatorsCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) // use IfIndex for Act_ID
 {
     uint32_t status = OK;
 
     static uint8_t Counter = 0;
 
+    static char Act_Cmd_flag = 0;
+
+    Act_Cmd_flag++;//prevent reset from previous command
+
     uint32_t Motion_Status = STOPPED;
 
-    ACTUATORS Act_ID = Global_Act_ID;
+    ACTUATORS Act_ID = (ACTUATORS) IfIndex;
 
     if (SafeRemoveControlCallback(ActControlId, HeadCard_ActuatorsCallBackFunction )==OK)
         ActControlId = 0xFF;
     else
         Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)HeadCard_ActuatorsCallBackFunction,0);
-   ///**************   do your stuff
 
    //Read movement status
     Motion_Status = HeadCard_Actuators_Status(Act_ID);
 
-    if(Motion_Status == MOVING)
+    if((Motion_Status == MOVING) && (Global_Act_Cmd_flag == Act_Cmd_flag))
     {
         //RST
         Head_I2C_EXP2_0x42.bits.OUTPUT_ACTLOW_ACT_RST = 0x00;//Disable movement
@@ -374,8 +379,6 @@ uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
 {
     uint32_t status = OK;
 
-    Global_Act_ID = Act_ID;
-
     if(Act_ID == ACTIN)//0
     {
         Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTIN_ON  = LOW;
@@ -389,8 +392,7 @@ uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
 
     status |= Head_Write_IO_Reg(0x46,LOW_AND_HIGH);
 
-    //Task_sleep(500);
-    delayms(750);
+    Task_sleep(500);
 
     if(Act_ID == ACTIN)//0
     {
@@ -402,8 +404,10 @@ uint32_t HeadCard_Actuators_Control(ACTUATORS Act_ID, bool Direction)
     }
     status |= Head_Write_IO_Reg(0x46,HIGH);
 
-    //5sec delay - Timeout
-    ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (2* eOneSecond), TemplateDataReadCBFunction,0,0, 0 );
+    Global_Act_Cmd_flag++;//Sync the callback to the command
+
+    //3sec delay - Timeout
+    ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (3* eOneSecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
 
     return status;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index ba11ff58c..bea68e314 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -585,7 +585,7 @@ uint32_t ControlLowLoop(uint32_t tick)
     {
         if (ControlArray[ControlLowDevice_i].ControlActive)
         {
-            if (tick == ControlArray[ControlLowDevice_i].StartTick)
+            if (tick - ControlArray[ControlLowDevice_i].StartTick<=skipped_ticks)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 4f5bec4f0..36d4505d0 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -315,10 +315,11 @@ uint32_t DiagnosticsLoadDigitalValues(void)
     DigitalOutputState[index++].value = ReadHeadMagnetBit();
 
     DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_LED3;
-    DigitalOutputState[index++].value = HeadCard_Actuators_Status(ACTIN);
+    //DigitalOutputState[index++].value = HeadCard_Actuators_Status(ACTIN);
 
     DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPO_LED4;
-    DigitalOutputState[index++].value = HeadCard_Actuators_Status(ACTOT);
+    //  indication of movement cause the switch in the tech board to change, also doesn't go through the task
+    //DigitalOutputState[index++].value = HeadCard_Actuators_Status(ACTOT);
 
     DigitalOutputState[index].interfaceio = INTERFACE_IOS__GPI_WCONTAINER_FULL;
     DigitalOutputState[index++].value = WHS_IsContainerFull();
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 677d3fa5f..51fe807d8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -391,8 +391,8 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
 
         if((request -> delay & 0xFFF000) == 0xCAF000)//Actuator
         {
-            //HeadCard_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), /*(bool)((request -> delay & 0x10)>>4), */(bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
-            Trigger_Head_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), (bool)((request -> delay & 0x10)>>4), (bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
+            HeadCard_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), /*(bool)((request -> delay & 0x10)>>4), */(bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
+            //Trigger_Head_Actuators_Control((ACTUATORS)(((request -> delay) & 0x100)>>8), (bool)((request -> delay & 0x10)>>4), (bool)((request -> delay & 0x01)));//Actuators: ID, power, Direction
             //response.progress = HeadCard_Actuators_Status((ACTUATORS)((request -> delay) & 0x100)>>8);
             response.has_progress = true;
         }