diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
6 files changed, 644 insertions, 2188 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index aedb49a62..56fb79c23 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -17,11 +17,20 @@ #include "../control/pidalgo.h" #include "thread_ex.h" -#include "drivers/SSI_Comm/SSI_Comm.h" #define NORMAL_COEF_DIVIDER 100 typedef struct { + uint32_t startoffsetpulses; + uint32_t spoolbackingrate; + uint32_t segmentoffsetpulses;// the spool winding initial length in mm + uint32_t milimetersperrotation; + uint32_t SpoolBottomBackingRate;// the angle of the bottom of the spool + double NumberOfRotationPerPassage; // how many rotations per spool passage + double diameter; +}InternalWinderConfigStruc; +typedef struct +{ bool m_isEnabled; int32_t m_SetParam; float m_mesuredParam; @@ -33,27 +42,12 @@ 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 NUM_OF_ROTENC +#define MAX_SYSTEM_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) 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; @@ -82,18 +76,14 @@ 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); 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 ffbb504db..c5cdb00d6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -16,9 +16,8 @@ #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" #include "PMR/Hardware/HardwareMotor.pb-c.h" @@ -31,104 +30,64 @@ #include "drivers/SSI_Comm/Dancer/Dancer.h" #include "drivers/motors/motor.h" #include "drivers/Flash_ram/MCU_E2Prom.h" -#include "Drivers/I2C_Communication/I2C.h" -#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" #include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h" -#include "PMR/ThreadLoading/StartThreadLoadingRequest.pb-c.h" -#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 "PMR/ThreadLoading/AttemptThreadJoggingResponse.pb-c.h" //#include <stdint.h> //#include <stdbool.h> - static char* LoadStagesStr[15] = { - /*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 { - 00 THREAD_LOAD_INIT, - 01 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - 02 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - 03 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY - 04 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID - 05 THREAD_LOAD_LIFT_DANCERS, - 06 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND - 07 THREAD_LOAD_CLOSE_ROCKERS, - 08 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD - 09 THREAD_LOAD_CLOSE_LIDS, - 10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION - 11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 - //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER + 0 THREAD_LOAD_INIT, + 1 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, + 2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION, //USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY + 3 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID + 4 THREAD_LOAD_LIFT_DANCERS, + 5 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE + 6 THREAD_LOAD_INITIAL_TENSION, //CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND + 7 THREAD_LOAD_CLOSE_ROCKERS, + 8 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD + 9 THREAD_LOAD_CLOSE_LIDS, + 10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT, //JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION + 11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 + //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER 12 THREAD_LOAD_RESUME_HEATING, - 13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING + 13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING 14 THREAD_LOAD_END }THREAD_LOAD_STAGES_ENUM;*/ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; ProcessParameters ProcessParametersClear,ProcessParametersRecover; - uint32_t LoadStatus = OK; - char LoadErrorMsg[100]; int32_t keepmicrostep; - int32_t keepkvalrun,keepkvalhold,kval_upperValue; + int32_t keepkvalrun,keepkvalhold; uint8_t CallbackCounter = 0; uint8_t TimeoutsCounter = 0; - bool SecondTry = false,TryAgain = false; - int MessageState = 0; //0 - none, 1 = start, 2 = continue - uint32_t UnloadingStart = 0; uint8_t NumberOfDrierLoaderCycles = 0; uint32_t status = OK; - 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; - double LengthCalculationMultiplier; - float numberOfSteps = 0; - float numberOfCycles = 0; - double DrierPrevLocation = 0; - int DrierCenterLocation = 0; - FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT}; - int Screw_Dir = false; - bool InitCalled = false; - uint32_t LoadArmRounds; - - int CurrentlyLoaded = 0; + uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF; 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); + typedef struct + { + uint32_t LoadArmRounds; + uint32_t LoadArmBackLash; + }LoadArmInfoStruc; + LoadArmInfoStruc LoadArmInfo; + //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; - 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); + //RUN MOTOR TO SWITCH WITH TIMEOUT + //RUN MOTOR TO BREAK SENSOR + //RUN MOTOR TO DANCER LOCATION (POOLER GOES TO THE OTHER DIRECTION) + + //RUN MOTOR A FULL CYCLE + //RUN A MOTOR NUMBER OF STEPS + //RUN CONTROL FOR A SINGLE DANCER bool ThreadLoadingActive(void) { if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END)) @@ -136,46 +95,49 @@ else return false; } - uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage) + uint32_t Thread_Load_Init(void) { //void* buffer = NULL; //uint32_t Bytes = 0; //FRESULT Fresult = FR_OK; - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Init"); - load.color = BLINK; - LoadStatus = OK; - //LoadArmRounds = 0xFF; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + + LoadArmInfo.LoadArmBackLash = 5; + //LoadArmInfo.LoadArmRounds = 0xFF; //EEPROM_STORAGE_DRYER_CYCLES - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); + if (LoadArmInfo.LoadArmRounds <= 2) + LoadArmInfo.LoadArmRounds = 20; + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); + Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,0); - Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,LoadArmRounds,RpWarning,Read_Dryer_ENC_Position(),0); + /*Fresult = f_mkdir(SW_INFO_DIR); + if ((Fresult == FR_OK)||(Fresult == FR_EXIST)) + { + Fresult = FileRead(LoadArmPath, &Bytes, &buffer); + if (Fresult == FR_OK) + { + if (Bytes>=sizeof(LoadArmInfo)) + memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo)); + free (buffer); + } + }*/ + + Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpMessage,LoadArmInfo.LoadArmRounds,0); StopInitSequence(); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); - if (Stage == THREAD_LOAD_INIT) - { - LoadStages++; - ThreadLoadStateMachine(LoadStages); - } - else - { - Report("Thread_Load_Init another state ",__FILE__,__LINE__,Stage,RpMessage,LoadArmRounds,0); - } - InitCalled = true; + LoadStages++; + ThreadLoadStateMachine(LoadStages); return OK; } 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"); + REPORT_MSG(LoadStages, "Thread Load State Machine step "); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); ProcessParametersClear.dryerzone1temp = 0; ProcessParametersClear.dryerzone2temp = 0; @@ -187,120 +149,122 @@ ProcessParametersClear.headzone4temp = 0; ProcessParametersClear.headzone5temp = 0; ProcessParametersClear.headzone6temp = 0; - ProcessParametersClear.headzone7temp = 0; - ProcessParametersClear.headzone8temp = 0; - ProcessParametersClear.headzone9temp = 0; - ProcessParametersClear.headzone10temp = 0; - ProcessParametersClear.headzone11temp = 0; - ProcessParametersClear.headzone12temp = 0; - ProcessParametersClear.rblowertemp = 0; - ProcessParametersClear.lblowertemp = 0; - ProcessParametersClear.rblowerflow = 0; - ProcessParametersClear.lblowerflow = 0; ProcessParametersClear.dyeingspeed = 40; ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; - ProcessParametersClear.pullertension = ProcessParametersKeep.pullertension; - ProcessParametersClear.windertension = ProcessParametersKeep.windertension; - - if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) + if (HandleProcessParameters(&ProcessParametersClear)!= 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); + LOG_ERROR (LoadStages, "Thread_Load_Init failed"); } else { - if(Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD) - { - WHS_Set_SetPoint_Q_value(headairflow); - } + Turn_the_Blower_On();//Turn on with the Default_Voltage + if (BlowerCfg.heatingvoltage) + Control_Voltage_To_Blower(BlowerCfg.heatingvoltage); else - { - Turn_the_Blower_On();//Turn on with the Default_Voltage - if (BlowerCfg.heatingvoltage) - Control_Voltage_To_Blower(BlowerCfg.heatingvoltage); - else - Control_Voltage_To_Blower(BlowerCfg.voltage-500); - } + Control_Voltage_To_Blower(BlowerCfg.voltage-500); Control_Dryer_Fan(STOP,75);//use START or STOP, 0 - 100% LoadStages++; ThreadLoadStateMachine(LoadStages); } return OK; } - uint32_t Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback(uint32_t deviceID, uint32_t BusyFlag) + uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag) { + // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - 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); + /*NumberOfDrierLoaderCycles--; + if (NumberOfDrierLoaderCycles) + { + Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); + } + else*/ + { + Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + //storeLoadArmParameters(); + MotorMove(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,LoadArmInfo.LoadArmBackLash ); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + LoadStages++; + ThreadLoadStateMachine(LoadStages); + } return OK; } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { - uint32_t temp = Read_Dryer_ENC_Position(); - int angle; - 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) + if(PullerControlId != 0xFF) { - CallbackCounter--; + MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); + RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); + PullerControlId = 0xFF; } - if (abs(angle)<10 ) // OK + if(WinderControlId != 0xFF) { - 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; - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); + MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); + RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); + WinderControlId = 0xFF; + } + /*Task_sleep(5) + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ - //storeLoadArmParameters(); - LoadStages++; + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); +//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_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); + ThreadLoadStateMachine(LoadStages); + return OK; + } + uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void) + { + uint32_t numberOfSteps = 0; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + if (LoadArmInfo.LoadArmRounds != 0xFF) + { + //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 + numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0); + NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds; + status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), + numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); } - else + else //number of circles is not known - compare to position of the motor to verify location at the stopper { - 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); + Report("Thread_Load_Set_Load_Arm_To_Start_Position unknown cycles",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); - LoadStatus = ERROR; - ThreadLoadingReport(); - TryAgain = true; - //ThreadLoadButton(LoadStages); } -//move to exact location? -// Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly + return OK; + } + uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag) + { + CallbackCounter--; + Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + //storeLoadArmParameters(); + LoadStages++; + ThreadLoadStateMachine(LoadStages); + return OK; } + uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { - Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0); + Report("Thread_Load_HomingCallback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); // REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id"); if ((LoadStages == THREAD_LOAD_CLOSE_ROCKERS)||(LoadStages == THREAD_LOAD_LIFT_ROCKERS)) { if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)) { - if(LoadStages == THREAD_LOAD_CLOSE_ROCKERS) - { - MotorMove(MotorId,MotorsCfg[MotorId].directionthreadwize,400); - Report("Move rockers further down",__FILE__,__LINE__,MotorId,RpMessage,400,0); - Task_sleep(200); - } MotorSetMicroStep(MotorId, keepmicrostep); MotorSetKvalRun(MotorId, keepkvalrun); } @@ -318,20 +282,12 @@ { if (TimeoutsCounter) { + LOG_ERROR(LoadStages,"Load sequence timeout"); TimeoutsCounter = 0; - load.color = fastBILNK; - 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 { LoadStages++; - LoadStatus = OK; if (LoadStages == THREAD_LOAD_CLOSE_DANCERS) { MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold); @@ -340,12 +296,6 @@ { ThreadLoadStateMachine(LoadStages); } - else - { - load.color = colorON; - Pannel_Leds(THREAD_LOAD, MODE_ON); - ThreadLoadingReport(); - } } } } @@ -353,62 +303,38 @@ LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); return OK; } + /*uint32_t Thread_Load_Center_Head_Rockers(void) + { + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + CallbackCounter++; + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 80, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000); + CallbackCounter++; + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 80, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000); + return OK; + }*/ uint32_t Thread_Load_Open_Covers(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Open_Covers"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); //Open Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //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); - 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 - } + MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000); return OK; } uint32_t Thread_Load_Lift_Dancers(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Dancers"); - /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); + keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);*/ + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); - status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,1); - if (Is_PP_Machine() == true) //PP machine - new LTFU - { - CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,25000); - 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); - } + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 600, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); return OK; } @@ -417,24 +343,15 @@ { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; - /*if (keepkvalrun>=25) - keepkvalrun = 25;*/ - - Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); - + if (keepkvalrun>=25) + keepkvalrun = 25; Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); - Report("Thread_Load_Lift_right dancer",__FILE__,__LINE__,0,RpMessage,0,0); - keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; - MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40); - CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); - - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Rockers"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70); 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); @@ -445,12 +362,10 @@ } uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Initial_Tension"); - load.color = BLINK; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); 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++; @@ -463,108 +378,57 @@ { 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, kval_upperValue); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70); + MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70); - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Rockers"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); 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"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); 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++; + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 600, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); - 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); - status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,¤t); - - ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); -/* if (current!=windertension) - { - CallbackCounter++; - 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) { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); //Close Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, CallbackCounter++; + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_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); - 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); - } - } + MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000); return OK; } uint32_t Thread_Load_Resume_Heating(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); ActivateHeadMagnet(); - HeadCard_Actuators_Relocate(); - if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) + if (HandleProcessParameters(&ProcessParametersRecover)!= OK) { - usnprintf(LoadErrorMsg, 100, "Stage %s error",LoadStagesStr[LoadStages]); - Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); + LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed"); } else { @@ -574,50 +438,15 @@ } return OK; } - uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition) - { - if (Condition == true) - { - CallbackCounter--; - Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - LoadStages++; - ThreadLoadStateMachine(LoadStages); - } - else - { - CallbackCounter--; - Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - if (SecondTry == true) - { - LoadStages++; - ThreadLoadStateMachine(LoadStages); - LoadStatus = OK; - } - else - { - TimeoutsCounter = 0; - load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!"); - Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - LoadStatus = ERROR; - ThreadLoadingReport(); - TryAgain = true; - //ThreadLoadButton(LoadStages); - } - } - - return OK; - } uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void) //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Feeder_To_Middle_Point"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 400, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,6000); + MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,2000); return OK; } + uint32_t Thread_Load_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); @@ -628,20 +457,11 @@ RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); LoadingControlId = 0xFF; } - if (SpeedTControlId != 0xFF) - { - RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); - SpeedTControlId = 0xFF; - } - Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0); - if (CallbackCounter) { CallbackCounter--; if (CallbackCounter == 0) { - LoadStatus = OK; - LoadStages++; if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call { @@ -656,103 +476,43 @@ } uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { - numberOfCycles++; - - Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,numberOfCycles,0); - - if ((fabs (TotalLoadedLen -DrierPrevLocation)>50 )&&(ReadValue == NOTBUSY)) - { - DrierPrevLocation = TotalLoadedLen ; - //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); - if (LoadStages != THREAD_LOAD_DRYER_LOADING) - { - 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, - numberOfSteps, Thread_Load_Dryer_Loading_Callback, 10000); - } - else - { - 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 + Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,__LINE__,MotorId,RpMessage,CallbackCounter,0); + /*if(LoadingControlId != 0xFF) { - TryAgain = true; - SecondTry = false; - 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, "$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); - if(LoadingControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); - LoadingControlId = 0xFF; - } - if (SpeedTControlId != 0xFF) - { - RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); - SpeedTControlId = 0xFF; - } - Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0); - - ThreadLoadingReport(); - //ThreadLoadButton(LoadStages); - } + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); + LoadingControlId = 0xFF; + }*/ + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000); return OK; } uint32_t Thread_Load_Dryer_Loading(void) { - REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Dryer_Loading"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); if (dryerbufferlength) - LoadArmRounds = (int)dryerbufferlength; - if (LoadArmRounds <= 2) - LoadArmRounds = 30; + LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + //if (LoadArmInfo.LoadArmRounds <= 2) + // LoadArmInfo.LoadArmRounds = 20; + float numberOfSteps = 0; //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 SetOriginMotorSpeed(19); // OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; // CurrentControlledSpeed[FEEDER_MOTOR] = 1000; - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&CurrentlyLoaded); - Report("Thread_Load_Dryer_Loading cycles",__FILE__,LoadArmRounds,CurrentlyLoaded,RpMessage,LoadArmRounds-CurrentlyLoaded,0); - if (CurrentlyLoaded) - { - LoadArmRounds -= CurrentlyLoaded; - } - if (LoadArmRounds == CurrentlyLoaded) - { - Report("Thread_Load_Dryer_Loading no need for cycles",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - LoadStages++; - ThreadLoadStateMachine(LoadStages); - return OK; - } - ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; - ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds); - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation); - numberOfCycles = 0; - 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; -// numberOfSteps -= 100; + numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + numberOfSteps -= 100; //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0); MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1; MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1; MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime; MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain; - MotorControlConfig[FEEDER_MOTOR].m_params.Ki = 0;//MotorsControl[FEEDER_MOTOR].integraltime; + MotorControlConfig[FEEDER_MOTOR].m_params.Ki = 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; @@ -767,54 +527,42 @@ 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/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); - //6 seconds per round + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); 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); - if (SpeedTControlId != 0xFF) - { - RemoveControlCallback(SpeedTControlId,ThreadLoadLengthCBFunction); - SpeedTControlId = 0xFF; - } - //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); - LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep); - TotalLoadedLen = 0; - DrierPrevLocation = 200; //initial safe value - SpeedTControlId = AddControlCallback("Thread length",ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0); - Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 0; 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-800, Thread_Load_Dryer_Loading_Callback, 10000); + numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); + + //Keep Notation How Many Rotations In The Dryer + //LoadArmInfo.LoadArmBackLash = 0; + //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER); 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"); + REPORT_MSG(LoadStages, "Thread Load State Machine step"); SafeRemoveControlCallback(LoadingControlId, Thread_Load_Jog_ThreadStop ); LoadingControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; - ProcessParametersKeep.feedertension = keepfeedertension; ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_Jog_Thread(void) //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; + REPORT_MSG(LoadStages, "Thread Load State Machine step"); ThreadJoggingFunc(20); - LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); + LoadingControlId = AddControlCallback(NULL,Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); return OK; } uint32_t Thread_Load_End(void) { REPORT_MSG(LoadStages,"Loading Ended"); - ThreadLoadingReport(); if(LoadingControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); @@ -833,18 +581,7 @@ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } - if (SpeedTControlId != 0xFF) - { - 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); @@ -852,194 +589,99 @@ 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_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); - LoadStatus = ERROR; - } - + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); 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) - { - if ((LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)||(LoadStatus == ERROR)) - { - return OK; - } - Screw_Dir = 1-Screw_Dir; - // movement of the screw while unloading the thread from the dryer to the spool - 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(void) { - bool direction; - numberOfCycles++; - uint32_t temp = Read_Dryer_ENC_Position(); - 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 ((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, 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,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("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); + REPORT_MSG(LoadStages, "Thread UnLoad State Machine step"); + LoadArmInfo.LoadArmRounds = 0; + //uint32_t numberOfSteps = 0; + //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 + SetOriginMotorSpeed(19); +// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; +// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; + //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + 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); - TimeoutsCounter = 0; - CallbackCounter = 0; - if(WinderControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); - RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); - WinderControlId = 0xFF; - } + PullerControlId = AddControlCallback(NULL,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); - MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + 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); - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); - load.color = fastBILNK; - usnprintf(LoadErrorMsg, 100, "&Stage %s - Dryer load arm timeout",LoadStagesStr[LoadStages]); - Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,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 Load State Machine step Dryer Unloading"); - //LoadArmRounds = 0; - //uint32_t numberOfSteps = 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("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + LoadArmInfo.LoadArmBackLash = 5; - 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) - { - 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); - 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/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-800, Thread_Load_Dryer_UnLoading_Callback, 10000); - - Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize; - - // 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,20000); + 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,LoadArmInfo.LoadArmBackLash,1000); //Keep Notation How Many Rotations In The Dryer - //LoadArmRounds = (int)dryerbufferlength; - * - */ - load.color = BLINK; + LoadArmInfo.LoadArmBackLash = 5; + LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER); return OK; } - double percentage; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue) { REPORT_MSG(ReadValue,"ThreadLoadStateMachine"); - //ThreadLoaded(); - percentage = ReadValue*7; - ThreadLoadingReport(); - if (LoadStages!=ReadValue) LoadStages = ReadValue; switch (ReadValue) { case THREAD_LOAD_INIT: //LoadStages++; - MessageState = 1; - - Thread_Load_Init(ReadValue); + Thread_Load_Init(); break; case THREAD_LOAD_REDUCE_HEAT: //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, //LoadStages++; - MessageState = 1; - if (InitCalled == false) - { - REPORT_MSG(ReadValue,"Thread_Load_Init called from 1"); - Thread_Load_Init(ReadValue); - } - else - { - REPORT_MSG(ReadValue,"Thread_Load_Init called from 0"); - } Thread_Load_Reduce_Heat(); - InitCalled = false; break; case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY //LoadStages++; @@ -1094,135 +736,37 @@ case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING //LoadStages++; Thread_Load_Jog_Thread(); - ThreadLoadingReport(); 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()||(MachineReadyForHeating == false)||(FPGABurningActive==true)) + if (JobIsActive()) { Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); return ERROR; } - if (CallbackCounter) - { - Report("ThreadLoadButton called when the loading is active",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - return ERROR; - } 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 (LoadStages > ReadValue) { - if ((SecondTry == false)&&(TryAgain == true)) - { - LoadStatus = OK; - SecondTry = true; - load.color = BLINK; - //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) - // ThreadLoadingRestartReport(); - Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); - ThreadLoadStateMachine(LoadStages); - } - else if (SecondTry == true) - { - SecondTry = false; - Report("Calling State machine 2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); - load.color = BLINK; - LoadStatus = OK; - if (LoadStages == THREAD_LOAD_LIFT_DANCERS) - { - LoadStages++; - } - else - ThreadLoadStateMachine(LoadStages+1); - } - else //((SecondTry == false)&&(TryAgain == false))??????? - { - Report("State ERROR with wrong parameters",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); - SecondTry = false; - } + ThreadLoadStateMachine(LoadStages+1); } else { - Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0); - //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) - // ThreadLoadingRestartReport(); - ThreadLoadStateMachine(LoadStages); - SecondTry = false; + ThreadLoadStateMachine(ReadValue); } return OK; } +uint32_t LoadCounter = 0; -uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) -{ - uint32_t positionDiff = 0,prevprev; - double length = 0.0; - char lenstr[150]; - static int counter = 0; - int index = MAX_THREAD_MOTORS_NUM; - if (IfIndex>>8 != IfTypeThread) - { - LOG_ERROR (IfIndex, "Wrong Interface type"); - return 0xFFFFFFFF; - } - index = IfIndex&0xFF; -// if (CurrentRequestedLength == 0.0) -// return OK; - if (index != HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) - { - LOG_ERROR (IfIndex, "Wrong Motor"); - return 0xFFFFFFFF; - } - currentPosition = MotorGetPosition(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); - if (currentPosition != 0) - { - if (Tinitialpos == 0xFFFF) - { - previousPosition = currentPosition; - Tinitialpos = 0; - } - prevprev = previousPosition; - if (Extended_Motor_Param[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING] == true) //powerstep driver reverses the direction - positionDiff = Control_Delta_Position_Pass(previousPosition,currentPosition); - else - positionDiff = Control_Delta_Position_Pass(currentPosition,previousPosition); - //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; - previousPosition = currentPosition; - - // total length = (position diff / full cycle) * pulley perimeter - //(positionDiff/pulseperround)*((2*PI*motor_Radius) - - //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; - length = (double)(positionDiff)*LengthCalculationMultiplier; - - if (counter++ % 50 == 0) - { - usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x TotalLoadedLen %d",(int)length*100,(int)positionDiff,previousPosition,prevprev, (int)TotalLoadedLen); -// SendJobProgress(0.0,0,false, lenstr); - ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalLoadedLen,RpWarning,(int)TotalLoadedLen,0); -// length = 0; - - } - - } - TotalLoadedLen += length; - - return OK; -} uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle @@ -1242,11 +786,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; @@ -1257,11 +801,6 @@ 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]++; @@ -1274,7 +813,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; - MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, + MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { @@ -1286,37 +825,6 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } - /* - - if (index == POOLER_MOTOR) - { - if ((TranslatedReadValue)<(-1200)) - { - if(PullerControlId != 0xFF) - { - Report("Puller stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0); - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - } - } - - } - if (index == WINDER_MOTOR) - { - if ((TranslatedReadValue)<(-1200)) - { - if(WinderControlId != 0xFF) - { - Report("Winder stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0); - MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); - RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); - WinderControlId = 0xFF; - } - - } - - }*/ /* LoadCounter++; if ((LoadCounter % 5001) == 0) { @@ -1324,33 +832,6 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ return OK; } -bool ThreadLoaded(void) -{ - /* 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__,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__,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__,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*/ - return false; -} void ThreadLoadRequest(MessageContainer* requestContainer) { //#ifdef DEBUG_TEST_FUNCTIONS @@ -1364,16 +845,10 @@ 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; - if (request->hardwarepidcontrol1!=NULL) - { - UpdatePidDuringRun(request->hardwarepidcontrol1); - } - else - { - LoadStages = LoadStage; - ThreadLoadStateMachine(LoadStage); - } + LoadStages = LoadStage; + ThreadLoadStateMachine(LoadStage); /*if (status) { @@ -1416,256 +891,3 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer) SendChars(container_buffer, container_size); stub_heating_test_poll_request__free_unpacked(request,NULL); } -char ThreadLoadingToken[36+1] = {0}; -char DefaultErrSrt[] = "OK"; -uint32_t ThreadLoadingReport(void) -{ - MessageContainer responseContainer; - bool last = false; - StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; - - if (ThreadLoadingToken[0] == 0) - return OK; - - if (MessageState == 0) - { - response.has_state = true; - response.state = THREAD_LOADING_STATE__None; - response.errorreason = DefaultErrSrt; - - } - else if (MessageState == 1) - { - response.has_state = true; - if (LoadStatus == OK) - { - response.state = THREAD_LOADING_STATE__Preparing; - if (LoadStages == THREAD_LOAD_INITIAL_TENSION) - { - response.state = THREAD_LOADING_STATE__ReadyForLoading; - response.errorreason = DefaultErrSrt; - } - } - else - { - response.state = THREAD_LOADING_STATE__PreparationError; - response.errorreason = LoadErrorMsg; - } - } - else if (MessageState == 2) - { - response.has_state = true; - if (LoadStatus == OK) - { - response.state = THREAD_LOADING_STATE__Finalizing; - if (LoadStages > THREAD_LOAD_JOG_THREAD) - { - response.state = THREAD_LOADING_STATE__Completed; - response.errorreason = DefaultErrSrt; - //last = true; - } - } - else - { - response.state = THREAD_LOADING_STATE__FinalizationError; - response.errorreason = LoadErrorMsg; - } - } - 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; - 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; - -} -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; - if (LoadStages == THREAD_LOAD_INIT) - 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; - if (JobIsActive()||(MachineReadyForHeating == false)||(FPGABurningActive==true)) - { - responseContainer.has_error = true; - responseContainer.error = ERROR_CODE__GENERAL_ERROR; - responseContainer.errormessage = "The machine is not ready for loading"; - } - 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; - -} - -uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) -{ - ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); - ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT; - MessageContainer responseContainer; - MessageState = 2; - ThreadLoadingReport(); - if (ThreadLoadingRestartFlag == false) - { - 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); - } - - responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_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); - if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call - { - ThreadLoadStateMachine(LoadStages); - } - return OK; - -} -uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer) -{ - StopThreadLoadingRequest *request = stop_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); - StopThreadLoadingResponse Cresponse = STOP_THREAD_LOADING_RESPONSE__INIT; - MessageContainer responseContainer; - MessageState = 2; - Thread_Load_End(); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); - Report("StopThreadLoadingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)LoadStages,0); - - responseContainer = createContainer(MESSAGE_TYPE__StopThreadLoadingResponse, requestContainer->token, true, &Cresponse, &stop_thread_loading_response__pack, &stop_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); - return OK; - -} -uint8_t attemptJogCounter = 0; -#define ATTEMPT_JOG_TIMEOUT 18 -char AttemptJoggingToken[36+1] = {0}; - -uint32_t AttemptThreadJoggingCallback(uint32_t index, uint32_t ReadValue) -{ - AttemptThreadJoggingResponse response = ATTEMPT_THREAD_JOGGING_RESPONSE__INIT; - MessageContainer responseContainer; - - attemptJogCounter++; - Report("AttemptThreadJoggingCallback",__FILE__,attemptJogCounter,JobEndReason,RpWarning,(int)JobIsActive(),0); - if (attemptJogCounter<ATTEMPT_JOG_TIMEOUT) - { - if (JobIsActive() == true)//jog still preparing/running - return OK; - } - if (JobEndReason == JOB_OK) - ThreadAbortJoggingFunc(); - else - { - responseContainer.has_error = true; - responseContainer.error = getJobError_to_ErrorCode(JobEndReason); - } - SafeRemoveControlCallback(LoadingControlId, AttemptThreadJoggingCallback ); - LoadingControlId = 0xFF; - - if (AttemptJoggingToken[0]) - { - responseContainer = createContainer(MESSAGE_TYPE__AttemptThreadJoggingResponse, AttemptJoggingToken, true, &response, &attempt_thread_jogging_response__pack, &attempt_thread_jogging_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); - } - return OK; - -} -uint32_t AttemptThreadJoggingFunc(MessageContainer* requestContainer) -{ - attemptJogCounter = 0; - if(requestContainer) - ustrncpy (AttemptJoggingToken, requestContainer->token,36); - Report("AttemptThreadJoggingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)ATTEMPT_JOG_TIMEOUT,0); - LoadingControlId = AddControlCallback("Load jog",AttemptThreadJoggingCallback, eOneSecond,TemplateDataReadCBFunction,0,0,0); - ThreadJoggingFunc(30); - return OK; -} - diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 03bc9d24d..a93387e0a 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" @@ -50,25 +50,22 @@ bool WinderMotorSpeedRollOver = false; int StartWinderCalculation = 0; #define DEFAULT_SCREW_SPEED 1200 double ScrewSpeed = DEFAULT_SCREW_SPEED; -double RetreatTime = 0; 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; static uint32_t WindingConeLocation; -static uint32_t WinderBackToBaseTime = 1500; +static uint32_t WinderBackToBaseTime = 800; -float NumberOfRotationPerPassage = 0.0; InternalWinderConfigStruc InternalWinderCfg = {0}; -//#define READ_SCREW_ENCODER +#define READ_SCREW_ENCODER #ifdef READ_SCREW_ENCODER uint32_t ScrewLocationLimitSwitch = 0,ScrewLocationStart = 0; uint32_t ScrewLocationRun[3]; -uint32_t ScrewLocationPrev[3]; float WinderRunAverage = 0.0,WinderRunSum = 0.0; int WinderRunSamples = 0; #endif @@ -90,48 +87,29 @@ 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 >= 200)) - 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 > 5.1) - InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage; - if (NumberOfRotationPerPassage > 1) - { - InternalWinderCfg.NumberOfRotationPerPassage = NumberOfRotationPerPassage; - Report("Rotation per passage set from stub",__FILE__,__LINE__,(int)(request->rotationsperpassage*1000),RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); - } + InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*2;//request->rotationsperpassage; 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.startoffsetpulses,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); + (int)InternalWinderCfg.spoolbackingrate,(int)InternalWinderCfg.SpoolBottomBackingRate); + Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.diameter,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); return status; } @@ -141,26 +119,6 @@ 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_Check_Cone(void) -{ - if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) - { - REPORT_MSG(LIMIT, "No cone in winder"); - 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 JOB_THREAD_BREAK; - } - } - } - return JOB_OK; -} uint32_t Winder_Prepare(void *JobDetails) { JobTicket* JobTicket = JobDetails; @@ -182,17 +140,9 @@ uint32_t Winder_Prepare(void *JobDetails) /*if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); - 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; - } - } + PrepareReady(Module_Winder,ModuleFail); + AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true); + return ERROR; }*/ #ifdef READ_SCREW_ENCODER @@ -222,51 +172,14 @@ uint32_t Winder_Prepare(void *JobDetails) { //REPORT_MSG(LIMIT, "Winder_Prepare at limit"); Winder_PrepareStage2(0,0); - Screw_wait_counter=1; } else { //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 - if (FPGA_Read_limit_Switches(GPI_LS_SPARE2_2)==LIMIT) - { - Winder_PrepareStage2(0,0); - Screw_wait_counter++; - } - else - { - Winder_ScrewHoming = true; - Screw_wait_counter++; - status = MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, Winder_PrepareStage2,2000); - } - if (FPGA_Read_limit_Switches(GPI_LS_SPARE1_2)==LIMIT) - { - Winder_PrepareStage2(0,0); - Screw_wait_counter++; - } - else - { - Winder_ScrewHoming = true; - Screw_wait_counter++; - status = MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, Winder_PrepareStage2,2000); - } - if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP)==LIMIT) - { - Winder_PrepareStage2(0,0); - Screw_wait_counter++; - } - else - { - Winder_ScrewHoming = true; - Screw_wait_counter++; - status = MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, Winder_PrepareStage2,2000); - } -#endif return status; } /* @@ -280,7 +193,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(); @@ -289,56 +202,26 @@ 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(deviceID,"Screw failed to reach the limit switch!!"); - //returned with a timeout - Winder_ScrewAtOffsetCallback(0,0); - PrepareReady(Module_Winder, ModuleFail); - } - else //short jog, etc - { - AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF); //on a short jog, this callback will be called without need - } + LOG_ERROR(ReadValue,"Screw failed to reach the limit switch!!"); + //returned with a timeout + Winder_ScrewAtOffsetCallback(0,0); } 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 MotorId, uint32_t BusyFlag) +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) { //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); - if (Screw_wait_counter) - Screw_wait_counter--; - 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*/ + MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands Task_sleep(5); #ifdef READ_SCREW_ENCODER Reset_Screw_Encoder(); @@ -349,12 +232,7 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t MotorId, uint32_t BusyFlag) REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location"); #endif - 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*/ + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home ScrewCurrentDirection = false; #ifdef READ_SCREW_ENCODER @@ -363,9 +241,8 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t MotorId, uint32_t BusyFlag) ScrewSpeed = 0; ScrewControlId = 0xFF; ScrewNumberOfSteps = 0; - ReportWithPackageFilter(ThreadFilter,"Winder_ScrewAtOffsetCallback",__FILE__,millisecondCounter,Screw_wait_counter,RpWarning,MotorId, 0); - if (Screw_wait_counter == 0) - PrepareReady(Module_Winder, ModuleDone); + REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback"); + PrepareReady(Module_Winder, ModuleDone); return OK; } @@ -401,23 +278,23 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__ double WinderReferenceSpeed=0; double TotalWinderSpeed=0; bool Add100 = false; +double Rotations = 6.0; 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; + double screw_horizontal_speed = 0; + double RotationsPerSecond; + //double Averagewinderspeed = 0; + +// { +// TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter]; +// WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR]; +// TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter]; if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION) { if (WinderMotorSpeedRollOver == false) @@ -427,21 +304,24 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) } // WinderMotorSpeedCounter=0; } +// } + if (flipflop == 0) + { + MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + } + else if (flipflop == 1) + { + speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + } + flipflop ++; + if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter) return OK; //deley TODO - //flipflop = 0; + flipflop = 0; 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; @@ -462,16 +342,16 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0); } } - if (abs(ScrewLocationPrev[1]-ScrewLocationRun[1])>21) - { - Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[1],RpWarning,ScrewLocationRun[1], 0); - } - ScrewLocationPrev[1] = ScrewLocationRun[1]; - if (abs(ScrewLocationPrev[0]-ScrewLocationRun[0])>21) - { - Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[0],RpWarning,ScrewLocationRun[0], 0); - } - ScrewLocationPrev[0] = ScrewLocationRun[0]; + //else + // Report("winder run error value" ,__FILE__,WinderRun,ScrewLocationRun[0],RpWarning,ScrewLocationRun[1], 0); + + //Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0); + //REPORT_MSG(abs(ScrewLocationRun[1] - ScrewLocationRun[0]), "Screw Run NumberOfSteps"); + //usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d intent %d rot %d",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,Rotations*10); + //usnprintf(ScrewStr, 150, "Winder Encoder:id, diff, intended, winderspeed, rotation, speed, time, mot speed {, %d, %d, %d, %d, %d, %d, %d, %d, }",CalculationDirectionChangeCounter, + // abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,(int)(WinderReferenceSpeed),(int)(Rotations*10),(int)ScrewSpeed,(int)ScrewRunningTime,(int)speedf); + //usnprintf(ScrewStr, 100, "Winder Encoder: 0 %d 1 %d diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],ScrewLocationRun[1] - ScrewLocationRun[0]); + //Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0); #endif if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out { @@ -497,6 +377,24 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) // Report("Bottom Backing ",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0); } } + /* if (WinderMotorSpeedRollOver) + { + if (WinderCalculation%60000 == 0)//100 minutes + { + Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION; + //Report("WinderSpeedUpdated",__FILE__,__LINE__,(int)TotalWinderSpeed,RpWarning,(int)Averagewinderspeed,0); + WinderReferenceSpeed = Averagewinderspeed; + } + WinderCalculation++; + }*/ + //WinderReferenceSpeed = 1000; + //ScrewNumberOfSteps = 1000; + screw_horizontal_speed = ScrewNumberOfSteps / Rotations;//InternalWinderCfg.NumberOfRotationPerPassage; + // if (Rotations > 6.6)//7.0) + // Rotations = 6.0; + RotationsPerSecond = WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround; + tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond; + //ROM_IntMasterDisable(); tempScrewSpeed = ScrewSpeed; CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed; @@ -559,6 +457,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId) InternalWinderCfg.SpoolBottomBackingRate = 2000; //no cone shape for samples InternalWinderCfg.NumberOfRotationPerPassage = 100; // a very slow movement of the screw JobSegment* Segment = SegmentDetails; + InternalWinderCfg.NumberOfRotationPerPassage = 100; // a very slow movement of the screw ScrewSpeed = InternalWinderCfg.segmentoffsetpulses/(((Segment->length *100)/dyeingspeed)/3); ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; @@ -571,6 +470,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId) InternalWinderCfg.SpoolBottomBackingRate = 2000; //no cone shape for samples InternalWinderCfg.NumberOfRotationPerPassage = 100; // a very slow movement of the screw JobSegment* Segment = SegmentDetails; + InternalWinderCfg.NumberOfRotationPerPassage = 100; // a very slow movement of the screw ScrewSpeed = InternalWinderCfg.segmentoffsetpulses/(((Segment->length *100)/dyeingspeed)/20); ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; } @@ -636,7 +536,6 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId) temp *= ScrewNumberOfSteps; temp /= ScrewSpeed; ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed; - RetreatTime = ScrewNumberOfSteps*1000/ScrewSpeed; REPORT_MSG((int)ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps"); REPORT_MSG((int)ScrewRunningTime,"Winder pre segment - ScrewRunningTime"); //MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection); @@ -662,7 +561,7 @@ uint32_t WinderDistanceToSpoolEnded(uint32_t deviceID, uint32_t ReadValue) } uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag) { - Report("ScrewDTSCallback called", __FILE__,__LINE__,msec_millisecondCounter, RpMessage, ScrewSpeed, 0); + REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback called"); SafeRemoveControlCallback(ScrewDTSControlId, ScrewDTSCallback); if (ScrewControlId != 0xFF) @@ -675,26 +574,17 @@ uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag) //move the cart to the edge so the spool can be easily replaced //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,RetreatTime*2); -#ifdef FOUR_WINDERS - MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, NULL,RetreatTime*2); - MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, NULL,RetreatTime*2); - MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, NULL,RetreatTime*2); -#endif + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, WinderDistanceToSpoolEnded,2000); return OK; } uint32_t WinderDistanceToSpoolState(void ) { double DTS_Time = (dryerbufferCentimeters/dyeingspeed)*1000;//distance to spool time in milliseconds - double WinderBackTimeBySpeed = WinderBackToBaseTime; - WinderBackTimeBySpeed *= 50; - WinderBackTimeBySpeed /= dyeingspeed; - Report("WinderDistanceToSpoolState start", __FILE__,DTS_Time,RetreatTime, RpMessage, (int)WinderBackTimeBySpeed, 0); - //REPORT_MSG ((int)DTS_Time,"WinderDistanceToSpoolState"); + REPORT_MSG ((int)DTS_Time,"WinderDistanceToSpoolState"); - ScrewDTSControlId = AddControlCallback("screw DTS",ScrewDTSCallback, DTS_Time-RetreatTime,TemplateDataReadCBFunction,0,0,0); - Report("ScrewDTSCallback start", __FILE__,DTS_Time,msec_millisecondCounter, RpMessage, (int)WinderBackTimeBySpeed, 0); + ScrewDTSControlId = AddControlCallback("screw DTS",ScrewDTSCallback, DTS_Time-WinderBackToBaseTime,TemplateDataReadCBFunction,0,0,0); + REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback start"); return OK; } @@ -712,16 +602,10 @@ uint32_t Winder_End(void) ScrewsStopControlTimer(); //move the cart to the edge so the spool can be easily replaced //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 + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); 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 @@ -736,7 +620,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 @@ -780,19 +664,7 @@ 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; @@ -806,7 +678,7 @@ void ScrewTimerInterrupt(int ARG0) TimerDisable(Screw_timerBase, TIMER_A); } ROM_IntMasterEnable(); - //Report("ScrewsStartControlTimer time direction,speed ", __FILE__,msec_millisecondCounter,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0); + //Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0); //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); //Rotations+=0.03; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 05cacd20e..29876549e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -9,42 +9,20 @@ #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer #define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer #define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer -#define WINDER_2_DANCER HARDWARE_DANCER_TYPE__FourthDancer -#define WINDER_3_DANCER HARDWARE_DANCER_TYPE__ThirdDancer -#define WINDER_4_DANCER HARDWARE_DANCER_TYPE__RightDancer - -#define NUM_OF_DANCERS NUM_OF_ROTENC - -typedef struct -{ - uint32_t startoffsetpulses; - uint32_t spoolbackingrate; - uint32_t segmentoffsetpulses;// the spool winding initial length in mm - uint32_t milimetersperrotation; - uint32_t SpoolBottomBackingRate;// the angle of the bottom of the spool - double NumberOfRotationPerPassage; // how many rotations per spool passage - double diameter; -}InternalWinderConfigStruc; - +#define NUM_OF_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1) +//} DANCER_ENUM; typedef enum threadMotorsEnum { FEEDER_MOTOR, 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); @@ -58,18 +36,13 @@ 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); - -uint32_t StoreDancerConfigMessage(int DancerId); +uint32_t StoreDancerConfigMessage(void); uint32_t LoadDancerConfigMessage(void); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); -extern float NumberOfRotationPerPassage; //debug for rotation per passage trials -extern InternalWinderConfigStruc InternalWinderCfg; - uint32_t Winder_Init(void); -uint32_t Winder_Check_Cone(void); uint32_t Winder_Prepare(void *JobDetails); uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId); uint32_t WinderDistanceToSpoolState(void ); @@ -79,10 +52,10 @@ typedef enum { /*00*/ THREAD_LOAD_INIT, /*01*/ THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - /*02*/ THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - /*03*/ THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY - /*04*/ THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID - /*05*/ THREAD_LOAD_LIFT_DANCERS, + /*02*/ THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY + /*03*/ THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID + /*04*/ THREAD_LOAD_LIFT_DANCERS, + /*05*/ THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE /*06*/ THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND /*07*/ THREAD_LOAD_CLOSE_ROCKERS, /*08*/ THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD @@ -101,14 +74,6 @@ bool ThreadLoadingActive(void); void ThreadLoadPollRequest(MessageContainer* requestContainer); void ThreadLoadRequest(MessageContainer* requestContainer); -uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer); -uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer); -uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer); -uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer); -uint32_t AttemptThreadJoggingFunc(MessageContainer* requestContainer); - -void ThreadCheckArcHeadCovers(void); uint32_t Thread_Load_End(void); -uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 588b1b391..416b53494 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -23,7 +23,6 @@ #include "drivers/Flash_ram/FlashProgram.h" #include "drivers/Flash_ram/MCU_E2Prom.h" -#include "modules/ids/ids_ex.h" #include "thread.h" MotorDriverConfigStruc MotorsCfg[NUM_OF_MOTORS]={0}; @@ -104,14 +103,10 @@ 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,79 +140,56 @@ 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); - if (Motor_i == WINDER_MOTOR) - { - temp = temp*3/2; - } + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*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"; -uint32_t StoreDancerConfigMessage(int DancerId) +uint32_t StoreDancerConfigMessage() { uint32_t status = OK; - // FRESULT Fresult = FR_OK; - //HardwareConfiguration DancerConfig; - HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; -#ifdef FOUR_WINDERS - uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2,EEPROM_STORAGE_DANCER_3,EEPROM_STORAGE_DANCER_4}; -#else - uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2}; -#endif + /* FRESULT Fresult = FR_OK; + // HardwareConfiguration DancerConfig; + // HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; + // uint8_t* response_buffer; // size_t response_size = 0; - int Dancer_i = DancerId; - - //hardware_configuration__init(&DancerConfig); - //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS); - hardware_dancer__init(&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; - 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); + // int Dancer_i; + 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]; + 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,0); + DancerConfig.n_dancers++; + DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; + } + */ + 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); +/* + response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig)); + if (response_buffer) + { + response_size = hardware_configuration__pack(&DancerConfig, response_buffer); + } + Fresult = FileWrite(response_buffer,response_size,DancerConfigPath,BIOS_WAIT_FOREVER); + EraseFlashSection(DANCERS_MAP_IN_FLASH,1024); + for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++) + { + ReadAppAndProgram(DANCERS_MAP_IN_FLASH, sizeof(Dancers), Dancers); + } - status |= MCU_E2PromProgram(DancerAddress[Dancer_i],DancersCfg[Dancer_i].zeropoint); - Report("~~~~~~Store eeprom dancer",__FILE__,__LINE__,Dancer_i,RpWarning,(int)DancersCfg[Dancer_i].zeropoint,0); + my_free(response_buffer); +*/ return status; } uint32_t LoadDancerConfigMessage() @@ -227,18 +199,11 @@ 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 87ad612ba..41d98e892 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -15,7 +15,6 @@ #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include "PMR/Printing/JobSegment.pb-c.h" #include "PMR/Printing/JobTicket.pb-c.h" -#include "PMR/Printing/ThreadParameters.pb-c.h" #include <PMR/Diagnostics/EventType.pb-c.h> #include <utils/ustdlib.h> @@ -29,15 +28,11 @@ #include "drivers/Heater/Heater.h" #include "drivers/Motors/Motor.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" -#include "drivers/FPGA/FPGA_SPI_Comm.h" #include "modules/heaters/heaters.h" #include "modules/General/process.h" #include "modules/ids/ids_ex.h" #include "Modules/AlarmHandling/AlarmHandling.h" #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 @@ -46,15 +41,9 @@ 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; @@ -66,10 +55,6 @@ 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; @@ -85,12 +70,10 @@ double TempTotalProcessedLength = 0.0; bool InitialProcess = false; bool PrepareState = false; - // job parameters bool EnableLubrication = false; bool EnableIntersegment = false; double IntersegmentLength = 0; -bool Thread_Rockers_Bypass = false; int CurrentSegmentId = 0; @@ -135,7 +118,7 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea if (Current_Read < Previous_Read) { Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1; - ReportWithPackageFilter(ThreadFilter,"Length rollover",__FILE__,Time_Pass,(int)Current_Read,RpWarning,(int)Previous_Read,0); + Report("Length rollover",__FILE__,__LINE__,(int)Current_Read,RpWarning,(int)Previous_Read,0); } else Time_Pass = Current_Read - Previous_Read; @@ -152,22 +135,15 @@ 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) { - ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); + REPORT_MSG(length,"ThreadUpdateProcessLength"); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; } -char Lenstr[190]; +char Lenstr[150]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -182,7 +158,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); + LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -190,7 +166,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) // return OK; if (index != FEEDER_MOTOR) { - ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); + LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); @@ -202,10 +178,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) initialpos = 0; } prevprev = PreviousPosition; - if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction - positionDiff = Control_Delta_Position_Pass(PreviousPosition,CurrentPosition); - else - positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); + positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PreviousPosition = CurrentPosition; @@ -219,22 +192,12 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); length = 0; } } - /*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 @@ -261,7 +224,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -271,6 +234,7 @@ 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; @@ -284,7 +248,7 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); + LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -292,7 +256,7 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) // return OK; if (index != POOLER_MOTOR) { - ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); + LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); @@ -304,10 +268,7 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) Poolerinitialpos = 0; } prevprev = PoolerPreviousPosition; - if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction - positionDiff = Control_Delta_Position_Pass(PoolerPreviousPosition,PoolerCurrentPosition); - else - positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition); + positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PoolerPreviousPosition = PoolerCurrentPosition; @@ -321,26 +282,13 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); 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_PULLER_TFU - if (CurrentControlledSpeed[WINDER_MOTOR]>100) - length = dyeingspeed/10; -#endif + PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION @@ -352,7 +300,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { if (PrepareState == true) { - TemperatureListString(Lenstr); + //later - add temperatures + TemperatureListString(Lenstr); + SendJobProgress(0.0,0,false, Lenstr); } else @@ -363,9 +313,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { - usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); + Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -390,7 +340,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { - ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); + LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -422,7 +372,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) int index; if (IfIndex>>8 != IfTypeThread) { - ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); + LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -434,72 +384,7 @@ 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; @@ -536,7 +421,7 @@ void testDancersControl() len = usnprintf(time, 150, "params: speed 50, divider %d p %d * %d i %d * %d Dt*1000 %d Norm Coef %d initial speed %d",NORMAL_COEF_DIVIDER,(int)MotorsControl[FEEDER_MOTOR].proportionalgain,(int)MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue, (int)MotorsControl[FEEDER_MOTOR].integraltime,(int)MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp,(int)(MotorsControl[FEEDER_MOTOR].controloutputtype*1000), (int)(NormalizedErrorCoEfficient[FEEDER_MOTOR]*1000000000),OriginalMotorSpd_2PPS[FEEDER_MOTOR]); - ReportWithPackageFilter(ThreadFilter,time,__FILE__,__LINE__,111,RpError,111,0); + Report(time,__FILE__,__LINE__,111,RpError,111,0); Task_sleep(100); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10); @@ -552,14 +437,18 @@ void testDancersControl() ThreadControlActive = false; } #endif +bool dancerinvalid = false; int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; +char TMessage[150]; //char time[150]; -bool FirstCalcInJob = true; +uint16_t BreakSensorCounter = 0; +uint16_t BreakSensorLatchCounter = 0; 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 DancerId; @@ -576,90 +465,38 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); + LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; - if (index == WINDER_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) // return OK; DancerId = ThreadMotorIdToDancerId[index]; -/* if (ReadValue < 10) + if (ReadValue < 10) { MotorFailedSample[index]++; - ReportWithPackageFilter(ThreadFilter,"Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; - }*/ + } if (ReadValue == 0x3FFF) { - if (Read_Dryer_Status(DancerId) != OK) + MotorFailedSample[index]++; + if (dancerinvalid == false) { - ReportWithPackageFilter(ThreadFilter,"Dancer value invalid.",__FILE__,ReadValue,(int)DancerId,RpError,(int)Read_Dryer_Status(DancerId),0); - MotorFailedSample[index]++; - return OK; + dancerinvalid = true; + LOG_ERROR(index, "Dancer value invalid."); } + return OK; } KeepReadValue = ReadValue; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (abs(TranslatedReadValue) > 0x2000) - { - TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment - } - if ((index == POOLER_MOTOR)||((index == FEEDER_MOTOR)&&(DancersCfg[DancerId].assemblydirectionright == true))) + if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - } -#ifdef BTSR_ROTATED_WINDER_TFU - if (index == WINDER_MOTOR) - TranslatedReadValue = (-1*TranslatedReadValue); -#endif -#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 == WINDER_MOTOR) - { - //pooler dancer is right sided: data is opposite JobCounter++; } //TranslatedReadValue = 0;//test @@ -675,23 +512,73 @@ 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" ); + LOG_ERROR(index, "ReadBreakSensor Error"); + return OK; + } //passed limit + }//ReadBreakSensor()==ERROR + else //reset counter - we are looking for consequent calls + { + if (BreakSensorCounter) + { + LOG_ERROR(BreakSensorCounter, "ReadBreakSensor Spike"); + } + 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" ); + LOG_ERROR(index, "thread speed too low Error"); + return OK; + } + } + + } + } + } //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { keepdata = false; usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); - ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); + Report(TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); //JobAbortedByUser = true; 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) @@ -706,28 +593,27 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true); break; }*/ - ReportWithPackageFilter(ThreadFilter,"Dancer Failure",__FILE__,DancerId,(int)avreageSampleValue,RpError,(int)JobCounter,0); + LOG_ERROR (DancerId, "Dancer Failure"); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; - if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit - { - NormalizedError = (-1*NormalizedError); - } - MotorControlConfig[index].m_mesuredParam = NormalizedError; DancerError[DancerId] = NormalizedError; - MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, + MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); - /*else + if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit + { + MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); + } + else { //KeepNormalizedError = NormalizedError; - }*/ + } /*if ((JobCounter % 100) == 0) { //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit //{ - // ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); + // Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); //} /`*if (JobCounter >= 3000) { @@ -738,58 +624,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) for (i=0;i<MAX_CONTROL_SAMPLES;i++) avreageMotorSampleValue += MotorSpeedSamples[index][i]; avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES; - //ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); + //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue; }*`/ }*/ 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 (0)//(JobCounter % 1000 == 0) -#ifdef FOUR_WINDERS - if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) - { - if (index >= WINDER_MOTOR) - { - // 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, (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 if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit #endif { - 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); - } - else - { - if (calculated_speed<0) - ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); - } - + CurrentControlledSpeed[index] = calculated_speed; + MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit { - ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); + Report("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) { @@ -801,11 +655,11 @@ 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;*/ - usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", + 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), (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); + Report(TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); //Task_sleep(100); //if (controlIndex++>=MAX_THREAD_CONTROL_LOG) // controlIndex = 0; @@ -845,332 +699,52 @@ double ThreadGetMotorCalculatedError(int DancerId) } //******************************************************************************************************************** -uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) -{ - 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.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.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.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) +uint32_t ThreadInitialTestStub(HardwareMotor * request) { - //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_FEEDER_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_FEEDER_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_FEEDER_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_FEEDER_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); - } + //MotorsConfigMessage(request); + ThreadPrepareState(request); + ThreadPreSegmentState(request,0); 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; - switch (DancerId) - { - case HARDWARE_DANCER_TYPE__LeftDancer: - address = EEPROM_WINDER_TENSION_POSITION; - HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER1; - break; - case HARDWARE_DANCER_TYPE__MiddleDancer: - if (Is_PP_Machine() == false) //LP machine - old LTFU - return OK; - address = EEPROM_PULLER_TENSION_POSITION; - HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2; - break; - case HARDWARE_DANCER_TYPE__RightDancer: - 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; - if (abs(current - request)<100) - return status; - else - { - if (current < request) //go down - { - direction = MotorsCfg[HW_Motor_Id].directionthreadwize; - movement = request - current; - } - else - { - direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize; - movement = current - request; - } - 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); - } - 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; -} //******************************************************************************************************************** -bool SkipOpenLids = false; -uint32_t ThreadPrepareState(void *JobDetails) + uint32_t ThreadPrepareState(void *JobDetails) { - int Motor_i,i, HW_Motor_Id, Pid_Id; + int Motor_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; - InitialProcess = true; - initialpos = 0xFFFF; - Poolerinitialpos = 0xFFFF; PrepareState = true; AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false); - AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,false); - -// status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); -// double feedertension = 0; -// double pullertension = 0; -// double windertension = 0; EnableLubrication = JobTicket->enablelubrication; EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; - PrepareWaitCount = 0; - status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); -#ifndef BTSR_NO_PULLER_TFU - status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); -#endif -#ifndef BTSR_NO_FEEDER_TFU - status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); - 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) - { - ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; - ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING; - } - else - { - ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; - ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; - } 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,RpWarning,LIMIT,0); - //JobEndReason = JOB_LIDS_OPEN; - // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); - //PrepareReady(Module_Thread,ModuleFail); - //return ERROR; + ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + JobEndReason = JOB_LIDS_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,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; - } + ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpFatalError,LIMIT,0); + JobEndReason = JOB_LIDS_OPEN; + PrepareReady(Module_Thread,ModuleFail); + return ERROR; } //start thread control for all motors @@ -1178,89 +752,57 @@ uint32_t ThreadPrepareState(void *JobDetails) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ -#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 + 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] = 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); -#ifndef BTSR_NO_FEEDER_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) + 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 { - RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); - SpeedControlId = 0xFF; + Report("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); } - //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 -//#ifndef BTSR_NO_PULLER_TFU - 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) + 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 { - if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); - PoolerSpeedControlId = 0xFF; + Report("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) + LOG_ERROR(Motor_i,"Remove Control Failed"); + 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); } - //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); - } -//#endif -#ifndef BTSR_NO_FEEDER_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) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); + LOG_ERROR(Motor_i,"Remove Control Failed"); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } @@ -1269,14 +811,12 @@ uint32_t ThreadPrepareState(void *JobDetails) //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); #endif } -#endif -#ifndef BTSR_NO_PULLER_TFU if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { 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); + LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } @@ -1284,29 +824,13 @@ uint32_t ThreadPrepareState(void *JobDetails) 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); - 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 - } -#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); + Report("Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].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); + LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } @@ -1314,72 +838,26 @@ uint32_t ThreadPrepareState(void *JobDetails) ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } -#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 == 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 + continue; } - #ifdef TEST_PID_THREAD testDancersControl(); #endif - if (PrepareWaitCount == 0) - PrepareReady(Module_Thread,ModuleDone); + PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions - 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->has_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->has_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->has_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->has_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); - } -////////////////////////////////////////////////// + InitialProcess = true; + initialpos = 0xFFFF; + Poolerinitialpos = 0xFFFF; return OK; } + void SetOriginMotorSpeed(float process_speed) { - int Motor_i, HW_Motor_Id; - for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++) + int i,Motor_i, HW_Motor_Id; + for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) @@ -1391,11 +869,10 @@ void SetOriginMotorSpeed(float process_speed) OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; InitialDryerSpeed = 0.0; CurrentControlledSpeed[Motor_i] = (int) motor_speed; - if (process_speed > 1) - ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0); + //Report("Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0); - // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) - // MotorSpeedSamples[Motor_i][i] = motor_speed; + for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) + MotorSpeedSamples[Motor_i][i] = motor_speed; } } void ThreadPreSegmentEnded(void) @@ -1404,36 +881,25 @@ void ThreadPreSegmentEnded(void) REPORT_MSG (0,"First ThreadPreSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } -int DrierDivider = 20; +#define DRYER_RAMPUP 1 +#ifdef DRYER_RAMPUP uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag) { - InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider); + InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/10); if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR]) { InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]; SafeRemoveControlCallback(ControlIdtoMotorId[DRYER_MOTOR], ThreadDryerRampUp ); ControlIdtoMotorId[DRYER_MOTOR] = 0xFF; - //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp end",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); } - if (InitialDryerSpeed == 0) - { - //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp Stopped",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); - return OK; - } - MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); - //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); + + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,InitialDryerSpeed ); + Report("ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); return OK; } -bool Set_Thread_Rockers_Bypass (int value) -{ - if (value == 0) - Thread_Rockers_Bypass = false; - else - Thread_Rockers_Bypass = true; - return Thread_Rockers_Bypass; -} +#endif //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) @@ -1444,50 +910,39 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) float process_speed = dyeingspeed; if (dyeingspeed == 0) { - ReportWithPackageFilter(ThreadFilter,"job speed zero.",__FILE__,__LINE__,(int)dyeingspeed,RpError,(int)SegmentId,0); + LOG_ERROR (dyeingspeed," job speed zero"); return ERROR; } - ReportWithPackageFilter(ThreadFilter,"ThreadPreSegmentState",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)SegmentId,0); + REPORT_MSG (dyeingspeed," ThreadPreSegmentState"); if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; - if (PrepareWaitCount == 0) - PrepareState = false; - PullerSpeedIndex = 0; - FeederSpeedIndex = 0; - + PrepareState = false; #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) - { - DrierDivider = dyeingspeed/3; //ramp up drier in 5 cm/sec steps - } - else - { - DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps - } - ReportWithPackageFilter(ThreadFilter,"Dryer ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); - InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; - MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); +#ifndef DRYER_RAMPUP + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); +#else + InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/10; + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,InitialDryerSpeed ); ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0); #endif +#endif #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); #endif - if (Thread_Rockers_Bypass == false) - { - if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0) - { - MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1); - } - if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].maxfrequency > 0) - { - MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1); - } - } + + if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0) + { + MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1); + } + if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].maxfrequency > 0) + { + MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1); + } if (EnableLubrication == true) { IDS_StartLubrication(); @@ -1590,10 +1045,10 @@ char Endstr[150]; { int Motor_i; ThreadControlActive = false; - uint32_t status = OK; - usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + + usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); - ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); ThreadUpdateProcessLength (0.0,(void *)NULL); //TotalProcessedLength = 0.0; @@ -1604,52 +1059,39 @@ char Endstr[150]; if (SpeedControlId != 0xFF) { if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)ThreadLengthCBFunction,RpError,(int)SpeedControlId,0); + LOG_ERROR(SpeedControlId,"RemoveControl Failed"); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)PoolerThreadLengthCBFunction,RpError,(int)PoolerSpeedControlId,0); + LOG_ERROR(PoolerSpeedControlId,"RemoveControl Failed"); PoolerSpeedControlId = 0xFF; } - for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { - status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); - if(status == OK) + if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else - ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0); + LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed"); } - } - Task_sleep(100); - for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) + Task_sleep(10); + for ( Motor_i = 0;Motor_i <= WINDER_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) |