diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 213 |
1 files changed, 152 insertions, 61 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 3af29a043..1c28a5447 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -35,26 +35,26 @@ //#include <stdint.h> //#include <stdbool.h> - typedef enum +/* typedef enum { - THREAD_LOAD_INIT, - THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY - THREAD_LOAD_CENTER_HEAD_ROCKERS, //puthead cleaning rockers to middle position - THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID - THREAD_LOAD_LIFT_DANCERS, - THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND - THREAD_LOAD_CLOSE_ROCKERS, - THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD - THREAD_LOAD_CLOSE_LIDS, - THREAD_LOAD_RESUME_HEATING, - THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION - THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 + 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_CENTER_HEAD_ROCKERS, //puthead cleaning rockers to middle position + 4 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID + 5 THREAD_LOAD_LIFT_DANCERS, + 6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE + 7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND + 8 THREAD_LOAD_CLOSE_ROCKERS, + 9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD + 10 THREAD_LOAD_CLOSE_LIDS, + 11 THREAD_LOAD_RESUME_HEATING, + 12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION + 13 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 - THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING - THREAD_LOAD_END - }THREAD_LOAD_STAGES_ENUM; + 14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING + 15 THREAD_LOAD_END + }THREAD_LOAD_STAGES_ENUM;*/ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; ProcessParameters ProcessParametersClear,ProcessParametersRecover; uint8_t NumberOfDrierLoaderCycles = 0; @@ -69,7 +69,7 @@ uint32_t LoadArmBackLash; }LoadArmInfoStruc; LoadArmInfoStruc LoadArmInfo; - char LoadArmPath[50] = "0://SWInfo//LoadArm.msg"; + char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; //RUN MOTOR TO SWITCH WITH TIMEOUT //RUN MOTOR TO BREAK SENSOR @@ -83,8 +83,9 @@ void* buffer = NULL; uint32_t Bytes = 0; FRESULT Fresult = FR_OK; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); - LoadArmInfo.LoadArmBackLash = 0; + LoadArmInfo.LoadArmBackLash = 10; LoadArmInfo.LoadArmRounds = 0xFF; Fresult = f_mkdir(SW_INFO_DIR); if ((Fresult == FR_OK)||(Fresult == FR_EXIST)) @@ -99,6 +100,7 @@ } } + Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); LoadStages++; @@ -108,7 +110,9 @@ } uint32_t Thread_Load_Reduce_Heat(void) { + //Heaters Off, Dryer Blower Off, Blower Low, + REPORT_MSG(LoadStages, "Thread Load State Machine step "); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); ProcessParametersClear.dryerzone1temp = 0; ProcessParametersClear.dryerzone2temp = 0; @@ -129,52 +133,66 @@ { Turn_the_Blower_On();//Turn on with the Default_Voltage if (BlowerCfg.heatingvoltage) - Cancel_Gradual_Increase_Blower(BlowerCfg.heatingvoltage); + Control_Voltage_To_Blower(BlowerCfg.heatingvoltage); else - Cancel_Gradual_Increase_Blower(BlowerCfg.voltage-500); + Control_Voltage_To_Blower(BlowerCfg.voltage-500); Control_Dryer_Fan(STOP,75);//use START or STOP, 0 - 100% LoadStages++; - ThreadLoadStateMachine(LoadStages); + //ThreadLoadStateMachine(LoadStages); } 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--; + /*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 + else*/ { + Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); //storeLoadArmParameters(); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home LoadStages++; - ThreadLoadStateMachine(LoadStages); + //ThreadLoadStateMachine(LoadStages); } return OK; } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { +// Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); LoadStages++; - ThreadLoadStateMachine(LoadStages); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + + //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), - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); + 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/10, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,LoadArmInfo.LoadArmBackLash,1000); + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, 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 @@ -183,19 +201,18 @@ uint8_t CallbackCounter = 0; uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { + Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id"); - if(ControlId != 0xFF) - { - RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); - ControlId = 0xFF; - } if (CallbackCounter) { CallbackCounter--; if (CallbackCounter == 0) { LoadStages++; - ThreadLoadStateMachine(LoadStages); + if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call + { + //ThreadLoadStateMachine(LoadStages); + } } } else @@ -204,6 +221,7 @@ } 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, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000); CallbackCounter++; @@ -212,6 +230,7 @@ } uint32_t Thread_Load_Open_Covers(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); //Open Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, @@ -220,30 +239,33 @@ MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000); 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); - MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 2, 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,4000); return OK; } uint32_t Thread_Load_Lift_Dancers(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,20000); return OK; } uint32_t Thread_Load_Lift_Rockers(void) //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 700, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 700, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); return OK; } 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"); if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); @@ -257,10 +279,11 @@ } uint32_t Thread_Load_Close_Rockers(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); return OK; } @@ -268,15 +291,17 @@ 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"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); + 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,4000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,10000); return OK; } uint32_t Thread_Load_Close_Lids(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); //Close Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, @@ -284,12 +309,13 @@ MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000); 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); - MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 0, 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,4000); return OK; } uint32_t Thread_Load_Resume_Heating(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); ActivateHeadMagnet(); if (HandleProcessParameters(&ProcessParametersRecover)!= OK) @@ -300,25 +326,80 @@ { Control_Dryer_Fan(START,75);//use START or STOP, 0 - 100% LoadStages++; - ThreadLoadStateMachine(LoadStages); + //ThreadLoadStateMachine(LoadStages); } 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"); CallbackCounter++; MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000); return OK; } + + uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) + { + Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id"); + if(ControlId != 0xFF) + { + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); + ControlId = 0xFF; + } + if (CallbackCounter) + { + CallbackCounter--; + if (CallbackCounter == 0) + { + LoadStages++; + if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call + { + //ThreadLoadStateMachine(LoadStages); + } + } + } + else + LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); + return OK; + } + uint32_t Thread_Load_Dryer_Loading(void) { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); LoadArmInfo.LoadArmRounds = 20; + uint32_t numberOfSteps = 0; //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 - OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; + SetOriginMotorSpeed(19); +// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; +// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; + + 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; + MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1; + MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1; + MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime; + MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain; + MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime; + MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp; + MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue; + MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon; + MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype; + MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue = MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses + MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0; + MotorControlConfig[FEEDER_MOTOR].m_integral = 0; + MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true; + MotorControlConfig[FEEDER_MOTOR].m_isReady = true; + MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0; + MotorControlConfig[FEEDER_MOTOR].m_preError = 0; + MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage + MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize); + ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); + CallbackCounter++; MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds, Thread_Load_HomingCallback, 100000); + numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); //Keep Notation How Many Rotations In The Dryer //LoadArmInfo.LoadArmBackLash = 0; @@ -328,17 +409,19 @@ } uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue) { - RemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop ); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop ); ControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; - ThreadLoadStateMachine(LoadStages); + //ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_Jog_Thread(void) //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading { - ThreadJoggingFunc(40); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + ThreadJoggingFunc(20); ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); return OK; } @@ -424,17 +507,18 @@ return OK; } -uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue) +uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int i; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0; + int index=MAX_THREAD_MOTORS_NUM; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; - DancerId = ThreadMotorIdToDancerId[index]; + DancerId = FEEDER_DANCER;//ThreadMotorIdToDancerId[index]; if (ReadValue < 10) { Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); @@ -445,11 +529,17 @@ uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue) return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) + if (IfIndex>>8 != IfTypeThread) + { + LOG_ERROR (IfIndex, "Wrong Interface type"); + return 0xFFFFFFFF; + } + index = IfIndex&0xFF; + /*if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - } + }*/ //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -490,14 +580,15 @@ void ThreadLoadRequest(MessageContainer* requestContainer) container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); - int LoadStages = (int)request->dryerzone1temp; - ThreadLoadStateMachine(LoadStages); + int LoadStage = (int)request->dryerzone1temp; + LoadStages = LoadStage; + ThreadLoadStateMachine(LoadStage); - //if (status) + /*if (status) { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; - } + }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); //USBCDCD_sendData(container_buffer, container_size,10); @@ -524,11 +615,11 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer) container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); - +/* { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; - } + }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars(container_buffer, container_size); |