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.c126
1 files changed, 90 insertions, 36 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index c478b82de..cd7c146fe 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);
@@ -346,14 +351,6 @@
//HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
//HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
//DeActivateHeadMagnet();
- if (Head_Type != HEAD_TYPE_ARC)
- {
- CallbackCounter++;
- if (MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000)==ERROR)
- {
- Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,NO_LIMIT);
- }
- }
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);
int direction;
@@ -366,11 +363,18 @@
direction = DRIER_LID_OPEN;
}
MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdUp[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,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000)==ERROR)
+ {
+ Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,NO_LIMIT);
+ }
+ }
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(ACTIN, ENABLE, UP);
+ Report("Thread_Load_Lift right actuator",__FILE__,__LINE__,LOW,RpMessage,false,0);
+ Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP
}
return OK;
}
@@ -391,6 +395,12 @@
status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1);
}
+ if (Head_Type == HEAD_TYPE_FLAT)
+ {
+ Report("Thread_Load_Lift left actuator",__FILE__,__LINE__,LOW,RpMessage,false,0);
+ Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP);
+ }
+
return OK;
}
uint32_t Thread_Load_Lift_Rockers(void)
@@ -458,6 +468,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 left actuator",__FILE__,__LINE__,LOW,RpMessage,true,0);
+ Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN);
+ }
return OK;
}
@@ -493,6 +508,11 @@
}*/
}
+ if (Head_Type == HEAD_TYPE_FLAT)
+ {
+ Report("Thread_Load_Close right actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
+ HeadCard_Actuators_Relocate();
+ }
return OK;
}
uint32_t Thread_Load_Close_Lids(void)
@@ -501,18 +521,6 @@
//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_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);
- }
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;
@@ -525,6 +533,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)
@@ -657,7 +674,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;
@@ -717,7 +734,7 @@
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.Ki = 0;//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;
@@ -754,6 +771,7 @@
numberOfSteps-800, Thread_Load_Dryer_Loading_Callback, 10000);
return OK;
}
+ double keepfeedertension = 0;
uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
{
REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_ThreadStop");
@@ -761,6 +779,7 @@
LoadingControlId = 0xFF;
ThreadAbortJoggingFunc();
LoadStages++;
+ ProcessParametersKeep.feedertension = keepfeedertension;
ThreadLoadStateMachine(LoadStages);
return OK;
}
@@ -768,6 +787,8 @@
//Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading
{
REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Thread");
+ keepfeedertension = feedertension;
+ ProcessParametersKeep.feedertension = 0.0;
ThreadJoggingFunc(20);
LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
return OK;
@@ -799,6 +820,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);
@@ -807,7 +833,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);
@@ -836,14 +865,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);
@@ -862,14 +891,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;
@@ -1001,6 +1030,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);
@@ -1020,10 +1050,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)
@@ -1108,6 +1140,7 @@
break;
case THREAD_LOAD_END:
LoadStages = THREAD_LOAD_END;
+ percentage = 100.0;
Thread_Load_End();
break;
default:
@@ -1138,20 +1171,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;
}
@@ -1460,9 +1501,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;
@@ -1495,11 +1541,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;
generated by cgit v1.3.1 (git 2.54.0) at 2026-06-20 21:08:01 +0000