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.c64
1 files changed, 47 insertions, 17 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 9ef7f3d0d..d2f6170cf 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -59,7 +59,7 @@
ProcessParameters ProcessParametersClear,ProcessParametersRecover;
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);
@@ -173,6 +173,24 @@
}
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);
NumberOfDrierLoaderCycles=0;
@@ -374,15 +392,14 @@
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);
if(ControlId != 0xFF)
{
MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
+ MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround);
+
ControlId = 0xFF;
}
if (CallbackCounter)
@@ -399,6 +416,13 @@
}
else
LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
+
+ }
+ 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");
+ MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
return OK;
}
@@ -413,6 +437,7 @@
// 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;
+ 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,6 +457,8 @@
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);
CallbackCounter++;
MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
@@ -472,7 +499,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;
@@ -496,7 +523,7 @@
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);
////////////////////////
MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
@@ -515,18 +542,20 @@
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);
////////////////////////
+ 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);
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;
@@ -623,7 +652,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 +670,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
generated by cgit v1.3.1 (git 2.54.0) at 2026-06-20 04:24:37 +0000