aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
diff options
context:
space:
mode:
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
-rw-r--r--Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c236
1 files changed, 175 insertions, 61 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 9ef7f3d0d..7d4b66dfb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -48,18 +48,24 @@
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
+ 11 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT, //JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+ 12 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
+ 13 THREAD_LOAD_RESUME_HEATING,
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;
+
+ int32_t keepmicrostep;
+ int32_t keepkvalrun,keepkvalhold;
+ uint8_t CallbackCounter = 0;
+ uint8_t TimeoutsCounter = 0;
+
uint8_t NumberOfDrierLoaderCycles = 0;
uint32_t status = OK;
- uint32_t ControlId = 0xFF;
+ uint32_t ControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF;
uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
uint32_t Thread_Load_Dryer_UnLoading(void);
@@ -93,7 +99,7 @@
FRESULT Fresult = FR_OK;
REPORT_MSG(LoadStages, "Thread Load State Machine step");
- LoadArmInfo.LoadArmBackLash = 10;
+ LoadArmInfo.LoadArmBackLash = 5;
LoadArmInfo.LoadArmRounds = 0xFF;
Fresult = f_mkdir(SW_INFO_DIR);
if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
@@ -109,6 +115,7 @@
}
Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
+ StopInitSequence();
memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
//NumberOfDrierLoaderCycles = loadLoadArmParameters();
LoadStages++;
@@ -133,6 +140,7 @@
ProcessParametersClear.headzone5temp = 0;
ProcessParametersClear.headzone6temp = 0;
ProcessParametersClear.dyeingspeed = 40;
+ ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength;
if (HandleProcessParameters(&ProcessParametersClear)!= OK)
{
LOG_ERROR (LoadStages, "Thread_Load_Init failed");
@@ -146,7 +154,7 @@
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;
}
@@ -163,24 +171,44 @@
}
else*/
{
- Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+ 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);
+ ThreadLoadStateMachine(LoadStages);
}
return OK;
}
uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
{
+
+ if(PullerControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
+ PullerControlId = 0xFF;
+ }
+ if(WinderControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
+ WinderControlId = 0xFF;
+ }
+ /*Task_sleep(5)
+ MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+ MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
+
+ MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
+
// 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);
+ CallbackCounter = 0;
+ Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
NumberOfDrierLoaderCycles=0;
//storeLoadArmParameters();
LoadStages++;
SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home
- //ThreadLoadStateMachine(LoadStages);
+ ThreadLoadStateMachine(LoadStages);
return OK;
}
uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
@@ -206,13 +234,21 @@
//Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
return OK;
}
- uint8_t CallbackCounter = 0;
- uint8_t TimeoutsCounter = 0;
+ uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag)
+ {
+ CallbackCounter--;
+ Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+ //storeLoadArmParameters();
+ LoadStages++;
+ ThreadLoadStateMachine(LoadStages);
+
+ return OK;
+ }
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");
+ Report("Thread_Load_HomingCallback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+// REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
if (CallbackCounter)
{
CallbackCounter--;
@@ -231,16 +267,20 @@
else
{
LoadStages++;
- if (LoadStages == THREAD_LOAD_LIFT_ROCKERS)
+ if (LoadStages == THREAD_LOAD_CLOSE_ROCKERS)
+ {
+ MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepmicrostep);
+ MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepmicrostep);
+ MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepkvalrun);
+ MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepkvalrun);
+ }
+ if (LoadStages == THREAD_LOAD_CLOSE_DANCERS)
{
- MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
- MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
- MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
- MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
+ MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold);
}
if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
{
- //ThreadLoadStateMachine(LoadStages);
+ ThreadLoadStateMachine(LoadStages);
}
}
}
@@ -275,26 +315,30 @@
uint32_t Thread_Load_Lift_Dancers(void)
{
REPORT_MSG(LoadStages, "Thread Load State Machine step");
+ keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
+ MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
CallbackCounter++;
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_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,20000);
+ 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,25000);
return OK;
}
uint32_t Thread_Load_Lift_Rockers(void)
//Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
{
+ keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
+ keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
REPORT_MSG(LoadStages, "Thread Load State Machine step");
- MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1);
- MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
- MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 35);
- MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 35);
+ MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
+ MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
+ MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70);
+ MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70);
Task_sleep(10);
CallbackCounter++;
- MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 50, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
+ MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,35000);
CallbackCounter++;
- MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 50, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000);
+ MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,35000);
return OK;
}
@@ -316,9 +360,9 @@
{
REPORT_MSG(LoadStages, "Thread Load State Machine step");
CallbackCounter++;
- MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
+ 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, 300, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,12000);
+ 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);
return OK;
}
@@ -361,7 +405,7 @@
{
Control_Dryer_Fan(START,75);//use START or STOP, 0 - 100%
LoadStages++;
- //ThreadLoadStateMachine(LoadStages);
+ ThreadLoadStateMachine(LoadStages);
}
return OK;
}
@@ -370,15 +414,14 @@
{
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);
+ MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,2000);
return OK;
}
- uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
+ uint32_t Thread_Load_Dryer_MovetoEncoderPosition_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");
- MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,0,0);
+ Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+ MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
if(ControlId != 0xFF)
{
MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
@@ -393,26 +436,45 @@
LoadStages++;
if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
{
- //ThreadLoadStateMachine(LoadStages);
+ ThreadLoadStateMachine(LoadStages);
}
}
}
else
LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
+
+ return OK;
+ }
+ uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
+ {
+ Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+ Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,__LINE__,MotorId,RpMessage,CallbackCounter,0);
+ /*if(ControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
+ ControlId = 0xFF;
+ }*/
+ MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
return OK;
}
uint32_t Thread_Load_Dryer_Loading(void)
{
REPORT_MSG(LoadStages, "Thread Load State Machine step");
- LoadArmInfo.LoadArmRounds = 20;
- uint32_t numberOfSteps = 0;
+ LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+ if (LoadArmInfo.LoadArmRounds <= 2)
+ LoadArmInfo.LoadArmRounds = 20;
+ float numberOfSteps = 0;
//Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
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;
+ numberOfSteps -= 100;
+ //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+ Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0);
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;
@@ -432,15 +494,19 @@
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);
+ MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4);
+
ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+ Report("AddControlCallback feeder",__FILE__,__LINE__,ControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0);
CallbackCounter++;
+ Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000);
//Keep Notation How Many Rotations In The Dryer
//LoadArmInfo.LoadArmBackLash = 0;
- LoadArmInfo.LoadArmRounds = 0xFF;
- FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
+ LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+ //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
return OK;
}
uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
@@ -450,7 +516,7 @@
ControlId = 0xFF;
ThreadAbortJoggingFunc();
LoadStages++;
- //ThreadLoadStateMachine(LoadStages);
+ ThreadLoadStateMachine(LoadStages);
return OK;
}
uint32_t Thread_Load_Jog_Thread(void)
@@ -464,6 +530,25 @@
uint32_t Thread_Load_End(void)
{
REPORT_MSG(LoadStages,"Loading Ended");
+ if(ControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
+ ControlId = 0xFF;
+ }
+ if(PullerControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
+ PullerControlId = 0xFF;
+ }
+ if(WinderControlId != 0xFF)
+ {
+ MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
+ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
+ WinderControlId = 0xFF;
+ }
+ LoadStages = THREAD_LOAD_INIT;
return OK;
}
uint32_t Thread_Load_Dryer_UnLoading(void)
@@ -472,7 +557,7 @@
LoadArmInfo.LoadArmRounds = 0;
uint32_t numberOfSteps = 0;
//Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
- SetOriginMotorSpeed(20);
+ SetOriginMotorSpeed(19);
// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
// CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
@@ -481,7 +566,7 @@
MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
- MotorControlConfig[POOLER_MOTOR].m_params.Ki = MotorsControl[POOLER_MOTOR].integraltime;
+ MotorControlConfig[POOLER_MOTOR].m_params.Ki = 0;//MotorsControl[POOLER_MOTOR].integraltime;
MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp;
MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue;
MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon;
@@ -496,13 +581,14 @@
MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
- ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),FEEDER_DANCER,POOLER_MOTOR);
+ PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR);
+ Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);
////////////////////////
MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime;
MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain;
- MotorControlConfig[WINDER_MOTOR].m_params.Ki = MotorsControl[WINDER_MOTOR].integraltime;
+ MotorControlConfig[WINDER_MOTOR].m_params.Ki = 0;//MotorsControl[WINDER_MOTOR].integraltime;
MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp;
MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue;
MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon;
@@ -515,29 +601,36 @@
MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0;
MotorControlConfig[WINDER_MOTOR].m_preError = 0;
MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
- MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
+ MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize);
- ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),FEEDER_DANCER,WINDER_MOTOR);
+ WinderControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR);
+ Report("AddControlCallback Winder",__FILE__,__LINE__,WinderControlId,RpMessage,IfTypeThread*0x100+WINDER_MOTOR,0);
////////////////////////
+ MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].directionthreadwize);
+ MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
CallbackCounter++;
//MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
// numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000);
Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+ LoadArmInfo.LoadArmBackLash = 5;
+
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/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+ MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
//Keep Notation How Many Rotations In The Dryer
//LoadArmInfo.LoadArmBackLash = 0;
- LoadArmInfo.LoadArmRounds = 0xFF;
- FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
+ LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+ FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
return OK;
}
uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
{
REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
+ if (LoadStages!=ReadValue)
+ LoadStages = ReadValue;
switch (ReadValue)
{
case THREAD_LOAD_INIT:
@@ -585,10 +678,6 @@
//LoadStages++;
Thread_Load_Close_Lids();
break;
- case THREAD_LOAD_RESUME_HEATING:
- //LoadStages++;
- Thread_Load_Resume_Heating();
- break;
case THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT://JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
//LoadStages++;
Thread_Load_Jog_Feeder_To_Middle_Point();
@@ -598,11 +687,16 @@
//LoadStages++;
Thread_Load_Dryer_Loading();
break;
+ case THREAD_LOAD_RESUME_HEATING:
+ //LoadStages++;
+ Thread_Load_Resume_Heating();
+ break;
case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
//LoadStages++;
Thread_Load_Jog_Thread();
break;
case THREAD_LOAD_END:
+ LoadStages = THREAD_LOAD_END;
Thread_Load_End();
break;
default:
@@ -611,6 +705,20 @@
}
return OK;
}
+uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
+{
+ Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
+ if (LoadStages > ReadValue)
+ {
+ ThreadLoadStateMachine(LoadStages+1);
+ }
+ else
+ {
+ ThreadLoadStateMachine(ReadValue);
+ }
+ return OK;
+}
+uint32_t LoadCounter = 0;
uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
@@ -623,7 +731,14 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
uint32_t calculated_speed;
double NormalizedError;
- DancerId = FEEDER_DANCER;//ThreadMotorIdToDancerId[index];
+ if (IfIndex>>8 != IfTypeThread)
+ {
+ LOG_ERROR (IfIndex, "Wrong Interface type");
+ return 0xFFFFFFFF;
+ }
+ index = IfIndex&0xFF;
+
+ DancerId = ThreadMotorIdToDancerId[index];
if (ReadValue < 10)
{
Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
@@ -634,12 +749,6 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
return OK;
}
TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
- 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
@@ -669,6 +778,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
CurrentControlledSpeed[index] = calculated_speed;
MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
}
+/* LoadCounter++;
+ if ((LoadCounter % 5001) == 0)
+ {
+ Report("MotorSpeedUpdated",__FILE__,index,abs(TranslatedReadValue),RpWarning,(int)CurrentControlledSpeed[index],0);
+ }*/
return OK;
}
void ThreadLoadRequest(MessageContainer* requestContainer)
generated by cgit v1.3.1 (git 2.54.0) at 2026-06-20 12:35:38 +0000