From e1532f3ce008c7c3d149f89fa80e74962d8cf2ca Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 1 Sep 2020 08:57:54 +0300 Subject: tests and prints for thread parameters in jog bugs --- Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index ab1ab77e6..275e7e5b7 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -842,6 +842,16 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) { return OK; } + if((ThreadParams->bypassrockers != true)&&(ThreadParams->bypassrockers != false)) + { + ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); + return OK; + } + if ((ThreadParams->feederp>100000)||(ThreadParams->feederi>100000)) + { + ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); + return OK; + } if(ThreadParams->feederp) MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp; if(ThreadParams->feederi) @@ -1168,6 +1178,7 @@ uint32_t ThreadPrepareState(void *JobDetails) MotorControlConfig[Motor_i].m_preError = 0; MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage + HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; @@ -1278,7 +1289,6 @@ uint32_t ThreadPrepareState(void *JobDetails) } #endif } - HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! #ifdef TEST_PID_THREAD testDancersControl(); -- cgit v1.3.1 From b2977e51824febd45cfb7d33e8c4c8aa6514666c Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 2 Sep 2020 10:34:05 +0300 Subject: version 1.4.6.44 p1 IDS pre-run and remove integral on thread loading feeder control --- .../Embedded_SW/Embedded/Common/SW_Info/SW_Info.c | 2 +- Software/Embedded_SW/Embedded/Modules/IDS/IDS.h | 4 + .../Embedded_SW/Embedded/Modules/IDS/IDS_maint.c | 10 +- .../Embedded_SW/Embedded/Modules/IDS/IDS_print.c | 103 +++++++++++---------- .../Embedded/Modules/Thread/ThreadLoad.c | 2 +- .../stubs/embeddedparametersbuild_w_cleaning.cs | 4 +- 6 files changed, 69 insertions(+), 56 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c index 64002276c..bf2c4e98d 100644 --- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c +++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c @@ -20,7 +20,7 @@ typedef struct } TangoVersion_t; -TangoVersion_t _gTangoVersion = {1,4,6,43}; +TangoVersion_t _gTangoVersion = {1,4,6,44}; #define BUILD_DATE __DATE__ char Dat[50] = BUILD_DATE; char _gTangoName [MAX_STRING_LEN] = "Tango01 ";//e diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h index ee308ce62..1c17c4f6b 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h @@ -13,6 +13,10 @@ extern double DispenserPreparePressure ; extern uint32_t DispenserPrepareTimeout ; extern uint32_t DispenserPrepareTimeLag ; extern uint32_t InitialDispenserSpeed; +extern double InitialDispenserPressure; +extern uint32_t InitialDispenserTimeout; + + extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS]; extern float DispenserPressure[MAX_SYSTEM_DISPENSERS]; diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c index 663a6713f..79418a8d8 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c @@ -72,8 +72,8 @@ bool PrimingActive[MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false uint32_t DispenserHomingControlId[MAX_SYSTEM_DISPENSERS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; uint32_t DispenserBacklashControlId[MAX_SYSTEM_DISPENSERS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; uint32_t DispenserHomingTime[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0}; -#define INITIAL_DISPENSER_PRESSURE 2.10 -#define INITIAL_DISPENSER_TIMEOUT_LIMIT 60000 +#define INITIAL_DISPENSER_PRESSURE 3.85 +#define INITIAL_DISPENSER_TIMEOUT_LIMIT 120000 #define INITIAL_DISPENSER_TIMEOUT 100 #define INITIAL_DISPENSER_SPEED 1000 @@ -105,8 +105,10 @@ void IDS_Dispenser_SetAutoHoming_Config(AutoHoming_Config_enum Config) void IDS_Dispenser_SetBackLashValues(double initialdispenserpressure, uint32_t initialdispensertimeout, uint32_t initialdispensertimelag, uint32_t initialdispenserspeed) { - InitialDispenserPressure = initialdispenserpressure; - InitialDispenserTimeout = initialdispensertimeout; + if (initialdispenserpressure>2) + InitialDispenserPressure = initialdispenserpressure; + if (initialdispensertimeout>70000) + InitialDispenserTimeout = initialdispensertimeout; InitialDispenserTimeLag = initialdispensertimelag; InitialDispenserSpeed = initialdispenserspeed; diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c index b15f70267..327d80eb1 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c @@ -514,6 +514,37 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue) char IdMessage[100]; int SegmentPrepareWFCFCounter = 0; bool pressureReady = false,endOfPrepareWCF = false; + +bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,double pressure) +{ + TimerMotors_t HW_Motor_Id = DispenserIdToMotorId[DispenserId]; + float updatedSpeed,tempSpeed; + + if (pressure > RefMaxPressure*1.03) + { + tempSpeed = CurrentDispenserSpeed[DispenserId]; + //updatedSpeed = (tempSpeed * 0.992 > 100) ? tempSpeed * 0.992 : 100; + updatedSpeed = tempSpeed*0.98; + MotorSetSpeed(HW_Motor_Id, updatedSpeed); + CurrentDispenserSpeed[DispenserId] = updatedSpeed; + ReportWithPackageFilter(IDSFilter,"IDS decrease speed",__FILE__,DispenserId,(int)updatedSpeed,RpWarning,(int)(pressure*100),0); + //return true; //ready + } + else if (pressure < RefMaxPressure*0.97) + { + tempSpeed = CurrentDispenserSpeed[DispenserId]; + if (tempSpeed<50) tempSpeed = 50; + updatedSpeed = (tempSpeed * 1.02 < InitialDispenserSpeed) ? tempSpeed * 1.02 : InitialDispenserSpeed; + MotorSetSpeed(HW_Motor_Id, updatedSpeed); + CurrentDispenserSpeed[DispenserId] = updatedSpeed; + ReportWithPackageFilter(IDSFilter,"IDS increase speed",__FILE__,DispenserId,(int)updatedSpeed,RpWarning,(int)(pressure*100),0); + //return false; //not ready + } + if ((pressure>RefMaxPressure)&&(pressure<(RefMaxPressure+0.1))) + return true; + return false; +} + //******************************************************************************************************************** uint32_t IDS_Prepare_Callback(uint32_t DispenserId, uint32_t ReadValue) { @@ -525,6 +556,7 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue) //set the speed only before the first segment, speed is constant accros job int Dispenser_i,n_dispensers; double segmentfirst_speed; + double pressure; //REPORT_MSG((int)DispenserPrepareControlId, "Prepare Callback"); @@ -532,75 +564,50 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue) if (pressureReady == false) { - if (Special_Dispensers == false) + if (DispenserBuildTimeCounter<(2*eOneSecond)) { - if (DispenserBuildTimeCounter<(2*eOneSecond)) + for (i = 0; i < MAX_DYE_DISPENSERS; i++) { - for (i = 0; i < MAX_DYE_DISPENSERS; i++) - { - DispenserTotalPrepareSteps[i]+=(CurrentDispenserSpeed[i]*PRESSURE_READ_TIME_GAP/eOneSecond); - DispenserLastMovementDown[i] = false; - } - //REPORT_MSG(DispenserBuildTimeCounter,"waiting 2 seconds for pressure changes"); - return OK; + DispenserTotalPrepareSteps[i]+=(CurrentDispenserSpeed[i]*PRESSURE_READ_TIME_GAP/eOneSecond); + DispenserLastMovementDown[i] = false; } + //REPORT_MSG(DispenserBuildTimeCounter,"waiting 2 seconds for pressure changes"); + return OK; } for (i = 0; i < MAX_DYE_DISPENSERS; i++) { + pressure = GetDispenserPressure(i); //IDS_StopHomeDispenser(i); if (DispenserUsedInJob[i] == true) //we actually should check for all dispensers { DispenserTotalPrepareSteps[i]+=(CurrentDispenserSpeed[i]*PRESSURE_READ_TIME_GAP/eOneSecond); - HW_Motor_Id = DispenserIdToMotorId[i]; - if (GetDispenserPressure(i) > DispenserPreparePressure*1.05) +//#ifdef DISPENSER_UP_MOVEMENT + if (DispenserTotalPrepareSteps[i]120000) -#endif - { - NumofReadyDispensers++; - ReportWithPackageFilter(IDSFilter,"Dispenser total prepare steps ready",__FILE__,DispenserTotalPrepareSteps[i],(int)(DispenserPreparePressure*100),RpWarning,IDS_Dispenser_Data[i].consumedinnanolitter,0); - } - if (Special_Dispensers == true) - { - MotorStop(HW_Motor_Id, Hard_Hiz); - CurrentDispenserSpeed[i] = 0; - //ReportWithPackageFilter(IDSFilter,"IDS stopped ready",__FILE__,i,(int)(DispenserPreparePressure*100),RpWarning,(int)NumofReadyDispensers,0); - } - else - { - tempSpeed = CurrentDispenserSpeed[i]; - updatedSpeed = (tempSpeed*0.992>100)?tempSpeed*0.992:100; - MotorSetSpeed(HW_Motor_Id, updatedSpeed); - CurrentDispenserSpeed[i] = updatedSpeed; - //ReportWithPackageFilter(IDSFilter,"IDS reduce speed",__FILE__,i,(int)tempSpeed,RpWarning,(int)updatedSpeed,0); - } + AdjustDispenserSpeedToPressure(i,InitialDispenserPressure,pressure); + } + else if (pressure > DispenserPreparePressure*1.2) + { + MotorStop(HW_Motor_Id, Hard_Hiz); + CurrentDispenserSpeed[i] = 1; } else { - if (Special_Dispensers == true) - { - tempSpeed = (DispenserPreparePressure-GetDispenserPressure(i))*InitialDispenserSpeed; - if (tempSpeed<300) - tempSpeed = 300; - } - else - { - tempSpeed = CurrentDispenserSpeed[i]; - } - updatedSpeed = (tempSpeed*1.03 0.02) + pressure = GetDispenserPressure(CLEANER_DISPENSER); + if (pressure > 0.02) { HW_Motor_Id = DispenserIdToMotorId[CLEANER_DISPENSER]; MotorStop(HW_Motor_Id, Hard_Hiz); if (CurrentDispenserSpeed[CLEANER_DISPENSER]) - ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(GetDispenserPressure(CLEANER_DISPENSER)*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0); + ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(pressure*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0); CurrentDispenserSpeed[CLEANER_DISPENSER] = 0; } @@ -724,7 +731,7 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue) HW_Motor_Id = DispenserIdToMotorId[CLEANER_DISPENSER]; MotorStop(HW_Motor_Id, Hard_Hiz); CurrentDispenserSpeed[CLEANER_DISPENSER] = 0; - ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(GetDispenserPressure(CLEANER_DISPENSER)*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0); + ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(pressure*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0); } SafeRemoveControlCallback(DispenserPrepareControlId, IDS_Prepare_Callback ); DispenserPrepareControlId = 0xFF; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index c478b82de..84cc5cf20 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -717,7 +717,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; diff --git a/Software/Stubs Collection/stubs/embeddedparametersbuild_w_cleaning.cs b/Software/Stubs Collection/stubs/embeddedparametersbuild_w_cleaning.cs index ee610b445..5b2cfdc0d 100644 --- a/Software/Stubs Collection/stubs/embeddedparametersbuild_w_cleaning.cs +++ b/Software/Stubs Collection/stubs/embeddedparametersbuild_w_cleaning.cs @@ -35,8 +35,8 @@ configurationParameters.CloseValveTimeout = 20; configurationParameters.OpenValveTimeout = 20; //backlash after filling - target pressure, timoeout, time staps (speed will be added in general parameters) -configurationParameters.InitialDispenserPressure = 0.25; -configurationParameters.InitialDispenserTimeout = 60000; //20 seconds +configurationParameters.InitialDispenserPressure = 3.85; //used for initial pre-run pressure +configurationParameters.InitialDispenserTimeout = 120000; ////used for initial pre-run number of steps configurationParameters.InitialDispenserTimeLag = 100; //job prepare - build pressure in dispensers - target pressure, timoeout, time staps, speed -- cgit v1.3.1 From bb8b2a7fac772ceb2a163aecb9c854f4fa9013c7 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 2 Sep 2020 12:38:57 +0300 Subject: thread load: handle safety alarm on head cover better, prevent RTFU arm raising on final jog --- .../Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 84cc5cf20..eb422875f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -346,14 +346,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,6 +358,14 @@ 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); @@ -754,6 +754,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 +762,7 @@ LoadingControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; + ProcessParametersKeep.feedertension = keepfeedertension; ThreadLoadStateMachine(LoadStages); return OK; } @@ -768,6 +770,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; -- cgit v1.3.1 From 7390afe73ac4844291632ae89b5ab1fbe7ab9271 Mon Sep 17 00:00:00 2001 From: Avi Levkovich Date: Wed, 2 Sep 2020 16:40:26 +0300 Subject: Change actuators logic on loading (move to Trigger_Head_Actuators_Control) --- .../Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c | 6 +++--- Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 9 +++++++-- 2 files changed, 10 insertions(+), 5 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c index 33d888b14..6bde7ec11 100644 --- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c +++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c @@ -517,7 +517,7 @@ uint32_t HeadCard_ActControlCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag Global_Act_Cmd_flag++;//Sync the callback to the command //3sec delay - Timeout - ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (3* eOneSecond), TemplateDataReadCBFunction,Act_ID,0, 0 ); + ActControlId = AddControlCallback("ActuatorControl", HeadCard_ActuatorsCallBackFunction, (2* eOneSecond), TemplateDataReadCBFunction,Act_ID,0, 0 ); return status; } @@ -640,8 +640,8 @@ uint32_t HeadCard_Actuators_Stub(ACTUATORS Act_ID, bool Enable, bool Direction)/ } } - status |= Head_Write_IO_Reg(0x42,HIGH); - status |= Head_Write_IO_Reg(0x46,LOW_AND_HIGH); + //status |= Head_Write_IO_Reg(0x42,HIGH); + //status |= Head_Write_IO_Reg(0x46,LOW_AND_HIGH); return status; } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index c478b82de..e986c1756 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -370,7 +370,9 @@ { 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); + //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP); + //Trigger_Head_Actuators_Control(ACTOT, LOW,true);//UP + Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP } return OK; } @@ -504,7 +506,10 @@ 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_Control(ACTIN, LOW,false);//DOWN + //Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN); + //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, DOWN); HeadCard_Actuators_Relocate(); } -- cgit v1.3.1 From 09df687f1d804afab0e2515bd29df1152d619f9e Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 6 Sep 2020 00:16:07 +0300 Subject: extended tracing in dispenser 3 consumption. improvements in thread loading --- .../Embedded_SW/Embedded/Drivers/Motors/Motor.h | 2 + .../Embedded/Drivers/Motors/MotorActions.c | 78 +++++++++++++-- .../Embedded/Drivers/flash_ram/MCU_E2Prom.h | 1 + .../Modules/Diagnostics/DiagnosticsHoming.c | 31 +++++- .../Embedded/Modules/IDS/IDS_dispenser.c | 7 +- .../Embedded/Modules/Thread/ThreadLoad.c | 110 ++++++++++++++++----- 6 files changed, 189 insertions(+), 40 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h index fdfb99561..e6947a8d4 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h @@ -305,6 +305,8 @@ typedef enum Soft_Hiz, }STOP_TYPE_ENUM; +extern int32_t AccumulatedArmMovement; + uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType); uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps); diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c index f500104f4..8c12b4ea3 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c @@ -203,6 +203,9 @@ uint32_t MotorGotoWithBusyCallback (TimerMotors_t MotorId,bool direction, uint32 MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } +int32_t AccumulatedArmMovement = 0; +int32_t InitialArmLocation = 0; +bool ArmDirection = true; uint32_t MotorMoveWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout) //TODO { @@ -219,12 +222,18 @@ uint32_t MotorMoveWithCallback (TimerMotors_t MotorId,bool direction, uint32_t S //SetMotorSpeed (deviceId, parameter); MotorCallback[MotorId] = callback; MotorTimeout[MotorId] = 0; - MotorTimeLag[MotorId] = 20; + MotorTimeLag[MotorId] = 40; MotorTimeLimit[MotorId] = timeout; + if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) + { + InitialArmLocation = Read_Dryer_ENC_Position(0,0); + AccumulatedArmMovement = 0; + ArmDirection = direction; + } //MotorStop(MotorId,Hard_Hiz ); MotorMove(MotorId,direction,Steps ); - MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveCallBackFunction, /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); + MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveCallBackFunction, /*eTenMillisecond*/40, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } @@ -471,7 +480,7 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t MotorId, uint32_t _freq, callb } uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO { - uint32_t MotorId,encoder; + uint32_t MotorId,encoder,temp = 0,tt; uint32_t Busy = BusyFlag; if (IfIndex>>8 != IfTypeMotors) { @@ -483,14 +492,66 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO CallbackCalls++; if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { - if (CallbackCalls%200 == 0) + encoder = Read_Dryer_ENC_Position(0,0); + tt=InitialArmLocation; + if (ArmDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize) //unloading - numbers going down + { + //previous number is bigger than current + if (InitialArmLocation>encoder) + { + temp = InitialArmLocation-encoder; + if ((temp>25)&&(temp<1000)) + { + AccumulatedArmMovement+=temp; + InitialArmLocation=encoder; + } + } + else if (InitialArmLocation25)&&(temp<1000)) + { + AccumulatedArmMovement+=temp; + InitialArmLocation=encoder; + InitialArmLocation=encoder; + } + } + } + else //loading - going up + { + //previous number is smaller than current + if (InitialArmLocation25)&&(temp<1000)) + { + AccumulatedArmMovement+=temp; + InitialArmLocation=encoder; + } + } + else if (InitialArmLocation>encoder)//rollover + { + temp = 0x3FFF + encoder -InitialArmLocation; + if ((temp>25)&&(temp<1000)) + { + AccumulatedArmMovement+=temp; + InitialArmLocation=encoder; + } + } + } + /*if (temp>1000) + { + Report("MotorMoveCallBackFunction temp curr prev total",__FILE__,encoder,tt,RpWarning,temp,0); + }*/ + if (CallbackCalls%50 == 0) { - Read_Dryer_ENC_Position(); + //encoder = Read_Dryer_ENC_Position(0,0); + Report("MotorMoveCallBackFunction periodic curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); } - if (CallbackCalls%200 == 1) + if (CallbackCalls%200 == 0) { - encoder = Read_Dryer_ENC_Position(0,0); - Report("MotorMoveCallBackFunction",__FILE__,__LINE__,DrierZeroPosition,RpWarning,MotorId,0); + //encoder = Read_Dryer_ENC_Position(0,0); + Report("MotorMoveCallBackFunction",__FILE__,__LINE__,encoder,RpWarning,CallbackCalls,0); } } else @@ -520,6 +581,7 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO Report("motor timeout",__FILE__,__LINE__,MotorId,RpMessage,MotorTimeout[MotorId],0); Busy = BUSY; } + Report("MotorMoveCallBackFunction curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); //call the module callback if (MotorCallback[MotorId]) { diff --git a/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.h b/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.h index 3691b41bd..bb171dda4 100644 --- a/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.h +++ b/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.h @@ -46,6 +46,7 @@ typedef enum { EEPROM_STORAGE_DANCER_4, EEPROM_PRESSURE_SENSOR_V0_0, EEPROM_PRESSURE_SENSOR_V0_1, + EEPROM_DRIER_LOADING_ARM_ANGLE, MAX_EEPROM_STORAGE }EEPROM_STORAGE_USAGE_ENUM; #define MAX_SERIAL_NUM_LEN 5 //5*4 bytes diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c index 04e333459..b40bbe4c7 100644 --- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c @@ -462,6 +462,23 @@ uint32_t DispenserAbortHomingRequestFunc(MessageContainer* requestContainer) return OK; } +/******************************************************************************** + * Drier Loading Arm Homing + ********************************************************************************/ +uint32_t Drier_Center = 0; +float Calculate_Arm_Angle(uint32_t Current_Angle) +{ + uint32_t angle; + float Calc_angle; + if (Current_Angle >= Drier_Center) + angle = Current_Angle - Drier_Center; + else + angle = Current_Angle + 0x3FFF - Drier_Center; + Calc_angle = angle/0x3FFF; + MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); + ReportWithPackageFilter(DiagnosticsFilter,"Calculate_Arm_Angle",__FILE__,(int)(Calc_angle*1000),Current_Angle,RpMessage,Drier_Center,0); + return Calc_angle; +} uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { MessageContainer responseContainer; @@ -490,7 +507,7 @@ uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadVal uint32_t temp = Read_Dryer_ENC_Position(); ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0); - if ((abs (temp -D_DrierPrevLocation)>1000 )&&(ReadValue == NOTBUSY)) // OK - take another round + if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round // if (ReadValue == NOTBUSY) // OK - take another round { D_DrierPrevLocation = temp; @@ -510,7 +527,7 @@ uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadVal } else //timeout or no movement { - ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + ReportWithPackageFilter(DiagnosticsFilter,"Unloading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm ReportWithPackageFilter(DiagnosticsFilter,"Drier unloading timeout(1) or no movement",__FILE__,temp,D_DrierPrevLocation,RpWarning,ReadValue,0); @@ -533,6 +550,7 @@ uint32_t Diagnostics_Dryer_UnLoading(void) MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + Drier_Center = temp; D_DrierPrevLocation = temp; ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_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); @@ -548,6 +566,7 @@ uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint MessageContainer responseContainer; MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT; + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,0,RpMessage,CallbackCounter,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[MotorId], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size); @@ -586,7 +605,8 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue } else { - ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + ReportWithPackageFilter(DiagnosticsFilter,"Loading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); } return OK; @@ -594,9 +614,14 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue uint32_t Diagnostics_Dryer_Loading(void) { + //float Calc_angle; +#warning assuming loading starts from 0 if (dryerbufferlength) LoadArmRounds = (int)dryerbufferlength; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&D_DrierPrevLocation); + //MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); + //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + Drier_Center = D_DrierPrevLocation; D_numberOfCycles = 0; D_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; diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c index 3c9e4ef05..b35259ad9 100644 --- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c +++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c @@ -353,7 +353,10 @@ void IDS_Dispenser_Content_Calculation (char DispenserId) // IDS_Dispenser_Data[DispenserId].nanolitterperpulse ;//* dir; //double consumedintimeframe = (double)(CurrentDispenserSpeed[DispenserId]); //pulses only //consumedintimeframe = 10+DispenserId; - IDS_Dispenser_Data[DispenserId].consumedinnanolitter += (CurrentDispenserSpeed[DispenserId]*4/10);//100 milliseconds ==> speed/10 + IDS_Dispenser_Data[DispenserId].consumedinnanolitter += (CurrentDispenserSpeed[DispenserId]*4/10);//400 milliseconds ==> speed*4/10 + if ((DispenserId == 2)&&(CurrentDispenserSpeed[DispenserId]>0)) + ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_Data ",__FILE__,DispenserId,(int)IDS_Dispenser_Data[DispenserId].consumedinnanolitter,RpWarning,(int)CurrentDispenserSpeed[DispenserId],0); + if (IDS_Dispenser_Data[DispenserId].consumedinnanolitter<0) IDS_Dispenser_Data[DispenserId].consumedinnanolitter = 0; if (CurrentDispenserSpeed[DispenserId]>0.1) @@ -367,7 +370,7 @@ void IDS_Dispenser_Content_Calculation (char DispenserId) } if (DispenserId == 0) { - if ((seconds_counter++>=9000)&&(DispenserDataUpdated == true))//9000)// - one hour) + if ((seconds_counter++>=600)&&(DispenserDataUpdated == true))//9000)// - one hour) { seconds_counter = 0; if (IDS_Dispenser_Data[DispenserId].consumedinnanolitter) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index bc240cd0f..a4172185a 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); @@ -369,7 +374,7 @@ 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(ACTOT, ENABLE, UP); //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP); //Trigger_Head_Actuators_Control(ACTOT, LOW,true);//UP Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP @@ -393,6 +398,15 @@ status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1); } + if (Head_Type == HEAD_TYPE_FLAT) + { + Report("Thread_Load_Lift_ OT actuator",__FILE__,__LINE__,LOW,RpMessage,false,0); + Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP); + //Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP); + //Trigger_Head_Actuators_Control(ACTOT, LOW,true);//UP + //Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP + } + return OK; } uint32_t Thread_Load_Lift_Rockers(void) @@ -460,6 +474,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_ OT actuator",__FILE__,__LINE__,LOW,RpMessage,true,0); + Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN); + } return OK; } @@ -495,29 +514,24 @@ }*/ } - return OK; - } - uint32_t Thread_Load_Close_Lids(void) - { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids"); - //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_Control(ACTIN, LOW,false);//DOWN + //Trigger_Head_Actuators_Control(ACTIN, LOW,false);//DOWN //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); - } + return OK; + } + uint32_t Thread_Load_Close_Lids(void) + { + REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids"); + //Close Dyeing Head Cover And Dryer Lid + //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, + //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, 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; @@ -530,6 +544,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) @@ -662,7 +685,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; @@ -808,6 +831,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); @@ -816,7 +844,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); @@ -845,14 +876,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); @@ -871,14 +902,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 (tempdata.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; -- cgit v1.3.1