Version 1.4.6.29 feeder dancer tension, loading arm homing, buttons fixed,

5 files changed, 206 insertions, 66 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 49058c359..d81ab7abb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -39,7 +39,6 @@
 char HomingToken[NUM_OF_MOTORS][36+1]={0};
 int HomingCounter[NUM_OF_MOTORS];
 uint32_t HomingControlId[NUM_OF_MOTORS];
-int NumberOfCycles;
 
 /********************************************************************************
  * Motor Homing
@@ -96,10 +95,10 @@ uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
         RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport);
         HomingControlId[deviceID] = 0xff;
     }
-    if (deviceID == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
+    /*if (deviceID == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
     {
         MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,NumberOfCycles);//it takes two cycles to identify a stop of the arm
-    }
+    }*/
     if (HomingToken[deviceID][0] == 0)
         return OK;
     responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
@@ -114,10 +113,67 @@ uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
 return OK;
 
 }
+uint32_t LoadArmRounds;
+
+uint32_t D_numberOfSteps = 0;
+float D_numberOfCycles = 0;
+double D_DrierPrevLocation = 0;
+
+uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
+{
+    Report("Diagnostics_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,D_numberOfCycles,RpMessage,0,0);
+    //storeLoadArmParameters();
+
+    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+    if (HomingControlId[deviceID] != 0xff)
+    {
+        Report("MotorHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0);
+        RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport);
+        HomingControlId[deviceID] = 0xff;
+    }
+    return OK;
+}
+
+uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
+{
+    D_numberOfCycles++;
+    uint32_t temp = Read_Dryer_ENC_Position();
+    Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0);
+    //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0);
+    if ((abs (temp -D_DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY)) // OK - take another round
+    {
+        D_DrierPrevLocation = temp;
+        Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,D_numberOfCycles,LoadArmRounds,RpMessage,0,0);
+        if (D_numberOfCycles<LoadArmRounds)
+        {
+            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                                   D_numberOfSteps,    Diagnostics_Dryer_UnLoading_Callback, 10000);
+        }
+        else //done enough cycles, go to the center point
+        {
+            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,30000);
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles);
+            Report("Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
+        }
+    }
+    else  //timeout or no movement
+    {
+        Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
+        MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm
+        Report("Drier unloading timeout(1) or no movement",__FILE__,temp,D_DrierPrevLocation,RpWarning,ReadValue,0);
+        if (HomingControlId[MotorId] != 0xff)
+        {
+            Report("MotorHomingProgressReport stopped",__FILE__,__LINE__,MotorId,RpMessage,HomingCounter[MotorId],0);
+            RemoveControlCallback(HomingControlId[MotorId],MotorHomingProgressReport);
+            HomingControlId[MotorId] = 0xff;
+        }
+    }
+    return OK;
+}
+
 uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
 {
-    uint32_t numberOfSteps = 0;
-    uint32_t LoadArmRounds = (int)dryerbufferlength;
+    //uint32_t numberOfSteps = 0;
     uint32_t status = FAILED;
     MessageContainer responseContainer;
 
@@ -125,6 +181,8 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
 
     MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
 
+    LoadArmRounds = (int)dryerbufferlength;
+
     int direction;
     TimerMotors_t MotorId = (TimerMotors_t)request->motortype;
     if (LoadArmRounds <= 2)
@@ -157,14 +215,16 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
                 {
                     if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
                     {
-                        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-                        Report("MotorHomingRequestFunc Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,LoadArmRounds,0);
-                        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, MotorHomingRequestCallback,0,1000);
-                        NumberOfCycles = 0;
+                        D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+                        Report("MotorHomingRequestFunc Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_numberOfSteps,RpMessage,LoadArmRounds,0);
+                        //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, MotorHomingRequestCallback,0,1000);
+                        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                                               D_numberOfSteps-800,    Diagnostics_Dryer_UnLoading_Callback, 10000);
+                        D_numberOfCycles = 0;
 
                         //MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
-                        //                                numberOfSteps, MotorHomingRequestCallback,1000);
+                        //                                D_numberOfSteps, MotorHomingRequestCallback,1000);
                         status = OK;
                     }
                     if ( Motor_Id_to_LS_IdDown[MotorId] != MAX_GPI)
@@ -208,15 +268,15 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
                         if (LoadArmRounds <= 2)
                                 LoadArmRounds = 30;
 
-                        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-                        numberOfSteps -= 100;
-                        NumberOfCycles = LoadArmRounds;
-                        Report("MotorHomingRequestFunc Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,LoadArmRounds,0);
+                        D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+                        D_numberOfSteps -= 100;
+                        D_numberOfCycles = LoadArmRounds;
+                        Report("MotorHomingRequestFunc Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_numberOfSteps,RpMessage,LoadArmRounds,0);
 
                         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4);
 
                         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                                               numberOfSteps,    MotorHomingRequestCallback, 100000);
+                                               D_numberOfSteps,    MotorHomingRequestCallback, 100000);
                         status = OK;
                     }
                     if ( Motor_Id_to_LS_IdUp[MotorId] != MAX_GPI)
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index e2ee53645..3406a75ae 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -597,11 +597,12 @@ uint32_t MidTankDataSetupFunc(MessageContainer* requestContainer)
     }
     //debug only
     Task_sleep(1000);
+    ADC_MUX_Init();
     for (i = 0;i<8;i++)
     {
         float a=0,b=0;
-        a = MCU_E2PromReadMidtank_A(i);
-        b = MCU_E2PromReadMidtank_B(i);
+        a = Initial_Offset_A[i];
+        b = Slope_B[i];
         Report("MidTankData Read", __FILE__,__LINE__,(int)(a*1000), RpMessage, (int)(b*1000), 0);
     }
     //debug only
diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.c b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
index 7326dc039..f81cfb88f 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/buttons.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
@@ -58,8 +58,8 @@ uint8_t  thraedJogging(uint8_t off);
 
 uint32_t ButtonLoadCallBackFunction(uint32_t IfIndex, uint32_t ReadValue);
 uint32_t ButtonLoadCBFunction(uint32_t IfIndex, uint32_t ReadValue);
-uint32_t LoadStatMachine( button *pBtn);
-uint32_t LoadLongPress( button *pBtn);
+//uint32_t LoadStatMachine( button *pBtn);
+//uint32_t LoadLongPress( button *pBtn);
 bool get_pbAction ( button *pBtn, PB_Status ReadValue);
 
 #define LOADLONGPRESS 3
@@ -152,14 +152,37 @@ bool get_pbAction (button *pBtn, PB_Status ReadValue )
             }
             else
             {
+                if (pBtn->count > SECOND1)
+                {
+                    pBtn->Action = SHORTPB;
+                    ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: COUNTPB -> SHORTPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+                }
+                else
+                {
+                    pBtn->Action = OFFPB;
+                    ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: COUNTPB -> OFFPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+                }
                 pBtn->count = 0 ;
-                pBtn->Action = SHORTPB;
-                ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: COUNTPB -> SHORTPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
             }
             break;
         case SHORTPB:
-            pBtn->Action = OFFPB;
-            ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: SHORTPB -> OFFPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+            if (ReadValue == PRESS)
+            {
+                pBtn->count += 1 ;
+                if (pBtn->count > SECOND5)
+                {
+                    pBtn->Action = LONGPB;
+                    ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: COUNTPB -> LONGPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+                }
+            }
+            else
+            {
+                pBtn->Action = OFFPB;
+                ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: COUNTPB -> OFFPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+                pBtn->count = 0 ;
+            }
+            //pBtn->Action = OFFPB;
+            //ReportWithPackageFilter(GeneralFilter,"------------ get_pbAction: SHORTPB -> OFFPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
             break ;
         case LONGPB:
             if (ReadValue == RELEASE)
@@ -328,7 +351,7 @@ uint32_t ButtonJogCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 //            }
             break;
         case sttJOGGING:
-            if (jog.Action == SHORTPB) //PB is OFF
+            if (jog.Action == OFFPB) //PB is OFF
             {
                 ReportWithPackageFilter(GeneralFilter,"------------ joggingMachine: sttJOGGING action SHORTPB, sttJOGGING -> sttENABLE,  stop JOGGING----------------", __FILE__,__LINE__,jog.state, RpMessage, jog.Action, 0);
                 ThreadAbortJoggingFunc(); 
@@ -373,11 +396,15 @@ uint32_t ButtonLoadCBFunction(uint32_t IfIndex, uint32_t ReadValue)
    {
        case SHORTPB:
            ReportWithPackageFilter(GeneralFilter,"------------ thread loading SHORTPB  -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-           LoadStatMachine( &load );
+           if (ThreadLoadingActive() == true)
+               ThreadLoadButton(0);  //fix failures and continue of planned stop
            break;
        case LONGPB:
            ReportWithPackageFilter(GeneralFilter,"------------ thread loading LONGPB  -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-           LoadLongPress( &load );
+           if (ThreadLoadingActive() == false)
+               ThreadLoadButton(0); //INIT
+           else
+               Thread_Load_End(); //END
            break;
        case REPLONGPB:
            ReportWithPackageFilter(GeneralFilter,"------------ thread loading REPLONGPB please release the PB  -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
@@ -573,77 +600,80 @@ return OK;
 /*------     THREAD LOAD/UNLOAD        ----------*/
 
 
-uint32_t LoadStatMachine( button *pBtn)
+/*uint32_t LoadStatMachine( button *pBtn)
 {
 
     ReportWithPackageFilter(GeneralFilter,"------------ (state) (color) -----------------", __FILE__,__LINE__,pBtn->state, RpMessage, pBtn->color, 0);
     switch (pBtn->state)
     {
         case sttRDY :
-//            ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-///*            switch (pBtn->Action)
-//            {
-//                case (SHORTPB):
-//                case (LONGPB):
-//                case (REPLONGPB):
-//                    pBtn->state = sttPRELOAD;
-//                    pBtn->color =  BLINK;*/
-//                    if (ThreadLoadButton( THREAD_LOAD_INIT)== OK)
-//                    {
-//                        pBtn->state = sttPRELOAD ;    // to do
+            ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+            switch (pBtn->Action)
+            {
+                case (SHORTPB):
+                //case (LONGPB):
+                case (REPLONGPB):
+                    pBtn->state = sttPRELOAD;
+//                    pBtn->color =  BLINK;
+                    if (ThreadLoadButton( THREAD_LOAD_INIT)== OK)
+                    {
+                        pBtn->state = sttPRELOAD ;    // to do
 //                        pBtn->color =  colorON;
 //                        Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
-//                    }
+                    }
 //                    else
 //                    {
 //                       // pBtn->state = sttDISABLE ;
 //                        Pannel_Leds(THREAD_LOAD, MODE_OFF);//AVI+
 //                    }
-//                    /*break;
-//                default :
-//                    break;
-//            }*/
+                    break;
+                case (LONGPB):
+                    Thread_Load_End();
+                        break;
+                default :
+                    break;
+            }
             break;
         case sttPRELOAD:
             ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttPRELOAD -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-            /*switch (pBtn->Action)
+            /`*switch (pBtn->Action)
             {
                 case (SHORTPB):
                 case (LONGPB):
                 case (REPLONGPB):
                     pBtn->color =  BLUE;
-                    Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+*/
+                    Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+*`/
                     if (ThreadLoadButton( THREAD_LOAD_INITIAL_TENSION)== OK)
                     {
                         pBtn->state = sttRDY ;    // to do
-                        pBtn->color =  colorON;
-                        Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+//                        pBtn->color =  colorON;
+//                        Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
                     }
                     else
                     {
                         //pBtn->state = sttLOADFAIL ;
-                        pBtn->color = fastBILNK ;    // to do
+//                        pBtn->color = fastBILNK ;    // to do
                     }
-                    /*break;
+                    /`*break;
                 default :
                     break;
-            }*/
+            }*`/
             break;
         case sttLOADING:
             ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttLOADING -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-            pBtn->color =  fastBILNK;
-            Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+//            pBtn->color =  fastBILNK;
+//            Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
             if (ThreadLoadButton( THREAD_LOAD_END)== OK)
             {
                 pBtn->state = sttRDY ;    // to do
-                pBtn->color =  colorON;
-                Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+//                pBtn->color =  colorON;
+//                Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
             }
             else
             {
                 pBtn->state = sttRDY ;    // to do
                 //pBtn->state = sttLOADFAIL ;
-                pBtn->color = fastBILNK ;    // to do
+//                pBtn->color = fastBILNK ;    // to do
             }
             break;
 //        case sttLOADSUCSESS ?????
@@ -659,7 +689,7 @@ uint32_t LoadStatMachine( button *pBtn)
             break;
     }
     return OK;
-}
+}*/
 
 void Ink_Cart_Led()//temporary for ITMA
 {
@@ -699,10 +729,11 @@ void test_avi()
 
 }
 
-uint32_t LoadLongPress( button *pBtn)
+/*uint32_t LoadLongPress( button *pBtn)
 {
     ReportWithPackageFilter(GeneralFilter,"------------ load.state = LoadLongPress -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-    switch (pBtn->state)
+    Thread_Load_End();
+/`*    switch (pBtn->state)
     {
         case sttRDY :
             ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
@@ -720,7 +751,7 @@ uint32_t LoadLongPress( button *pBtn)
             break;
         default:
             Thread_Load_End();
-    }
+    }*`/
     return OK;
-}
+}*/
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index d504ceebd..f34e56470 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -715,8 +715,11 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
         Report("Setting pressure from RML feeder tension",__FILE__,__LINE__,(int)(pressurebuildup*100),RpWarning,(int)(DispenserPreparePressure*100),0);
         DispenserPreparePressure = pressurebuildup;
     }
-    if (JobTicket->headcleaningparameters->has_cleanerflow)
-            cleanerFlow =  JobTicket->headcleaningparameters->cleanerflow;
+    if (JobTicket->headcleaningparameters)
+    {
+        if (JobTicket->headcleaningparameters->has_cleanerflow)
+                cleanerFlow =  JobTicket->headcleaningparameters->cleanerflow;
+    }
     if ((cleanerFlow>200)&&(cleanerFlow<1150))
     {
         Report("Setting pressure from RML feeder tension",__FILE__,__LINE__,(int)(cleanerFlow),RpWarning,(int)CleaningDispenserSpeed,0);
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;
 }