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

1 files changed, 48 insertions, 3 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index f0e459514..edfc517bf 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -814,14 +814,55 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
 
     return OK;
 }
+bool RTFU_Up = false;
+
+uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag)
+{
+    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
+    Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0);
+
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands
+    return OK;
+}
+uint32_t Release_Right_TFU_Tension()
+{
+    uint32_t status = OK;
+    if (RTFU_Up == true)
+    {
+        Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0);
+        RTFU_Up = false;
+        status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4, Release_Right_TFU_TensionCallback,1000);
+    }
+
+    return status;
+}
+uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
+{
+    Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0);
+    RTFU_Up = true;
+    return OK;
+}
+
+uint32_t Adjust_Right_TFU_Tension(double tension)
+{
+    uint32_t status = OK;
+    if (tension > 0.5) //0 = lower position, 1 = high position
+    {
+        if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
+        {
+            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 40,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
+            Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,GPI_LS_RDANCER_UP,0);
+        }
+    }
+
+    return status;
+}
 uint32_t ThreadPrepare_Tension (int DancerId, double tension)
 {
     int current, request = (int)tension,movement;
     int HW_Motor_Id;
     bool direction;
     uint32_t status = OK, address = 0;
-    if (tension < 100)
-        return OK; //do not handle tension of zero
     switch (DancerId)
     {
     case HARDWARE_DANCER_TYPE__LeftDancer:
@@ -835,10 +876,13 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
         HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2;
         break;
     case HARDWARE_DANCER_TYPE__RightDancer:
-        return ERROR;
+        return Adjust_Right_TFU_Tension(tension);
+        break;
     default:
         return ERROR;
     }
+    if (tension < 100)
+        return OK; //do not handle tension of zero
     status |= MCU_E2PromRead(address,&current);
     if ((status!= OK )||(current == 0xFFFF))
         return status;
@@ -1304,6 +1348,7 @@ char Endstr[150];
      }
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+     Release_Right_TFU_Tension();
      IDS_StopLubrication();
     return OK;
 }