extended tracing in dispenser 3 consumption. improvements in thread loading

1 files changed, 83 insertions, 27 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index bc240cd0f..a4172185a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -103,7 +103,7 @@
     uint32_t UnloadingStart = 0;
     uint8_t NumberOfDrierLoaderCycles = 0;
     uint32_t status = OK;
-    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF;
+    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF,ThreadLoadControlId = 0xFF;
     uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF;
     uint32_t LoadCounter = 0;
     double TotalLoadedLen = 0.0;
@@ -111,6 +111,7 @@
     float numberOfSteps = 0;
     float numberOfCycles = 0;
     double DrierPrevLocation = 0;
+    int DrierCenterLocation = 0;
     FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT};
     int Screw_Dir = false;
     bool InitCalled = false;
@@ -120,6 +121,9 @@
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
     uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue);
+    uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t ReadValue);
+
+
     uint32_t Thread_Load_Dryer_UnLoading(void);
     uint32_t ThreadLoadingReport(void);
     bool ThreadLoaded(void);
@@ -200,6 +204,7 @@
         {
             LOG_ERROR (LoadStages, "Thread_Load_Reduce_Heat failed");
             strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed");
+            load.color = fastBILNK;
             LoadStatus = ERROR;
             TryAgain = true;
             //ThreadLoadButton(LoadStages);
@@ -369,7 +374,7 @@
         if (Head_Type == HEAD_TYPE_FLAT)
         {
             Report("Thread_Load_Lift_actuators",__FILE__,__LINE__,LOW,RpMessage,false,0);
-            Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP);
+            //Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP);
             //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP);
             //Trigger_Head_Actuators_Control(ACTOT, LOW,true);//UP
             Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP
@@ -393,6 +398,15 @@
             status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1);
 
         }
+        if (Head_Type == HEAD_TYPE_FLAT)
+        {
+            Report("Thread_Load_Lift_ OT actuator",__FILE__,__LINE__,LOW,RpMessage,false,0);
+            Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP);
+            //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP);
+            //Trigger_Head_Actuators_Control(ACTOT, LOW,true);//UP
+            //Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP
+        }
+
         return OK;
     }
     uint32_t Thread_Load_Lift_Rockers(void)
@@ -460,6 +474,11 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000);
+        if (Head_Type == HEAD_TYPE_FLAT)
+        {
+            Report("Thread_Load_Close_ OT actuator",__FILE__,__LINE__,LOW,RpMessage,true,0);
+            Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN);
+        }
 
         return OK;
     }
@@ -495,29 +514,24 @@
             }*/
 
         }
-        return OK;
-    }
-    uint32_t Thread_Load_Close_Lids(void)
-    {
-        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids");
-        //Close Dyeing Head Cover And Dryer Lid
-        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
-        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
         if (Head_Type == HEAD_TYPE_FLAT)
         {
             Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
 
-            Trigger_Head_Actuators_Control(ACTIN, LOW,false);//DOWN
+            //Trigger_Head_Actuators_Control(ACTIN, LOW,false);//DOWN
             //Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN);
 
             //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, DOWN);
             HeadCard_Actuators_Relocate();
         }
-        if (Head_Type != HEAD_TYPE_ARC)
-        {
-            CallbackCounter++;
-            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000);
-        }
+        return OK;
+    }
+    uint32_t Thread_Load_Close_Lids(void)
+    {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids");
+        //Close Dyeing Head Cover And Dryer Lid
+        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
+        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
         CallbackCounter++;
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
         int direction;
@@ -530,6 +544,15 @@
             direction = DRIER_LID_CLOSE;
         }
         MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
+        if (Head_Type != HEAD_TYPE_ARC)
+        {
+            CallbackCounter++;
+            if(MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000)==ERROR)
+            {
+                Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,NO_LIMIT);
+            }
+        }
+
         return OK;
     }
     uint32_t Thread_Load_Resume_Heating(void)
@@ -662,7 +685,7 @@
         {
             TryAgain = true;
             SecondTry = false;
-            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
+            Report("Loading drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded);
             TimeoutsCounter = 0;
             CallbackCounter = 0;
@@ -808,6 +831,11 @@
            RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
            SpeedTControlId = 0xFF;
         }
+        if (ThreadLoadControlId != 0xFF)
+        {
+           RemoveControlCallback(ThreadLoadControlId, ThreadUpdateCallBackFunction );
+           ThreadLoadControlId = 0xFF;
+        }
 
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING);
@@ -816,7 +844,10 @@
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LDANCER1);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
-        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Stop);
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_WINDER,Hard_Hiz);
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,Hard_Hiz);
         if (LoadStages!=THREAD_LOAD_END)
         {
             usnprintf(LoadErrorMsg, 100, "Load sequence stopped by user on stage %d",LoadStages);
@@ -845,14 +876,14 @@
         bool direction;
         numberOfCycles++;
         uint32_t temp = Read_Dryer_ENC_Position();
-        Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
+        Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,AccumulatedArmMovement,temp,RpMessage,DrierPrevLocation,0);
         //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
         if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
         {
             Report("Thread_Load_Dryer_UnLoading_Callback wrong state",__FILE__,__LINE__,LoadStages,RpMessage,DrierPrevLocation,0);
             return OK;
         }
-        if ((abs (temp -DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY)) // OK - take another round
+        if ((AccumulatedArmMovement>8000  )&&(ReadValue == NOTBUSY)) // OK - take another round
         {
             DrierPrevLocation = temp;
             Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,numberOfCycles,LoadArmRounds,RpMessage,0,0);
@@ -871,14 +902,14 @@
         }
         else  //timeout or no movement
         {
-            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
+            Report("Unloading drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             if (SecondTry == true)
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
             else
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
-            if (abs(temp -DrierPrevLocation)<200)
+            if (abs(temp -DrierCenterLocation)<200)
             {
-                if (temp<DrierPrevLocation)
+                if (temp<DrierCenterLocation)
                     direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
                 else
                     direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
@@ -1010,6 +1041,7 @@
             }
         }
         MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
+        DrierCenterLocation = DrierPrevLocation;
         DrierPrevLocation = temp;
         Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0);
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
@@ -1029,10 +1061,12 @@
         return OK;
     }
 
+    double percentage;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
     {
         REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
         //ThreadLoaded();
+        percentage = ReadValue*7;
         ThreadLoadingReport();
 
         if (LoadStages!=ReadValue)
@@ -1117,6 +1151,7 @@
             break;
         case THREAD_LOAD_END:
             LoadStages = THREAD_LOAD_END;
+            percentage = 100.0;
             Thread_Load_End();
             break;
         default:
@@ -1147,20 +1182,28 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
         {
             LoadStatus = OK;
             SecondTry = true;
-            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            load.color = BLINK;
+            Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
             ThreadLoadStateMachine(LoadStages);
         }
         else if (SecondTry == true)
         {
             SecondTry = false;
-            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            Report("Calling State machine  2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            load.color = BLINK;
             ThreadLoadStateMachine(LoadStages+1);
         }
+        else //((SecondTry == false)&&(TryAgain == false))???????
+        {
+            Report("State ERROR with wrong parameters",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            SecondTry = false;
+        }
     }
     else
     {
         Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0);
         ThreadLoadStateMachine(LoadStages);
+        SecondTry = false;
     }
     return OK;
 }
@@ -1469,9 +1512,14 @@ uint32_t ThreadLoadingReport(void)
             response.errorreason = LoadErrorMsg;
         }
     }
-    else return ERROR;
+    else
+    {
+        Report("ThreadLoadingReport ERROR",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0);
+        return ERROR;
+    }
     Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0);
-
+    response.has_progresspercentage = true;
+    response.progresspercentage =  percentage;
     //-------------------------------------------------------------------------------------------
     responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size);
     responseContainer.has_continuous = true;
@@ -1504,11 +1552,19 @@ uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer)
     ThreadLoadButton(LoadStages);
     return OK;
 }
+uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
+{
+
+    ThreadLoadingReport();
+    return OK;
+}
+
 uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer)
 {
     StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     ustrncpy (ThreadLoadingToken, requestContainer->token,36);
     //MessageState = 1;
+    ThreadLoadControlId = AddControlCallback("ThreadLoad", ThreadUpdateCallBackFunction, 4* eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
     Report("StartThreadLoadingFunc",__FILE__,__LINE__,0,RpWarning,(int)LoadStages,0);
     ThreadLoadingReport();
     return OK;