From 4ac616ace9a815921bb380d7dd7d67c3aae0543d Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 3 May 2020 15:10:47 +0300 Subject: support init state messages and some bug fixed --- .../Embedded/Modules/Thread/ThreadLoad.c | 382 +++++++++------------ 1 file changed, 165 insertions(+), 217 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 1181082ea..cdcecaa2a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -34,6 +34,7 @@ #include "Drivers/I2C_Communication/I2C.h" #include #include +#include "drivers/FPGA/FPGA_SPI_Comm.h" #include "PMR/Stubs/StubHeatingTestRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestResponse.pb-c.h" @@ -48,24 +49,40 @@ //#include //#include + static char* LoadStagesStr[15] = { + /*00*/ "INIT", + /*01*/ "REDUCE_HEAT", + /*02*/ "LIFT_ROCKERS", + /*03*/ "SET_LOAD_ARM", + /*04*/ "OPEN_COVERS", + /*05*/ "LIFT_DANCERS", + /*06*/ "INITIAL_TENSION", + /*07*/ "CLOSE_ROCKERS", + /*08*/ "CLOSE_DANCERS", + /*09*/ "CLOSE_LIDS", + /*10*/ "JOG_FEEDER", + /*11*/ "DRYER_LOADING", + /*12*/ "RESUME_HEATING", + /*13*/ "JOG_THREAD" , + /*14*/ "END"}; /* typedef enum { - 0 THREAD_LOAD_INIT, - 1 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - 2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION, //USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY - 3 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID - 4 THREAD_LOAD_LIFT_DANCERS, - 5 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - 6 THREAD_LOAD_INITIAL_TENSION, //CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND - 7 THREAD_LOAD_CLOSE_ROCKERS, - 8 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD - 9 THREAD_LOAD_CLOSE_LIDS, - 10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT, //JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION - 11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 - //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER + 00 THREAD_LOAD_INIT, + 01 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, + 02 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE + 03 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY + 04 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID + 05 THREAD_LOAD_LIFT_DANCERS, + 06 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND + 07 THREAD_LOAD_CLOSE_ROCKERS, + 08 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD + 09 THREAD_LOAD_CLOSE_LIDS, + 10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION + 11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 + //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER 12 THREAD_LOAD_RESUME_HEATING, - 13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING + 13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING 14 THREAD_LOAD_END }THREAD_LOAD_STAGES_ENUM;*/ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; @@ -88,28 +105,20 @@ uint32_t LoadCounter = 0; double TotalLoadedLen = 0.0; double LengthCalculationMultiplier; + float numberOfSteps = 0; + float numberOfCycles = 0; + double DrierPrevLocation = 0; + FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT}; + int Screw_Dir = false; + bool InitCalled = false; + uint32_t LoadArmRounds; + 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 Thread_Load_Dryer_UnLoading(void); uint32_t ThreadLoadingReport(void); bool ThreadLoaded(void); - - typedef struct - { - uint32_t LoadArmRounds; - uint32_t LoadArmBackLash; - }LoadArmInfoStruc; - LoadArmInfoStruc LoadArmInfo; - //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; - - //RUN MOTOR TO SWITCH WITH TIMEOUT - //RUN MOTOR TO BREAK SENSOR - //RUN MOTOR TO DANCER LOCATION (POOLER GOES TO THE OTHER DIRECTION) - - //RUN MOTOR A FULL CYCLE - //RUN A MOTOR NUMBER OF STEPS - //RUN CONTROL FOR A SINGLE DANCER bool ThreadLoadingActive(void) { if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END)) @@ -117,44 +126,24 @@ else return false; } - bool InitCalled = false; uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage) { //void* buffer = NULL; //uint32_t Bytes = 0; //FRESULT Fresult = FR_OK; - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Init"); load.color = BLINK; LoadStatus = OK; - LoadArmInfo.LoadArmBackLash = MCU_E2PromDryerBacklashRead(); - REPORT_MSG(LoadArmInfo.LoadArmBackLash, "Thread Load MCU_E2PromDryerBacklashRead"); - if (LoadArmInfo.LoadArmBackLash > 10) - LoadArmInfo.LoadArmBackLash = 5; - //LoadArmInfo.LoadArmRounds = 0xFF; + //LoadArmRounds = 0xFF; //EEPROM_STORAGE_DRYER_CYCLES - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); - //if (LoadArmInfo.LoadArmRounds <= 2) - // LoadArmInfo.LoadArmRounds = 20; - Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,0); - - /*Fresult = f_mkdir(SW_INFO_DIR); - if ((Fresult == FR_OK)||(Fresult == FR_EXIST)) - { + Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmRounds,RpWarning,0,0); - Fresult = FileRead(LoadArmPath, &Bytes, &buffer); - if (Fresult == FR_OK) - { - if (Bytes>=sizeof(LoadArmInfo)) - memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo)); - free (buffer); - } - }*/ - - Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpMessage,LoadArmInfo.LoadArmRounds,0); + Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmRounds,RpMessage,LoadArmRounds,0); StopInitSequence(); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); @@ -163,6 +152,10 @@ LoadStages++; ThreadLoadStateMachine(LoadStages); } + else + { + Report("Thread_Load_Init another state ",__FILE__,__LINE__,Stage,RpMessage,LoadArmRounds,0); + } InitCalled = true; return OK; @@ -171,7 +164,7 @@ { //Heaters Off, Dryer Blower Off, Blower Low, - REPORT_MSG(LoadStages, "Thread Load State Machine step "); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Reduce_Heat"); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); ProcessParametersClear.dryerzone1temp = 0; ProcessParametersClear.dryerzone2temp = 0; @@ -197,8 +190,8 @@ ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { - LOG_ERROR (LoadStages, "Thread_Load_Init failed"); - strcpy(LoadErrorMsg,"Thread_Load_Init failed"); + LOG_ERROR (LoadStages, "Thread_Load_Reduce_Heat failed"); + strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed"); LoadStatus = ERROR; TryAgain = true; } @@ -216,36 +209,15 @@ } return OK; } - /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag) - { - // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - - /`*NumberOfDrierLoaderCycles--; - if (NumberOfDrierLoaderCycles) - { - Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); - } - else*`/ - { - Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home - LoadStages++; - ThreadLoadStateMachine(LoadStages); - } - return OK; - }*/ uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { - if(PullerControlId != 0xFF) + /*if(PullerControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); PullerControlId = 0xFF; - } + }*/ if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); @@ -273,64 +245,6 @@ ThreadLoadStateMachine(LoadStages); return OK; } - /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void) - { - uint32_t numberOfSteps = 0; - REPORT_MSG(LoadStages, "Thread Load State Machine step"); - if (LoadArmInfo.LoadArmRounds != 0xFF) - { - //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 - numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0); - NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds; - status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), - numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); - } - else //number of circles is not known - compare to position of the motor to verify location at the stopper - { - Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); - - } - //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly - return OK; - }*/ - uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition) - { - if (Condition == true) - { - CallbackCounter--; - Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - LoadStages++; - ThreadLoadStateMachine(LoadStages); - } - else - { - CallbackCounter--; - Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - if (SecondTry == true) - { - LoadStages++; - ThreadLoadStateMachine(LoadStages); - LoadStatus = OK; - } - else - { - TimeoutsCounter = 0; - load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!"); - LoadStatus = ERROR; - ThreadLoadingReport(); - TryAgain = true; - } - } - - return OK; - } - uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0); @@ -362,10 +276,11 @@ { if (TimeoutsCounter) { - LOG_ERROR(LoadStages,"Load sequence timeout"); TimeoutsCounter = 0; load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId); + usnprintf(LoadErrorMsg, 100, "Load sequence timeout %s motor %d",LoadStagesStr[LoadStages], MotorId); + Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); + LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; @@ -394,18 +309,9 @@ LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); return OK; } - /*uint32_t Thread_Load_Center_Head_Rockers(void) - { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); - CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 80, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000); - CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 80, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000); - return OK; - }*/ uint32_t Thread_Load_Open_Covers(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Open_Covers"); //Open Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, @@ -415,13 +321,14 @@ CallbackCounter++; //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + Report("Thread_Load_Lift_actuators",__FILE__,__LINE__,LOW,RpMessage,false,0); Trigger_Head_Actuators_Control(ACTOT, LOW,false); Trigger_Head_Actuators_Control(ACTIN, LOW,false); return OK; } uint32_t Thread_Load_Lift_Dancers(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Dancers"); /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40); CallbackCounter++; @@ -456,7 +363,7 @@ CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Rockers"); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70); @@ -471,7 +378,7 @@ } uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Initial_Tension"); load.color = BLINK; if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { @@ -497,7 +404,7 @@ MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70); - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Rockers"); CallbackCounter++; 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++; @@ -505,11 +412,10 @@ return OK; } - uint32_t Thread_Load_Close_Dancers(void) //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Dancers"); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); CallbackCounter++; @@ -530,7 +436,7 @@ } uint32_t Thread_Load_Close_Lids(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + 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, @@ -539,18 +445,20 @@ 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); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0); Trigger_Head_Actuators_Control(ACTOT, LOW,true); Trigger_Head_Actuators_Control(ACTIN, LOW,true); return OK; } uint32_t Thread_Load_Resume_Heating(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating"); ActivateHeadMagnet(); if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) { - LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed"); + usnprintf(LoadErrorMsg, 100, "Load sequence error %d",LoadStagesStr[LoadStages]); + Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } else { @@ -560,15 +468,49 @@ } return OK; } + uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition) + { + if (Condition == true) + { + CallbackCounter--; + Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + //storeLoadArmParameters(); + LoadStages++; + ThreadLoadStateMachine(LoadStages); + } + else + { + CallbackCounter--; + Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + //storeLoadArmParameters(); + if (SecondTry == true) + { + LoadStages++; + ThreadLoadStateMachine(LoadStages); + LoadStatus = OK; + } + else + { + TimeoutsCounter = 0; + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!"); + Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + LoadStatus = ERROR; + ThreadLoadingReport(); + TryAgain = true; + } + } + + return OK; + } uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void) //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position { - REPORT_MSG(LoadStages, "Thread Load State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Feeder_To_Middle_Point"); CallbackCounter++; - MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 400, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,4000); + MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 400, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,6000); return OK; } - uint32_t Thread_Load_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); @@ -605,9 +547,6 @@ return OK; } - float numberOfSteps = 0; - float numberOfCycles = 0; - double DrierPrevLocation = 0; uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { numberOfCycles++; @@ -623,7 +562,7 @@ { return OK; } - if (numberOfCycles1000 )&&(ReadValue == NOTBUSY)) + 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 { DrierPrevLocation = temp; - if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION) - { - return OK; - } - if (numberOfCyclesprocessparameters) { dryerbufferlength = request->processparameters->dryerbufferlength; - LoadArmInfo.LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + LoadArmRounds = (int)(request->processparameters->dryerbufferlength); } Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); -- cgit v1.3.1