diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 599 |
1 files changed, 393 insertions, 206 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 19392c754..63072e4d8 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -16,7 +16,7 @@ #include "Modules/Control/control.h" #include "Modules/General/process.h" #include "Modules/control/pidalgo.h" - +#include "Modules/AlarmHandling/AlarmHandling.h" #include "Modules/heaters/heaters_ex.h" #include "Modules/General/buttons.h" #include "StateMachines/Initialization/InitSequence.h" @@ -35,6 +35,7 @@ #include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h> #include <Drivers/I2C_Communication/I2C_Task.h> #include "drivers/FPGA/FPGA_SPI_Comm.h" +#include "drivers/FPGA/Full_Vme/FPGA_Programming_Up.h" #include "PMR/Stubs/StubHeatingTestRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestResponse.pb-c.h" @@ -44,26 +45,28 @@ #include "PMR/ThreadLoading/StartThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/ContinueThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/ContinueThreadLoadingResponse.pb-c.h" +#include "PMR/ThreadLoading/TryThreadLoadingRequest.pb-c.h" +#include "PMR/ThreadLoading/TryThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/StopThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h" //#include <stdint.h> //#include <stdbool.h> 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" , + /*00*/ "Initialization", + /*01*/ "Heating stop", + /*02*/ "Rockers opening", + /*03*/ "Dryer unloading", + /*04*/ "Lids opening", + /*05*/ "Dancers opening", + /*06*/ "Initial tension building", + /*07*/ "Rockers closing", + /*08*/ "Dancers closing", + /*09*/ "Lids closing", + /*10*/ "Feeder jogging", + /*11*/ "Dryer loading", + /*12*/ "Heating resuming", + /*13*/ "Thread jogging" , /*14*/ "END"}; /* typedef enum @@ -91,7 +94,7 @@ uint32_t LoadStatus = OK; char LoadErrorMsg[100]; int32_t keepmicrostep; - int32_t keepkvalrun,keepkvalhold; + int32_t keepkvalrun,keepkvalhold,kval_upperValue; uint8_t CallbackCounter = 0; uint8_t TimeoutsCounter = 0; bool SecondTry = false,TryAgain = false; @@ -100,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; @@ -108,16 +111,22 @@ float numberOfSteps = 0; float numberOfCycles = 0; double DrierPrevLocation = 0; - FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT}; + int DrierCenterLocation = 0; + FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT}; int Screw_Dir = false; bool InitCalled = false; uint32_t LoadArmRounds; + int CurrentlyLoaded = 0; 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); + uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage bool ThreadLoaded(void); bool ThreadLoadingActive(void) { @@ -138,12 +147,8 @@ //EEPROM_STORAGE_DRYER_CYCLES MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); + Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,LoadArmRounds,RpWarning,Read_Dryer_ENC_Position(),0); - Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); - - Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmRounds,RpWarning,0,0); - - Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmRounds,RpMessage,LoadArmRounds,0); StopInitSequence(); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); @@ -163,6 +168,11 @@ uint32_t Thread_Load_Reduce_Heat(void) { + if (Is_PP_Machine()) + kval_upperValue = 28; + else + kval_upperValue = 70; + //Heaters Off, Dryer Blower Off, Blower Low, REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Reduce_Heat"); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); @@ -188,18 +198,23 @@ ProcessParametersClear.lblowerflow = 0; ProcessParametersClear.dyeingspeed = 40; ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; + ProcessParametersClear.pullertension = ProcessParametersKeep.pullertension; + ProcessParametersClear.windertension = ProcessParametersKeep.windertension; + if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Reduce_Heat failed"); strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed"); + load.color = fastBILNK; LoadStatus = ERROR; TryAgain = true; + //ThreadLoadButton(LoadStages); } else { - if(Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD) + if(Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD) { - WHS_Set_SetPoint_Q_value(headairflow/2); + WHS_Set_SetPoint_Q_value(headairflow); } else { @@ -215,40 +230,57 @@ } return OK; } + uint32_t Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback(uint32_t deviceID, uint32_t BusyFlag) + { + + Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); + + Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + NumberOfDrierLoaderCycles=0; + //storeLoadArmParameters(); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + + return OK; + } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { + uint32_t angle, temp = Read_Dryer_ENC_Position(); - /*if(PullerControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - }*/ - if(WinderControlId != 0xFF) + ReportWithPackageFilter(DiagnosticsFilter,"Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,(int)numberOfCycles,(int)DrierPrevLocation,RpMessage,temp,0); + + angle = Calculate_Arm_Distance(DrierPrevLocation,temp); + + DrierPrevLocation = temp; + if (CallbackCounter) { - MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); - RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); - WinderControlId = 0xFF; + CallbackCounter--; } + if ((angle>14000 )&&(BusyFlag == NOTBUSY)) // OK - take another round + { + Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); - MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - /*Task_sleep(5) - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ + Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + NumberOfDrierLoaderCycles=0; + //storeLoadArmParameters(); + LoadStages++; + + ThreadLoadStateMachine(LoadStages); + } + else + { + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM]); + Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); + LoadStatus = ERROR; + ThreadLoadingReport(); + TryAgain = true; + //ThreadLoadButton(LoadStages); + } //move to exact location? // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - CallbackCounter = 0; - Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,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); return OK; } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) @@ -284,12 +316,13 @@ { TimeoutsCounter = 0; load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Load sequence timeout %s motor %d",LoadStagesStr[LoadStages], MotorId); + usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[MotorId]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; + //ThreadLoadButton(LoadStages); } else { @@ -306,6 +339,7 @@ else { load.color = colorON; + Pannel_Leds(THREAD_LOAD, MODE_ON); ThreadLoadingReport(); } } @@ -323,13 +357,30 @@ //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, //DeActivateHeadMagnet(); CallbackCounter++; - 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); - 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); + int direction; + if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep + { + direction = DRIER_LID_CLOSE; + } + else + { + 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 right actuator",__FILE__,__LINE__,LOW,RpMessage,false,0); + Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP + } return OK; } uint32_t Thread_Load_Lift_Dancers(void) @@ -349,6 +400,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) @@ -356,8 +413,8 @@ { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; - if (keepkvalrun>=25) - keepkvalrun = 25; + /*if (keepkvalrun>=25) + keepkvalrun = 25;*/ Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); @@ -372,8 +429,8 @@ 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); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); Task_sleep(10); CallbackCounter++; 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); @@ -389,6 +446,7 @@ if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); + Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_WINDER,NO_LIMIT); //return ERROR; } CallbackCounter++; @@ -401,43 +459,67 @@ { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; - if (keepkvalrun>=25) - keepkvalrun = 25; + /*if (keepkvalrun>=25) + keepkvalrun = 25;*/ Report("Thread_Load_Close_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); 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); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); 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++; 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; } uint32_t Thread_Load_Close_Dancers(void) //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread { + uint32_t current = 1; 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++; - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500); + + Report("Thread_Load_Close dancers tension",__FILE__,__LINE__,(int)windertension,RpMessage,(int)pullertension,0); // 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,25000); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000); - status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension); - if (Is_PP_Machine() == true) //PP machine - new LTFU + status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,¤t); + + ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); +/* if (current!=windertension) { CallbackCounter++; - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000); - status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension); - } + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500); + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000); + status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension); + }*/ + if (Is_PP_Machine() == true) //PP machine - new LTFU + { + ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); + /*status |= MCU_E2PromRead(EEPROM_PULLER_TENSION_POSITION,¤t); + if (current!=pullertension) + { + CallbackCounter++; + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500); + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000); + status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension); + }*/ + } + 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) @@ -447,23 +529,37 @@ //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, CallbackCounter++; - 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); - 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); + int direction; + if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep + { + direction = DRIER_LID_OPEN; + } + else + { + 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) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating"); ActivateHeadMagnet(); + HeadCard_Actuators_Relocate(); if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) { - usnprintf(LoadErrorMsg, 100, "Load sequence error %d",LoadStagesStr[LoadStages]); + usnprintf(LoadErrorMsg, 100, "Stage %s error",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } else @@ -504,6 +600,7 @@ LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; + //ThreadLoadButton(LoadStages); } } @@ -557,7 +654,7 @@ { numberOfCycles++; - Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0); + Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,numberOfCycles,0); if ((fabs (TotalLoadedLen -DrierPrevLocation)>50 )&&(ReadValue == NOTBUSY)) { @@ -568,6 +665,7 @@ { return OK; } + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded); if (numberOfCycles<LoadArmRounds) { MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, @@ -575,20 +673,20 @@ } else { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); - Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); + Report("Store Number of cycles in dryer",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } else { TryAgain = true; SecondTry = false; - Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); + Report("Loading dryer - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded); TimeoutsCounter = 0; + CallbackCounter = 0; load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Load sequence timeout %s motor %d",LoadStagesStr[LoadStages], HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); + usnprintf(LoadErrorMsg, 100, "Stage %s - Dryer load arm timeout",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); LoadStatus = ERROR; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); @@ -606,6 +704,7 @@ Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0); ThreadLoadingReport(); + //ThreadLoadButton(LoadStages); } return OK; } @@ -623,7 +722,12 @@ // OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; // CurrentControlledSpeed[FEEDER_MOTOR] = 1000; - //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds); + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&CurrentlyLoaded); + Report("Thread_Load_Dryer_Loading cycles",__FILE__,LoadArmRounds,CurrentlyLoaded,RpMessage,LoadArmRounds-CurrentlyLoaded,0); + if (CurrentlyLoaded) + { + LoadArmRounds -= CurrentlyLoaded; + } ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; @@ -637,7 +741,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; @@ -652,7 +756,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*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); + //6 seconds per round LoadingControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0); @@ -670,9 +775,10 @@ 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-400, Thread_Load_Dryer_Loading_Callback, 10000); + 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"); @@ -680,6 +786,7 @@ LoadingControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; + ProcessParametersKeep.feedertension = keepfeedertension; ThreadLoadStateMachine(LoadStages); return OK; } @@ -687,6 +794,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; @@ -718,7 +827,13 @@ RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); SpeedTControlId = 0xFF; } + if (ThreadLoadControlId != 0xFF) + { + RemoveControlCallback(ThreadLoadControlId, ThreadUpdateCallBackFunction ); + ThreadLoadControlId = 0xFF; + } + LoadingStopArmReset(); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING); @@ -726,7 +841,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); @@ -735,6 +853,7 @@ LoadStages = THREAD_LOAD_INIT; load.color = colorON; + Pannel_Leds(THREAD_LOAD, MODE_ON); return OK; } uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag) @@ -745,49 +864,62 @@ } Screw_Dir = 1-Screw_Dir; // movement of the screw while unloading the thread from the dryer to the spool - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000); + AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF); //handle alarm detection and operation + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000); return OK; } - uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) + /*uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { + 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); if (numberOfCycles<LoadArmRounds) { + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - numberOfSteps, Thread_Load_Dryer_UnLoading_Callback, 30000); + numberOfSteps, Thread_Load_Dryer_UnLoading_Callback, 10000); } else //done enough cycles, go to the center point { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,10000); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles); - Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); + Report("Store Number of cycles in dryer",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } else //timeout or no movement { - Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(numberOfCycles-2));//it takes two cycles to identify a stop of the arm + Report("Unloading dryer - 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 -DrierCenterLocation)<200) + { + if (temp<DrierCenterLocation) + direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; + else + direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction,10); + + } + else + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); + + TimeoutsCounter = 0; CallbackCounter = 0; - /*if(PullerControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - }*/ if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); @@ -796,109 +928,61 @@ } MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - /*Task_sleep(5) - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Load sequence timeout %s motor %d",LoadStagesStr[LoadStages], HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); + usnprintf(LoadErrorMsg, 100, "Stage %s - Dryer load arm timeout",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); - Report("Drier unloading timeout(1) or no movement",__FILE__,temp,DrierPrevLocation,RpWarning,ReadValue,0); + Report("Dryer unloading timeout(1) or no movement",__FILE__,temp,DrierPrevLocation,RpWarning,ReadValue,0); LoadStatus = ERROR; TryAgain = true; ThreadLoadingReport(); + //ThreadLoadButton(LoadStages); } return OK; - } + }*/ uint32_t Thread_Load_Dryer_UnLoading(void) { uint32_t temp; - REPORT_MSG(LoadStages, "Thread UnLoad State Machine step"); + REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading"); //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(22); -// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; -// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; - -// Rockers are up already, so puller handling is not needed - //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1; - 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 = 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; - MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype; - MotorControlConfig[POOLER_MOTOR].m_ingnoreValue = MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0; - MotorControlConfig[POOLER_MOTOR].m_integral = 0; - MotorControlConfig[POOLER_MOTOR].m_isEnabled = true; - MotorControlConfig[POOLER_MOTOR].m_isReady = true; - MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0; - MotorControlConfig[POOLER_MOTOR].m_preError = 0; - 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); - - 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 = 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; - MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype; - MotorControlConfig[WINDER_MOTOR].m_ingnoreValue = MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0; - MotorControlConfig[WINDER_MOTOR].m_integral = 0; - MotorControlConfig[WINDER_MOTOR].m_isEnabled = true; - MotorControlConfig[WINDER_MOTOR].m_isReady = true; - 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_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize); - - WinderControlId = AddControlCallback(NULL,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("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); - //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); -//shlomo - UnloadingStart = msec_millisecondCounter; - numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - numberOfCycles = 0; - if (SecondTry == true) + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + DrierCenterLocation = DrierPrevLocation; + DrierPrevLocation = temp; + UnloadingStart = msec_millisecondCounter; + //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + //numberOfCycles = 0; + LoadingArmReset(Thread_Load_Set_Load_Arm_To_Stopper_Callback,300000); + /*if (SecondTry == true) { - LoadArmRounds = 30; + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); + if (LoadArmRounds == 0) //prev trial stopped + { + LoadArmRounds = (int)dryerbufferlength; + } + //LoadArmRounds = 30; } else { MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); - } - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + if (LoadArmRounds == 0) //prev trial stopped + { + LoadArmRounds = (int)dryerbufferlength; + } + }*/ + /*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/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); // 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, Thread_Load_Set_Load_Arm_To_Stopper_Callback,0,1000); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - numberOfSteps-400, Thread_Load_Dryer_UnLoading_Callback, 30000); + numberOfSteps-800, Thread_Load_Dryer_UnLoading_Callback, 10000); Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize; @@ -907,14 +991,20 @@ //Keep Notation How Many Rotations In The Dryer //LoadArmRounds = (int)dryerbufferlength; + * + */ load.color = BLINK; return OK; } + double percentage; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue) { REPORT_MSG(ReadValue,"ThreadLoadStateMachine"); - ThreadLoaded(); + //ThreadLoaded(); + percentage = ReadValue*7; + ThreadLoadingReport(); + if (LoadStages!=ReadValue) LoadStages = ReadValue; switch (ReadValue) @@ -997,17 +1087,19 @@ break; case THREAD_LOAD_END: LoadStages = THREAD_LOAD_END; + percentage = 100.0; Thread_Load_End(); break; default: LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE"); break; } + return OK; } uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) { - if (JobIsActive()) + if (JobIsActive()||(MachineReadyForHeating == false)||(FPGABurningActive==true)) { Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); return ERROR; @@ -1019,26 +1111,43 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) } Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); Report("ThreadLoadButton params",__FILE__,LoadStatus,TryAgain,RpMessage,SecondTry,0); + if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call + MessageState = 2; + ThreadLoadingReport(); if (LoadStatus == ERROR) { if ((SecondTry == false)&&(TryAgain == true)) { LoadStatus = OK; SecondTry = true; - Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); + load.color = BLINK; + //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + // ThreadLoadingRestartReport(); + Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); ThreadLoadStateMachine(LoadStages); } - else + 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; + //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + // ThreadLoadingRestartReport(); 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); + //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + // ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages); + SecondTry = false; } return OK; } @@ -1118,11 +1227,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) index = IfIndex&0xFF; DancerId = ThreadMotorIdToDancerId[index]; - if (ReadValue < 10) + /*if (ReadValue < 10) { Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; - } + }*/ if (ReadValue == 0x3FFF) { return OK; @@ -1133,6 +1242,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } + if (abs(TranslatedReadValue) > 0x2000) + { + TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment + } + //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -1197,29 +1311,29 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } bool ThreadLoaded(void) { - uint32_t ReadValue; + /* uint32_t ReadValue; int32_t PoolerValue,WinderValue,FeederValue; //this function tests if there is a thread in the machine. all dancers are in lowermost position then there is no thread. ReadValue = Control_Read_Dancer_Position(FEEDER_DANCER,0); FeederValue = ReadValue - DancersCfg[FEEDER_DANCER].zeropoint; - Report("Feeder out",__FILE__,FeederValue,ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0); + Report("Feeder out",__FILE__,abs(FeederValue),ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0); ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0); WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint; - Report("Winder out",__FILE__,WinderValue,ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0); + Report("Winder out",__FILE__,abs(WinderValue),ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0); ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0); PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint; //pooler dancer is right sided: data is opposite PoolerValue = (-1*PoolerValue); - Report("Puller out",__FILE__,PoolerValue,ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0); + Report("Puller out",__FILE__,abs(PoolerValue),ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0); if (((PoolerValue)<(-1200))&&((WinderValue)<(-1200))&&((FeederValue)<(-1200))) { Report("Thread is out",__FILE__,(-1*PoolerValue),(-1*WinderValue),RpWarning,(-1*FeederValue),0); return true; } - else + else*/ return false; } void ThreadLoadRequest(MessageContainer* requestContainer) @@ -1235,10 +1349,16 @@ void ThreadLoadRequest(MessageContainer* requestContainer) responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size); container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); - int LoadStage = (int)request->dryerzone1temp; - LoadStages = LoadStage; - ThreadLoadStateMachine(LoadStage); + if (request->hardwarepidcontrol1!=NULL) + { + UpdatePidDuringRun(request->hardwarepidcontrol1); + } + else + { + LoadStages = LoadStage; + ThreadLoadStateMachine(LoadStage); + } /*if (status) { @@ -1336,9 +1456,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; @@ -1351,12 +1476,68 @@ uint32_t ThreadLoadingReport(void) return OK; } +bool ThreadLoadingRestartFlag = false; +uint32_t ThreadLoadingRestartReport(void) //sending after a failure in the finalizing stage +{ + MessageContainer responseContainer; + StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; + + if (ThreadLoadingToken[0] == 0) + return OK; + ThreadLoadingRestartFlag = true; + + response.has_state = true; + response.state = THREAD_LOADING_STATE__ReadyForLoading; + response.errorreason = DefaultErrSrt; + Report("ThreadLoadingRestartReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); + //------------------------------------------------------------------------------------------- + responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, false, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); + responseContainer.has_continuous = true; + responseContainer.continuous = true; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + + return OK; + +} + +uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) +{ + TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT; + MessageContainer responseContainer; + //MessageState = 2; + ThreadLoadingReport(); + Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0); + + if (requestContainer) + { + responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size); + responseContainer.continuous = false; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + } + + ThreadLoadButton(LoadStages); + return OK; +} +uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) +{ + + if(ThreadLoadingActive()) + 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; @@ -1370,13 +1551,19 @@ uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) MessageContainer responseContainer; MessageState = 2; ThreadLoadingReport(); - if (request->processparameters) + if (ThreadLoadingRestartFlag == false) { - dryerbufferlength = request->processparameters->dryerbufferlength; - LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + if (request->processparameters) + { + dryerbufferlength = request->processparameters->dryerbufferlength; + LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + windertension = request->processparameters->windertension; + pullertension = request->processparameters->pullertension; + memcpy (&ProcessParametersRecover,request->processparameters,sizeof(ProcessParameters)); + } + Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); } - Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size); responseContainer.continuous = false; |