diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
6 files changed, 1137 insertions, 421 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index d1d23598f..13032f49d 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -17,6 +17,7 @@ #include "../control/pidalgo.h" #include "thread_ex.h" +#include "drivers/SSI_Comm/SSI_Comm.h" #define NORMAL_COEF_DIVIDER 100 typedef struct @@ -42,12 +43,27 @@ typedef struct PID_Config_Params m_params; }MotorControlConfig_t; +#ifdef FOUR_WINDERS +#define SCREW_2_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE2_1 +#define SCREW_3_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE1_1 +#define SCREW_4_Motor HARDWARE_MOTOR_TYPE__MOTO_RDANCER +#define Winder_2_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2 +#define Winder_3_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE1_2 +#define Winder_4_Motor HARDWARE_MOTOR_TYPE__MOTO_RLOADARM +#endif + #define MAX_THREAD_FEED_MOTORS (WINDER_MOTOR+1) -#define MAX_SYSTEM_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) +#define MAX_SYSTEM_DANCERS NUM_OF_ROTENC extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM]; + +#ifdef FOUR_WINDERS +extern Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM]; +#else extern HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM]; +#endif + extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM]; extern bool BreakSensorenabled; @@ -76,7 +92,6 @@ extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM]; uint32_t InternalWinderConfigMessage(HardwareWinder* request); uint32_t MotorsConfigMessage( HardwareConfiguration* HW_request); uint32_t InternalWindingConfigMessage(JobSpool* request); -uint32_t ThreadInitialTestStub(HardwareMotor * request); uint32_t MotorPidRequestMessage(HardwarePidControl* request); void SetKeepWindingCone(bool value); void SetWinderBackToBaseTime(uint32_t value); @@ -85,6 +100,10 @@ uint32_t DancerConfigMessage(HardwareDancer * request); uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read); void SetOriginMotorSpeed(float process_speed); +uint32_t UpdatePidDuringRun(HardwarePidControl *hardwarepidcontrol); + +uint32_t ThreadPrepare_Tension (int DancerId, double tension); + #endif //MODULES_THREAD_THREAD_H_ 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; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 41ef9e480..a6cb25c45 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -17,7 +17,7 @@ #include "Common/report/report.h" #include "drivers/FPGA/FPGA.h" - +#include "drivers/Flash_ram/MCU_E2Prom.h" #include "drivers/FPGA/FPGA_SPI_Comm.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" @@ -54,7 +54,7 @@ double ScrewRunningTime = 0; uint32_t ScrewNumberOfSteps = 0; //holds the current number of steps for the next screw run - will be used to build the cone bool SCREW_TimerActivated = false; uint32_t ScrewControlId = 0xFF; - +bool IgnoreConeMissing = false; static bool KeepWindingCone = false; @@ -89,26 +89,38 @@ void SetWinderBackToBaseTime(uint32_t value) ReportWithPackageFilter(ThreadFilter,"Set WinderBackToBaseTime",__FILE__,__LINE__,WinderBackToBaseTime,RpWarning,KeepWindingCone, 0); } -uint32_t InternalWinderConfigMessage(HardwareWinder* request) +/*uint32_t InternalWinderConfigMessage(HardwareWinder* request) { uint32_t status = PASSED; InternalWinderCfg.milimetersperrotation = request->millimeterperrotation; return status; -} +}*/ char ScrewStr[150]; uint32_t InternalWindingConfigMessage(JobSpool* request) { uint32_t status = PASSED; + int limitswitchstartpointoffset = 0; InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses; InternalWinderCfg.spoolbackingrate = request->backingrate; InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; + if (request->has_limitswitchstartpointoffset) + { + //InternalWinderCfg.startoffsetpulses += request->limitswitchstartpointoffset; + Report("limit switch start point offset",__FILE__,__LINE__,(int)(request->startoffsetpulses),RpWarning,(int)(request->limitswitchstartpointoffset), 0); + } + status |= MCU_E2PromRead(EEPROM_STORAGE_WINDER_CALIBRATION,&limitswitchstartpointoffset); + if ((status!= OK )||(limitswitchstartpointoffset == 0xFFFF)) + limitswitchstartpointoffset = 0; + Report("limit switch start point offset",__FILE__,(int)(request->startoffsetpulses),(int)(limitswitchstartpointoffset),RpWarning,status, 0); + InternalWinderCfg.startoffsetpulses += limitswitchstartpointoffset; + InternalWinderCfg.SpoolBottomBackingRate = request->bottombackingrate; InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*3;//request->rotationsperpassage; - if (request->rotationsperpassage > 6.1) + if (request->rotationsperpassage > 5.1) InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage; if (NumberOfRotationPerPassage > 1) { @@ -118,7 +130,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request) InternalWinderCfg.diameter = request->diameter; usnprintf(ScrewStr, 150, "WindingConfig start,offset,head,tail {, %d, %d, %d, %d, %d}",InternalWinderCfg.startoffsetpulses,(int)InternalWinderCfg.segmentoffsetpulses, (int)InternalWinderCfg.spoolbackingrate,(int)InternalWinderCfg.SpoolBottomBackingRate,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000)); - Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.diameter,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); + Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.startoffsetpulses,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); return status; } @@ -128,6 +140,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request) * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. * report ready to the job STM */ +int Screw_wait_counter; uint32_t Winder_Prepare(void *JobDetails) { JobTicket* JobTicket = JobDetails; @@ -149,9 +162,17 @@ uint32_t Winder_Prepare(void *JobDetails) if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); - //PrepareReady(Module_Winder,ModuleFail); - //AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true); - //return ERROR; + if (Is_PP_Machine()) + { + if (IgnoreConeMissing == false) + { + JobEndReason = JOB_THREAD_BREAK; + usnprintf(AlarmReasonStr, 100, "No cone in winder"); + PrepareReady(Module_Winder,ModuleFail); + AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true); + return ERROR; + } + } } #ifdef READ_SCREW_ENCODER @@ -187,7 +208,16 @@ uint32_t Winder_Prepare(void *JobDetails) //REPORT_MSG(ScrewSpeed, "Winder_Prepare"); Winder_ScrewHoming = true; //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit"); + Screw_wait_counter=1; status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2,2000); +#ifdef FOUR_WINDERS + Screw_wait_counter++; + status = MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, Winder_PrepareStage2,2000); + Screw_wait_counter++; + status = MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, Winder_PrepareStage2,2000); + Screw_wait_counter++; + status = MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, Winder_PrepareStage2,2000); +#endif } return status; } @@ -202,7 +232,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) uint32_t status=OK; uint32_t numOfSteps = WindingConeLocation*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; - //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses); + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses); //REPORT_MSG(numOfSteps, "Winder_PrepareStage2"); #ifdef READ_SCREW_ENCODER Read_Screw_Encoder(); @@ -211,13 +241,15 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) #endif REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2"); + Screw_wait_counter--; if (ReadValue != LIMIT) { if (JobIsActive()==true) { - LOG_ERROR(ReadValue,"Screw failed to reach the limit switch!!"); + LOG_ERROR(deviceID,"Screw failed to reach the limit switch!!"); //returned with a timeout Winder_ScrewAtOffsetCallback(0,0); + PrepareReady(Module_Winder, ModuleFail); } else //short jog, etc { @@ -226,18 +258,39 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) } else { + if (Screw_wait_counter!=0) + { + ReportWithPackageFilter(ThreadFilter,"waiting for winders",__FILE__,__LINE__,Screw_wait_counter,RpWarning,deviceID, 0); + return status; + } + Screw_wait_counter++; status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000); +#ifdef FOUR_WINDERS + Screw_wait_counter++; + status |= MotorMoveWithCallback (SCREW_2_Motor,(1-MotorsCfg[SCREW_2_Motor].directionthreadwize), numOfSteps, Winder_ScrewAtOffsetCallback,1000); + Screw_wait_counter++; + status |= MotorMoveWithCallback (SCREW_3_Motor,(1-MotorsCfg[SCREW_3_Motor].directionthreadwize), numOfSteps, Winder_ScrewAtOffsetCallback,1000); + Screw_wait_counter++; + status |= MotorMoveWithCallback (SCREW_4_Motor,(1-MotorsCfg[SCREW_4_Motor].directionthreadwize), numOfSteps, Winder_ScrewAtOffsetCallback,1000); +#endif } //set motor location 0 here return status; } -uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) +uint32_t Winder_ScrewAtOffsetCallback(uint32_t MotorId, uint32_t BusyFlag) { //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); + if (Screw_wait_counter) + Screw_wait_counter--; - MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands + MotorStop (MotorId,Soft_Hiz); //per L6470 errata between mov and run commands +/*#ifdef FOUR_WINDERS + MotorStop(SCREW_2_Motor, Hard_Hiz); + MotorStop(SCREW_3_Motor, Hard_Hiz); + MotorStop(SCREW_4_Motor, Hard_Hiz); +#endif*/ Task_sleep(5); #ifdef READ_SCREW_ENCODER Reset_Screw_Encoder(); @@ -248,7 +301,12 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location"); #endif - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + SetMotHome(MotorId); //set this point as the spool home +/*#ifdef FOUR_WINDERS + SetMotHome(SCREW_2_Motor); + SetMotHome(SCREW_3_Motor); + SetMotHome(SCREW_4_Motor); +#endif*/ ScrewCurrentDirection = false; #ifdef READ_SCREW_ENCODER @@ -257,8 +315,9 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) ScrewSpeed = 0; ScrewControlId = 0xFF; ScrewNumberOfSteps = 0; - REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback"); - PrepareReady(Module_Winder, ModuleDone); + ReportWithPackageFilter(ThreadFilter,"Winder_ScrewAtOffsetCallback",__FILE__,millisecondCounter,Screw_wait_counter,RpWarning,MotorId, 0); + if (Screw_wait_counter == 0) + PrepareReady(Module_Winder, ModuleDone); return OK; } @@ -298,10 +357,17 @@ int flipflop = 0; uint32_t motspeed; float speedf; int WinderCalculation = 0; +#ifdef FOUR_WINDERS +int MotorOrder[4] = {HARDWARE_MOTOR_TYPE__MOTO_SCREW,SCREW_2_Motor,SCREW_3_Motor,SCREW_4_Motor}; +#endif + uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { //uint32_t Steps; double temp,tempScrewSpeed; +#ifdef FOUR_WINDERS + int tempmot; +#endif //double screw_horizontal_speed = 0; //double RotationsPerSecond; if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION) @@ -320,6 +386,14 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) ScrewCurrentDirection = 1-ScrewCurrentDirection; CalculationDirectionChangeCounter++; +#ifdef FOUR_WINDERS + tempmot = MotorOrder[0]; + MotorOrder[0] = MotorOrder[1]; + MotorOrder[1] = MotorOrder[2]; + MotorOrder[2] = MotorOrder[3]; + MotorOrder[3] = tempmot; + //Report("order change" ,__FILE__,MotorOrder[0],MotorOrder[1],RpWarning,MotorOrder[2], 0); +#endif if (SampleWinding) return OK; //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed; @@ -539,7 +613,7 @@ uint32_t WinderDistanceToSpoolEnded(uint32_t deviceID, uint32_t ReadValue) } uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag) { - REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback called"); + Report("ScrewDTSCallback called", __FILE__,__LINE__,msec_millisecondCounter, RpMessage, ScrewSpeed, 0); SafeRemoveControlCallback(ScrewDTSControlId, ScrewDTSCallback); if (ScrewControlId != 0xFF) @@ -553,6 +627,11 @@ uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag) //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0); //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, WinderDistanceToSpoolEnded,2000); +#ifdef FOUR_WINDERS + MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, NULL,2000); + MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, NULL,2000); + MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, NULL,2000); +#endif return OK; } @@ -562,7 +641,7 @@ uint32_t WinderDistanceToSpoolState(void ) REPORT_MSG ((int)DTS_Time,"WinderDistanceToSpoolState"); ScrewDTSControlId = AddControlCallback("screw DTS",ScrewDTSCallback, DTS_Time-WinderBackToBaseTime,TemplateDataReadCBFunction,0,0,0); - REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback start"); + Report("ScrewDTSCallback start", __FILE__,DTS_Time,msec_millisecondCounter, RpMessage, WinderBackToBaseTime, 0); return OK; } @@ -582,9 +661,14 @@ uint32_t Winder_End(void) //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0); //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //bug #2709 +#ifdef FOUR_WINDERS + MotorAbortMovetoLimitSwitch (SCREW_2_Motor); + MotorAbortMovetoLimitSwitch (SCREW_3_Motor); + MotorAbortMovetoLimitSwitch (SCREW_4_Motor); +#endif return OK; } -void Winder_ScrewHomeLimitSwitchInterrupt(void) +/*void Winder_ScrewHomeLimitSwitchInterrupt(void) { //uint32_t status; //handle glitch - send information to the next time that the motor stops @@ -599,7 +683,7 @@ void Winder_ScrewOutLimitSwitchInterrupt(void) //handle glitch - send information to the next time that the motor stops //uint32_t status; MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out -} +}*/ uint32_t Screw_timerBase = TIMER3_BASE; //Timer handle @@ -643,7 +727,19 @@ void ScrewTimerInterrupt(int ARG0) { ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime); MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection); +#ifdef FOUR_WINDERS + MotorSetDirection (SCREW_2_Motor, ScrewCurrentDirection); + MotorSetDirection (SCREW_3_Motor, ScrewCurrentDirection); + MotorSetDirection (SCREW_4_Motor, ScrewCurrentDirection); +#endif +#ifdef FOUR_WINDERS + MotorSetSpeedDirect(MotorOrder[0],ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[1], ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[2], ScrewSpeed); + MotorSetSpeedDirect (MotorOrder[3], ScrewSpeed); +#else MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); +#endif #ifdef READ_SCREW_ENCODER Read_Screw_Encoder(); ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 1e730e252..5d69e2980 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -9,7 +9,7 @@ #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer #define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer #define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer -#define NUM_OF_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) +#define NUM_OF_DANCERS NUM_OF_ROTENC //} DANCER_ENUM; typedef enum threadMotorsEnum { @@ -17,12 +17,19 @@ typedef enum threadMotorsEnum DRYER_MOTOR, POOLER_MOTOR, WINDER_MOTOR, +#ifdef FOUR_WINDERS + WINDER_2_MOTOR, + WINDER_3_MOTOR, + WINDER_4_MOTOR, +#endif SCREW_MOTOR, MAX_THREAD_MOTORS_NUM }threadMotorsEnum; extern double TotalProcessedLength; extern double PoolerTotalProcessedLength; +extern int SecondFeederCorrection; +extern bool SkipOpenLids,IgnoreConeMissing; void ThreadSetBreakSensorLimit(int limit); @@ -36,7 +43,6 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId); uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId); uint32_t ThreadDistanceToSpoolState(void); uint32_t ThreadEndState(); -uint32_t ThreadInitialTestStub(); bool Set_Thread_Rockers_Bypass (int value); @@ -81,7 +87,8 @@ void ThreadLoadRequest(MessageContainer* requestContainer); uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer); uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer); uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer); - +uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer); +void ThreadCheckArcHeadCovers(void); uint32_t Thread_Load_End(void); #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index fb3b78752..1920db03a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -104,14 +104,14 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest) MotorsCfg[Motor_i].tfast = request->tfast ; MotorsCfg[Motor_i].tonmin = request->tonmin ; MotorsCfg[Motor_i].toffmin = request->toffmin ; + MotorsCfg[Motor_i].has_p01configword = (bool)(request->has_p01configword) ;//needed for backward compatibility + MotorsCfg[Motor_i].p01configword = request->p01configword ; status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]); if ((Motor_i >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_i <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8)) { IDS_Dispenser_Init(Motor_i-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1); } -// if (Motor_i == MOTOR_RDRIVING) -// ThreadInitialTestStub(request); } return status; @@ -145,8 +145,45 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2); + if (Motor_i == WINDER_MOTOR) + { + temp = temp*3/2; + } DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_MOTOR) + { + int j,winderMotorId; + for (j=0;j<3;j++) + { + winderMotorId = j+WINDER_2_MOTOR; + memcpy (&MotorsControl[winderMotorId],request,sizeof(HardwarePidControl)); + if (MotorsControl[winderMotorId].pvinputfilterfactormode > MAX_CONTROL_SAMPLES) + MotorsControl[winderMotorId].pvinputfilterfactormode = MAX_CONTROL_SAMPLES; + #ifdef TEST_LONGER_PID_THREAD + MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control + #endif + for (i = 0;i < (int)MotorsControl[winderMotorId].pvinputfilterfactormode; i++) + { + MotorSamples[winderMotorId][i] = 0; //reset the samples value for control beginning + MotorSpeedSamples[winderMotorId][i] = 0; + } + NormalizedErrorCoEfficient[winderMotorId] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + NormalizedErrorCoEfficient[winderMotorId] = NormalizedErrorCoEfficient[winderMotorId] / temp; + // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2); + if (Motor_i == WINDER_MOTOR) + { + temp = temp*3/2; + } + DancerStopActivityLimit[winderMotorId] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + } + } +#endif return OK; } char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg"; @@ -155,31 +192,37 @@ uint32_t StoreDancerConfigMessage() { uint32_t status = OK; // FRESULT Fresult = FR_OK; - HardwareConfiguration DancerConfig; + //HardwareConfiguration DancerConfig; HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; // uint8_t* response_buffer; // size_t response_size = 0; int Dancer_i; - hardware_configuration__init(&DancerConfig); - DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); + //hardware_configuration__init(&DancerConfig); + //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++) { hardware_dancer__init(&Dancers[Dancer_i]); - DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; + //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i]; Dancers[Dancer_i].has_zeropoint = true; Dancers[Dancer_i].hardwaredancertype = Dancer_i; Dancers[Dancer_i].has_hardwaredancertype = true; - DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0); - DancerConfig.n_dancers++; - DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; + Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0); + //DancerConfig.n_dancers++; + DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint; + DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0); } status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint); status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint); Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0); -/* +#ifdef FOUR_WINDERS + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint); + status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_4,DancersCfg[HARDWARE_DANCER_4].zeropoint); + Report("~~~~~~Store eeprom 1",__FILE__,__LINE__,(int)DancersCfg[HARDWARE_DANCER_3].zeropoint,RpWarning,(int)DancersCfg[HARDWARE_DANCER_4].zeropoint,0); +#endif + /* response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig)); if (response_buffer) { @@ -204,11 +247,18 @@ uint32_t LoadDancerConfigMessage() MCU_E2PromRead(EEPROM_STORAGE_DANCER_0,&DancersCfg[0].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_1,&DancersCfg[1].zeropoint); MCU_E2PromRead(EEPROM_STORAGE_DANCER_2,&DancersCfg[2].zeropoint); - +#ifdef FOUR_WINDERS + MCU_E2PromRead(EEPROM_STORAGE_DANCER_3,&DancersCfg[3].zeropoint); + MCU_E2PromRead(EEPROM_STORAGE_DANCER_4,&DancersCfg[4].zeropoint); +#endif status |= Report("Dancer 0 Store data eeprom ",__FILE__,__LINE__,0,RpWarning,(int)DancersCfg[0].zeropoint,0); status |= Report("Dancer 1 Store data eeprom ",__FILE__,__LINE__,1,RpWarning,(int)DancersCfg[1].zeropoint,0); status |= Report("Dancer 2 Store data eeprom ",__FILE__,__LINE__,2,RpWarning,(int)DancersCfg[2].zeropoint,0); +#ifdef FOUR_WINDERS + status |= Report("Dancer 3 Store data eeprom ",__FILE__,__LINE__,3,RpWarning,(int)DancersCfg[3].zeropoint,0); + status |= Report("Dancer 4 Store data eeprom ",__FILE__,__LINE__,4,RpWarning,(int)DancersCfg[4].zeropoint,0); +#endif return status; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 69f640c45..bc4e6cb8f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -37,6 +37,8 @@ #include "Control/MillisecTask.h" #include "drivers/Flash_ram/MCU_E2Prom.h" +#include "drivers/SSI_Comm/SSI_Comm.h" + ////////////////////////////////State machine operation//////////////////////////////////// //the state machine operation is used to operate in runtime correct profile flow execution //by recieved esign flow of the user from the UI @@ -44,9 +46,15 @@ double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; +#ifdef FOUR_WINDERS +TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; +Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS}; +uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; +#else TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF}; +#endif uint32_t SpeedControlId=0xFF; uint32_t PoolerSpeedControlId=0xFF; @@ -58,6 +66,10 @@ uint32_t JobCounter = 0; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; +int MotorTiming[MAX_THREAD_MOTORS_NUM]; +int MotorTimer[MAX_THREAD_MOTORS_NUM]; + + uint32_t PreviousPosition = 0, CurrentPosition = 0; double CurrentRequestedLength = 0.0; double CurrentProcessedLength = 0.0; @@ -140,15 +152,22 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea * **************************************************************************************/ uint32_t initialpos = 0xFFFF; uint32_t Poolerinitialpos = 0xFFFF; +#define SPEED_STORE_SIZE 20 +float PullerSpeedStore[SPEED_STORE_SIZE]; +float PullerSpeedAverage; +int PullerSpeedIndex = 0; +float FeederSpeedStore[SPEED_STORE_SIZE]; +float FeederSpeedAverage; +int FeederSpeedIndex = 0,Speed_i; void ThreadUpdateProcessLength (double length, void *Funcptr) { - REPORT_MSG(length,"ThreadUpdateProcessLength"); + ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; } -char Lenstr[160]; +char Lenstr[190]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -206,6 +225,16 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } } + /*FeederSpeedStore[FeederSpeedIndex++] = length; + if (FeederSpeedIndex>=SPEED_STORE_SIZE) + { + FeederSpeedIndex = 0; + FeederSpeedAverage = 0; + for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++) + FeederSpeedAverage+=FeederSpeedStore[Speed_i]; + FeederSpeedAverage = FeederSpeedAverage/SPEED_STORE_SIZE; + ReportWithPackageFilter(ThreadFilter,"Avg len 100ms last 2 sec",__FILE__,(int)PoolerTotalProcessedLength,(int)(FeederSpeedAverage*1000),RpWarning,(int)(PullerSpeedAverage*1000),0); + }*/ TotalProcessedLength += (length/100); TempTotalProcessedLength = TotalProcessedLength; #ifdef FEEDER_LENGTH_CALCULATION @@ -242,7 +271,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) return OK; } - uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -297,9 +325,22 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) length = 0; } + /*PullerSpeedStore[PullerSpeedIndex++] = length; + if (PullerSpeedIndex>=SPEED_STORE_SIZE) + { + PullerSpeedIndex = 0; + PullerSpeedAverage = 0; + for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++) + PullerSpeedAverage+=PullerSpeedStore[Speed_i]; + PullerSpeedAverage = PullerSpeedAverage/SPEED_STORE_SIZE; + //ReportWithPackageFilter(ThreadFilter,"Average Speed 2 second",__FILE__,__LINE__,(int)(FeederSpeedAverage*100),RpWarning,(int)(PullerSpeedAverage*100),0); + }**/ //} - +#ifdef BTSR_NO_TFU + if (CurrentControlledSpeed[WINDER_MOTOR]>100) + length = dyeingspeed/10; +#endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION @@ -322,9 +363,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { - usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -393,7 +434,72 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } +uint16_t BreakSensorCounter = 0; +uint16_t BreakSensorLatchCounter = 0; +char TMessage[150]; +#ifdef FOUR_WINDERS +char ATMessage[MAX_THREAD_MOTORS_NUM][150]; +int c = 0; +#endif +uint32_t checkBreakSensor(uint32_t index) +{ + if (BreakSensorenabled == true) + { + if (JobCounter > eOneSecond) + { + if (ReadBreakSensor()==ERROR) + { + BreakSensorCounter++; + BreakSensorLatchCounter++; + if (BreakSensorCounter>=BreakSensordebouncetimemilli) + { + //consider applying the debouce parameters later + usnprintf(TMessage, 60, "ReadBreakSensor Error"); + //BreakSensordebouncetimemilli + JobEndReason = JOB_THREAD_BREAK; + ThreadControlActive = false; + SendJobProgress(0.0,0,false, TMessage); + strcpy(AlarmReasonStr,TMessage); + SendSegmentFail(); + //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); + //EndState(CurrentJob,"ReadBreakSensor Error" ); + ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); + return ERROR; + } //passed limit + }//ReadBreakSensor()==ERROR + else //reset counter - we are looking for consequent calls + { + if (BreakSensorCounter) + { + ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); + } + BreakSensorCounter = 0; + } + if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3)) + { + BreakSensorCounter++; + BreakSensorLatchCounter++; + if (BreakSensorCounter>=BreakSensordebouncetimemilli) + { + //consider applying the debouce parameters later + usnprintf(TMessage, 60, "thread speed too low"); + strcpy(AlarmReasonStr,TMessage); + JobEndReason = JOB_THREAD_BREAK; + ThreadControlActive = false; + SendJobProgress(0.0,0,false, TMessage); + SendSegmentFail(); + //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); + //EndState(CurrentJob,"ReadBreakSensor Error" ); + ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); + return ERROR; + } + } + } + } + return OK; + +} int controlIndex = 0; bool keepdata = true; int32_t KeepReadValue = 0; @@ -447,20 +553,15 @@ void testDancersControl() } #endif int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; -char TMessage[150]; //char time[150]; -uint16_t BreakSensorCounter = 0; -uint16_t BreakSensorLatchCounter = 0; bool FirstCalcInJob = true; uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; - //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; - int len; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; @@ -480,6 +581,23 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } index = IfIndex&0xFF; + if (index == POOLER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec + { + if (checkBreakSensor(index) == ERROR) + return OK; + } + if (MotorTiming[index]>1) + { + MotorTimer[index]++; + if (MotorTimer[index]>=MotorTiming[index]) + { + MotorTimer[index]=0; + } + else + { + return OK; + } + } if(MotorControlConfig[index].m_isEnabled ) { //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true) @@ -511,6 +629,30 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } +#ifdef FOUR_WINDERS + if (index == WINDER_MOTOR) + { + c++; + } + if (index == WINDER_2_MOTOR) + { + c++; + } + if (index == WINDER_3_MOTOR) + { + c++; + } + if (index == WINDER_4_MOTOR) + { + c++; + } + //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR)) + /*if (index >= WINDER_MOTOR) + { + //pooler dancer is right sided: data is opposite + TranslatedReadValue = (-1*TranslatedReadValue); + }*/ +#endif if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite @@ -529,61 +671,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; - if (BreakSensorenabled == true) - { - if (index == POOLER_MOTOR) - { - if (JobCounter > eOneSecond) - { - if (ReadBreakSensor()==ERROR) - { - BreakSensorCounter++; - BreakSensorLatchCounter++; - if (BreakSensorCounter>=BreakSensordebouncetimemilli) - { - //consider applying the debouce parameters later - usnprintf(TMessage, 60, "ReadBreakSensor Error"); - //BreakSensordebouncetimemilli - JobEndReason = JOB_THREAD_BREAK; - ThreadControlActive = false; - SendJobProgress(0.0,0,false, TMessage); - SendSegmentFail(); - //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); - //EndState(CurrentJob,"ReadBreakSensor Error" ); - ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); - return OK; - } //passed limit - }//ReadBreakSensor()==ERROR - else //reset counter - we are looking for consequent calls - { - if (BreakSensorCounter) - { - ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); - } - BreakSensorCounter = 0; - } - if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3)) - { - BreakSensorCounter++; - BreakSensorLatchCounter++; - if (BreakSensorCounter>=BreakSensordebouncetimemilli) - { - //consider applying the debouce parameters later - usnprintf(TMessage, 60, "thread speed too low"); - JobEndReason = JOB_THREAD_BREAK; - ThreadControlActive = false; - SendJobProgress(0.0,0,false, TMessage); - SendSegmentFail(); - //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); - //EndState(CurrentJob,"ReadBreakSensor Error" ); - ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); - return OK; - } - } - - } - } - } //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) @@ -595,7 +682,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; +#ifdef FOUR_WINDERS + if (DancerId>HARDWARE_DANCER_2) + JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0; +#endif SendJobProgress(0.0,0,false, TMessage); + strcpy(AlarmReasonStr,TMessage); //EndState(CurrentJob,TMessage ); SendSegmentFail(); /*switch (index) @@ -647,21 +739,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + if (index < WINDER_MOTOR) + calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; - if (FirstCalcInJob == true) + //if (0)//(JobCounter % 1000 == 0) +#ifdef FOUR_WINDERS + if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) { - if (index == FEEDER_MOTOR) + if (index >= WINDER_MOTOR) { - FirstCalcInJob = false; - len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", - ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), + // FirstCalcInJob = false; + usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1, + TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), - (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed); - ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); + ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); } + JobCounter++; } - +#endif #ifndef TEST_PID_THREAD if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) #else @@ -670,6 +767,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { if (calculated_speed>5.0) { + /*if (calculated_speed>(CurrentControlledSpeed[index]+100)) + { + ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,index,0); + calculated_speed=CurrentControlledSpeed[index]+100; + }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } @@ -684,7 +786,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); }*/ #ifdef TEST_PID_THREAD - int len; if ((JobCounter % 2000) == index*100) //if (keepdata == true) { @@ -696,7 +797,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam; mIntegral[controlIndex] = MotorControlConfig[index].m_integral; timestamp[controlIndex] = msec_millisecondCounter;*/ - len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", + usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed); @@ -740,64 +841,166 @@ double ThreadGetMotorCalculatedError(int DancerId) } //******************************************************************************************************************** -uint32_t ThreadInitialTestStub(HardwareMotor * request) -{ - - - //MotorsConfigMessage(request); - ThreadPrepareState(request); - ThreadPreSegmentState(request,0); - return OK; -} uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) { - /* - FEEDER_MOTOR, - DRYER_MOTOR, - POOLER_MOTOR, - WINDER_MOTOR, - MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; - MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; - MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; - - */ if (ThreadParams == NULL) { 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.Kd = ThreadParams->feederp; + MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp; if(ThreadParams->feederi) MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi; if(ThreadParams->feederd) MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd; if(ThreadParams->pullerp) - MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerp; + MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp; if(ThreadParams->pulleri) MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri; if(ThreadParams->pullerd) MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd; if(ThreadParams->winderp) - MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderp; + { + MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp; + MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp; + MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp; +#endif + } if(ThreadParams->winderi) + { MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi; + MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi; + MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi; +#endif + } if(ThreadParams->winderd) + { MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd; +#ifdef FOUR_WINDERS + MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd; + MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd; + MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd; +#endif + } + ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0); if(ThreadParams->bypassrockers) Thread_Rockers_Bypass = true; return OK; } +bool RTFU_Up = false; + +uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag) +{ + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); + Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0); + + MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands + return OK; +} +uint32_t Release_Right_TFU_Tension() +{ + uint32_t status = OK; +#ifndef BTSR_NO_TFU + if (RTFU_Up == true) + { + Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0); + RTFU_Up = false; + status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000); + } +#endif + return status; +} +int SecondFeederCorrection = 4; +int PrepareWaitCount = 0; +uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) +{ +#ifndef BTSR_NO_TFU + MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands + Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0); + if (JobIsActive()==false) + { + Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0); + Release_Right_TFU_Tension(); + } + if (PrepareWaitCount) + { + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); + PrepareWaitCount--; + } + if ((PrepareWaitCount == 0)&&(PrepareState == true)) + { + PrepareState = false; + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); + PrepareReady(Module_Thread,ModuleDone); + } +#endif + return OK; +} +uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) +{ +#ifndef BTSR_NO_TFU + Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0); + MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000); + RTFU_Up = true; +#endif + return OK; +} + +uint32_t Adjust_Right_TFU_Tension(double tension) +{ + uint32_t status = OK; +#ifndef BTSR_NO_TFU + if (tension > 0.5) //0 = lower position, 1 = high position + { + if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) + { + PrepareWaitCount++; + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); + Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0); + } + } +#endif + return status; +} +uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension) +{ + //MotorStop(MotorId,Hard_Hiz); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); + if (PrepareWaitCount) + { + PrepareWaitCount--; + } + if ((PrepareWaitCount == 0)&&(PrepareState == true)) + { + PrepareState = false; + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); + PrepareReady(Module_Thread,ModuleDone); + } + return OK; +} uint32_t ThreadPrepare_Tension (int DancerId, double tension) { int current, request = (int)tension,movement; int HW_Motor_Id; bool direction; uint32_t status = OK, address = 0; - if (tension < 100) - return OK; //do not handle tension of zero switch (DancerId) { case HARDWARE_DANCER_TYPE__LeftDancer: @@ -811,10 +1014,13 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2; break; case HARDWARE_DANCER_TYPE__RightDancer: - return ERROR; + return Adjust_Right_TFU_Tension(tension); + //break; default: return ERROR; } + if (tension < 100) + return OK; //do not handle tension of zero status |= MCU_E2PromRead(address,¤t); if ((status!= OK )||(current == 0xFFFF)) return status; @@ -832,24 +1038,37 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize; movement = current - request; } - MotorSetMaxSpeed (HW_Motor_Id, 500); - MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000); + MotorSetMaxSpeed (HW_Motor_Id, 800); + MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000); + PrepareWaitCount++; + ReportWithPackageFilter(ThreadFilter,"PrepareWaitCount",__FILE__,PrepareWaitCount,current,RpWarning,request,0); status |= MCU_E2PromProgram(address,request); } - usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d", - DancerId,request,current,movement,direction,HW_Motor_Id,address); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + if (DancerId == HARDWARE_DANCER_TYPE__LeftDancer) + { + usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", + DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,current,RpWarning,request,0); + } + else + { + usnprintf(TMessage, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", + DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); + ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,address,current,RpWarning,request,0); + + } return status; } //******************************************************************************************************************** - uint32_t ThreadPrepareState(void *JobDetails) +bool SkipOpenLids = true; +uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i,i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; uint32_t status = OK; CurrentSegmentId = 0; - + float temp_dt = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; @@ -873,13 +1092,15 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; + PrepareWaitCount = 0; +#ifndef BTSR_NO_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,status,RpFatalError,(int)windertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,status,RpFatalError,(int)pullertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,status,RpFatalError,(int)feedertension,0); - + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0); +#endif FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { @@ -893,19 +1114,57 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop); - if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT) + /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT) { - ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; + // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; } if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT) { - ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpFatalError,LIMIT,0); + ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; + // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; + }*/ + +// if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false)) + if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false)) + { + if(Head_Type != HEAD_TYPE_ARC) + { + ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0); + if (SkipOpenLids == false) + { + JobEndReason = JOB_LIDS_OPEN; + usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); + PrepareReady(Module_Thread,ModuleFail); + return ERROR; + } + } + } + if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { + if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { + ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); + } + if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { + ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); + } + } +// if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false)) + if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false)) + { + ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0); + if (SkipOpenLids == false) + { + JobEndReason = JOB_LIDS_OPEN; + usnprintf(AlarmReasonStr, 100, "Dryer lid is open!!!"); + PrepareReady(Module_Thread,ModuleFail); + return ERROR; + } } //start thread control for all motors @@ -913,61 +1172,81 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ - MotorControlConfig[Motor_i].m_params.MAX = 1; - MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; - MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; - MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; - MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; - MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; - MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; - MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; - MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; - MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[Motor_i].m_calculatedError = 0; - MotorControlConfig[Motor_i].m_integral = 0; - MotorControlConfig[Motor_i].m_isEnabled = true; - MotorControlConfig[Motor_i].m_isReady = true; - MotorControlConfig[Motor_i].m_mesuredParam = 0; - MotorControlConfig[Motor_i].m_preError = 0; - MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR; + if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR; + if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR; +#endif + MotorControlConfig[Motor_i].m_params.MAX = 1; + MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; + MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; + MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; + MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; + MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; + MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; + MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; + MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; + MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses + MotorControlConfig[Motor_i].m_calculatedError = 0; + MotorControlConfig[Motor_i].m_integral = 0; + MotorControlConfig[Motor_i].m_isEnabled = true; + MotorControlConfig[Motor_i].m_isReady = true; + MotorControlConfig[Motor_i].m_mesuredParam = 0; + 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; + if (MotorTiming[Motor_i]) + { + MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; + ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); + } ////////////////////////////////////////////////// - for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) - { - //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - MotorSamples[Motor_i][i] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0); //reset the samples value for control beginning - // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) - // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; - //MotorSpeedSamples[Motor_i][i] = 0; - } - MotorSamplePointer[Motor_i] = 0; + for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) + { + //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + MotorSamples[Motor_i][i] = 0; + if (Motor_i == FEEDER_MOTOR) + MotorSamples[Motor_i][i] = -500; + // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) + // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; + //MotorSpeedSamples[Motor_i][i] = 0; + } + MotorSamplePointer[Motor_i] = 0; ///////////////////////////////////////////////////// - MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); + MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); - if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled +#ifndef BTSR_NO_TFU + if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + { + ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + if (SpeedControlId != 0xFF) { - ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); - if (SpeedControlId != 0xFF) - { - RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); - SpeedControlId = 0xFF; - } - //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); - LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); + SpeedControlId = 0xFF; } - if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); + SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + } +#endif + if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + { + ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + if (PoolerSpeedControlId != 0xFF) { - ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); - if (PoolerSpeedControlId != 0xFF) - { - if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); - PoolerSpeedControlId = 0xFF; - } - //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); - PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); + PoolerSpeedControlId = 0xFF; } + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); + PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + } +#ifndef BTSR_NO_TFU if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) @@ -995,6 +1274,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } +#endif if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { ReportWithPackageFilter(ThreadFilter,"Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); @@ -1009,25 +1289,87 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } -// if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled -// AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); - if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - continue; +#ifdef FOUR_WINDERS + if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled + { + ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0); + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); + CurrentControlledSpeed[Motor_i] = 0; + ControlIdtoMotorId[Motor_i] = 0xFF; + } +#ifndef TEST_PID_THREAD + ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); +#endif + } +#endif } - HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! #ifdef TEST_PID_THREAD testDancersControl(); #endif - PrepareReady(Module_Thread,ModuleDone); + if (PrepareWaitCount == 0) + PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions - return OK; + return status; } +uint32_t UpdatePidDuringRun(HardwarePidControl *request) +{ + int Motor_i = MAX_THREAD_MOTORS_NUM,i; + double temp_dt; + + for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) + { + if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype) + { + Motor_i = i; + break; + } + } + if (Motor_i == MAX_THREAD_MOTORS_NUM) + return ERROR; + + if (request->derivativetime == true) + { + MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0); + } + if (request->proportionalgain == true) + { + MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0); + } + if (request->integraltime == true) + { + MotorControlConfig[Motor_i].m_params.Ki = request->integraltime; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0); + } + if (request->epsilon == true) + { + MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0); + } + if (request->has_controloutputtype == true) + { + MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype; + temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; + MotorTiming[Motor_i] = (int)temp_dt; + if (MotorTiming[Motor_i]) + { + MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; + } + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0); + } +////////////////////////////////////////////////// + return OK; +} void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; - for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++) + for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) @@ -1100,7 +1442,11 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; - PrepareState = false; + if (PrepareWaitCount == 0) + PrepareState = false; + PullerSpeedIndex = 0; + FeederSpeedIndex = 0; + #ifndef TEST_PID_THREAD // set the new speed in the dryer motor to the speed of the new segment if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) @@ -1111,7 +1457,7 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps } - ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); + ReportWithPackageFilter(ThreadFilter,"Dryer ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0); @@ -1235,7 +1581,7 @@ char Endstr[150]; int Motor_i; ThreadControlActive = false; uint32_t status = OK; - usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); @@ -1258,7 +1604,7 @@ char Endstr[150]; PoolerSpeedControlId = 0xFF; } - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { @@ -1271,18 +1617,29 @@ char Endstr[150]; } Task_sleep(100); - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); + } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); + Release_Right_TFU_Tension(); IDS_StopLubrication(); return OK; } - - +void ThreadCheckArcHeadCovers(void) +{ + if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { + if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { + ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); + } + if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { + ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); + } + } +} //******************************************************************************************************************** void ThreadStartPrinting(void) |