/* * ThreadLoad.c * * Created on: Jan 16, 2019 * Author: shlomo */ #include #include "include.h" #include "thread_ex.h" #include "Common/SWUpdate/FileSystem.h" #include "StateMachines/Printing/PrintingSTM.h" #include "Modules/thread/thread.h" #include "Modules/Control/control.h" #include "Modules/General/process.h" #include "Modules/control/pidalgo.h" #include "Modules/heaters/heaters_ex.h" #include "Modules/General/buttons.h" #include "StateMachines/Initialization/InitSequence.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" #include "PMR/Hardware/HardwareMotorType.pb-c.h" #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include "PMR/Printing/ProcessParameters.pb-c.h" #include "drivers/Flash_Memory/fatfs/ff.h" #include "drivers/I2C_Communication/DAC/Blower.h" #include "drivers/SSI_Comm/Dancer/Dancer.h" #include "drivers/motors/motor.h" #include "drivers/Flash_ram/MCU_E2Prom.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 //#include /* typedef enum { 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 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; uint8_t CallbackCounter = 0; uint8_t TimeoutsCounter = 0; uint32_t UnloadingStart = 0; uint8_t NumberOfDrierLoaderCycles = 0; uint32_t status = OK; uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF; uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF; uint32_t LoadCounter = 0; double TotalLoadedLen = 0.0; double LengthCalculationMultiplier; 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 Thread_Load_Dryer_UnLoading(void); uint32_t ThreadLoadingReport(void); typedef struct { uint32_t LoadArmRounds; uint32_t LoadArmBackLash; }LoadArmInfoStruc; LoadArmInfoStruc LoadArmInfo; //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; //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)) return true; else return false; } 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"); LoadStatus = OK; LoadArmInfo.LoadArmBackLash = MCU_E2PromDryerBacklashRead(); REPORT_MSG(LoadArmInfo.LoadArmBackLash, "Thread Load MCU_E2PromDryerBacklashRead"); if (LoadArmInfo.LoadArmBackLash > 10) LoadArmInfo.LoadArmBackLash = 5; //LoadArmInfo.LoadArmRounds = 0xFF; //EEPROM_STORAGE_DRYER_CYCLES MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); if (LoadArmInfo.LoadArmRounds <= 2) LoadArmInfo.LoadArmRounds = 20; Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,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(); LoadStages++; ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_Reduce_Heat(void) { //Heaters Off, Dryer Blower Off, Blower Low, REPORT_MSG(LoadStages, "Thread Load State Machine step "); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); ProcessParametersClear.dryerzone1temp = 0; ProcessParametersClear.dryerzone2temp = 0; ProcessParametersClear.dryerzone3temp = 0; ProcessParametersClear.mixertemp = 0; ProcessParametersClear.headzone1temp = 0; ProcessParametersClear.headzone2temp = 0; ProcessParametersClear.headzone3temp = 0; ProcessParametersClear.headzone4temp = 0; ProcessParametersClear.headzone5temp = 0; ProcessParametersClear.headzone6temp = 0; ProcessParametersClear.dyeingspeed = 40; ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Init failed"); strcpy(LoadErrorMsg,"Thread_Load_Init failed"); LoadStatus = ERROR; } 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_Dryer_Fan(STOP,75);//use START or STOP, 0 - 100% LoadStages++; ThreadLoadStateMachine(LoadStages); } return OK; } /*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); /`*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(); 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) { if(PullerControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); PullerControlId = 0xFF; } if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); /*Task_sleep(5) MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ 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 time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); LoadStages++; SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home ThreadLoadStateMachine(LoadStages); return OK; } /*uint32_t Thread_Load_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 //number of circles is not known - compare to position of the motor to verify location at the stopper { Report("Thread_Load_Set_Load_Arm_To_Start_Position",__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/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); } //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_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); } } if (CallbackCounter) { CallbackCounter--; load.color = BLINK; if (ReadValue != LIMIT) { //returned with a timeout TimeoutsCounter++; } if (CallbackCounter == 0) { if (TimeoutsCounter) { LOG_ERROR(LoadStages,"Load sequence timeout"); TimeoutsCounter = 0; load.color = fastBILNK; usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId); LoadStatus = ERROR; ThreadLoadingReport(); } else { LoadStages++; load.color = colorON; if (LoadStages == THREAD_LOAD_CLOSE_DANCERS) { MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold); } if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call { ThreadLoadStateMachine(LoadStages); } else { load.color = fastBILNK; ThreadLoadingReport(); } } } } else 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"); //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); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); return OK; } uint32_t Thread_Load_Lift_Dancers(void) { 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); 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); return OK; } uint32_t Thread_Load_Lift_Rockers(void) //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; if (keepkvalrun>=25) keepkvalrun = 25; Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); 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, 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); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,35000); return OK; } 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"); if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); //return ERROR; } CallbackCounter++; MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500, Thread_Load_HomingCallback,10000); return OK; } uint32_t Thread_Load_Close_Rockers(void) { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; if (keepkvalrun>=25) keepkvalrun = 25; Report("Thread_Load_Close_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70); 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); return OK; } uint32_t Thread_Load_Close_Dancers(void) //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread { 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++; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500); // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, (2000*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000); return OK; } uint32_t Thread_Load_Close_Lids(void) { 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); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); return OK; } uint32_t Thread_Load_Resume_Heating(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step"); ActivateHeadMagnet(); if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed"); } else { Control_Dryer_Fan(START,75);//use START or STOP, 0 - 100% LoadStages++; ThreadLoadStateMachine(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"); CallbackCounter++; 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); 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); if (CallbackCounter) { CallbackCounter--; if (CallbackCounter == 0) { LoadStages++; if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call { ThreadLoadStateMachine(LoadStages); } } } else LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); return OK; } float numberOfSteps = 0; float numberOfCycles = 0; uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { numberOfCycles++; //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; } if (numberOfCycles ReadValue) { ThreadLoadStateMachine(LoadStages+1); } else { ThreadLoadStateMachine(ReadValue); } return OK; } 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; 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 int i; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0; int index=MAX_THREAD_MOTORS_NUM; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) { Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; } if (ReadValue == 0x3FFF) { return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i MotorControlConfig[index].m_ingnoreValue) { 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) { Report("MotorSpeedUpdated",__FILE__,index,abs(TranslatedReadValue),RpWarning,(int)CurrentControlledSpeed[index],0); }*/ return OK; } void ThreadLoadRequest(MessageContainer* requestContainer) { //#ifdef DEBUG_TEST_FUNCTIONS MessageContainer responseContainer; uint8_t* container_buffer; //uint32_t status = OK; StubHeatingTestRequest* request = stub_heating_test_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT; responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size); container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); int LoadStage = (int)request->dryerzone1temp; LoadStages = LoadStage; ThreadLoadStateMachine(LoadStage); /*if (status) { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); //USBCDCD_sendData(container_buffer, container_size,10); SendChars(container_buffer, container_size); stub_heating_test_request__free_unpacked(request,NULL); //#else // LOG_ERROR (-1, "Heating Control not on debug"); // return ERROR; //#endif } void ThreadLoadPollRequest(MessageContainer* requestContainer) { uint8_t* container_buffer; StubHeatingTestPollRequest* request = stub_heating_test_poll_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); StubHeatingTestPollResponse response = STUB_HEATING_TEST_POLL_RESPONSE__INIT; MessageContainer responseContainer; response.has_heater1percentage = true; response.heater1percentage = LoadStages; responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, requestContainer->token, false, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size); container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); /* { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars(container_buffer, container_size); stub_heating_test_poll_request__free_unpacked(request,NULL); } char ThreadLoadingToken[36+1] = {0}; int MessageState = 0; //0 - none, 1 = start, 2 = continue uint32_t ThreadLoadingReport(void) { MessageContainer responseContainer; bool last = false; StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT; if (ThreadLoadingToken[0] == 0) return OK; 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; last = true; } } else { response.state = THREAD_LOADING_STATE__PreparationError; response.errorreason = LoadErrorMsg; } responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); responseContainer.continuous = true; } else if (MessageState == 2) { Cresponse.has_state = true; if (LoadStatus == OK) { Cresponse.state = THREAD_LOADING_STATE__Finalizing; if (LoadStages >= THREAD_LOAD_JOG_THREAD) { Cresponse.state = THREAD_LOADING_STATE__Completed; last = true; } } else { Cresponse.state = THREAD_LOADING_STATE__FinalizationError; Cresponse.errorreason = LoadErrorMsg; } responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, ThreadLoadingToken, last, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size); responseContainer.continuous = true; } else return ERROR; //------------------------------------------------------------------------------------------- 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 StartThreadLoadingFunc(MessageContainer* requestContainer) { StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ustrncpy (ThreadLoadingToken, requestContainer->token,36); MessageState = 1; ThreadLoadingReport(); return OK; } uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) { ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ustrncpy (ThreadLoadingToken, requestContainer->token,36); MessageState = 2; ThreadLoadingReport(); return OK; }