diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 723 |
1 files changed, 644 insertions, 79 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 5a1bb9063..b9bbf3aab 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -18,6 +18,7 @@ #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" @@ -29,11 +30,18 @@ #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 "PMR/ThreadLoading/StopThreadLoadingRequest.pb-c.h" +#include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h" //#include <stdint.h> //#include <stdbool.h> @@ -60,17 +68,29 @@ 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; + 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; + 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); + bool ThreadLoaded(void); typedef struct { @@ -78,7 +98,7 @@ uint32_t LoadArmBackLash; }LoadArmInfoStruc; LoadArmInfoStruc LoadArmInfo; - char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; + //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; //RUN MOTOR TO SWITCH WITH TIMEOUT //RUN MOTOR TO BREAK SENSOR @@ -94,16 +114,31 @@ else return false; } - uint32_t Thread_Load_Init(void) + bool InitCalled = false; + uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage) { - void* buffer = NULL; - uint32_t Bytes = 0; - FRESULT Fresult = FR_OK; + //void* buffer = NULL; + //uint32_t Bytes = 0; + //FRESULT Fresult = FR_OK; REPORT_MSG(LoadStages, "Thread Load State Machine step"); + load.color = BLINK; + 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); - LoadArmInfo.LoadArmBackLash = 5; - LoadArmInfo.LoadArmRounds = 0xFF; - Fresult = f_mkdir(SW_INFO_DIR); + + Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); + + //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)) { @@ -114,14 +149,18 @@ memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo)); free (buffer); } - } + }*/ - Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0); + Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpMessage,LoadArmInfo.LoadArmRounds,0); StopInitSequence(); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); - LoadStages++; - ThreadLoadStateMachine(LoadStages); + if (Stage == THREAD_LOAD_INIT) + { + LoadStages++; + ThreadLoadStateMachine(LoadStages); + } + InitCalled = true; return OK; } @@ -143,9 +182,12 @@ ProcessParametersClear.headzone6temp = 0; ProcessParametersClear.dyeingspeed = 40; ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; - if (HandleProcessParameters(&ProcessParametersClear)!= OK) + if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Init failed"); + strcpy(LoadErrorMsg,"Thread_Load_Init failed"); + LoadStatus = ERROR; + TryAgain = true; } else { @@ -160,18 +202,18 @@ } return OK; } - uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_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); - /*NumberOfDrierLoaderCycles--; + /`*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*/ + else*`/ { Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); //storeLoadArmParameters(); @@ -180,7 +222,7 @@ ThreadLoadStateMachine(LoadStages); } return OK; - } + }*/ uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { @@ -196,6 +238,8 @@ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } + + MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); /*Task_sleep(5) MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ @@ -204,6 +248,8 @@ //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(); @@ -213,7 +259,7 @@ ThreadLoadStateMachine(LoadStages); return OK; } - uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void) + /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void) { uint32_t numberOfSteps = 0; REPORT_MSG(LoadStages, "Thread Load State Machine step"); @@ -230,27 +276,66 @@ { 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/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); + 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) + }*/ + uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition) { - CallbackCounter--; - Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - //storeLoadArmParameters(); - LoadStages++; - ThreadLoadStateMachine(LoadStages); + 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!"); + LoadStatus = ERROR; + ThreadLoadingReport(); + TryAgain = true; + } + } return OK; } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { - Report("Thread_Load_HomingCallback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + 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--; @@ -265,17 +350,16 @@ { 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(); + TryAgain = true; } else { LoadStages++; - if (LoadStages == THREAD_LOAD_CLOSE_ROCKERS) - { - MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepmicrostep); - MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepmicrostep); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepkvalrun); - MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepkvalrun); - } + LoadStatus = OK; if (LoadStages == THREAD_LOAD_CLOSE_DANCERS) { MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold); @@ -284,6 +368,11 @@ { ThreadLoadStateMachine(LoadStages); } + else + { + load.color = colorON; + ThreadLoadingReport(); + } } } } @@ -311,16 +400,18 @@ 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,4000); + MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + HeadCard_Actuators_Control(ACTOT, LOW,true); + 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; + /*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); @@ -331,6 +422,19 @@ { 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); + + 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"); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); @@ -347,10 +451,11 @@ 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"); + load.color = BLINK; if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); - return ERROR; + //return ERROR; } CallbackCounter++; MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500, Thread_Load_HomingCallback,10000); @@ -360,6 +465,17 @@ } 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); @@ -376,7 +492,10 @@ 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, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); + 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, (5000*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000); return OK; } @@ -390,7 +509,8 @@ 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,4000); + MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + HeadCard_Actuators_Control(ACTOT, LOW,false); return OK; } @@ -399,7 +519,7 @@ REPORT_MSG(LoadStages, "Thread Load State Machine step"); ActivateHeadMagnet(); - if (HandleProcessParameters(&ProcessParametersRecover)!= OK) + if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed"); } @@ -416,7 +536,7 @@ { 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); + 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,4000); return OK; } @@ -430,11 +550,20 @@ 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 { @@ -447,34 +576,87 @@ return OK; } + float numberOfSteps = 0; + float numberOfCycles = 0; + double DrierPrevLocation = 0; uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { - 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) + numberOfCycles++; + + Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0); + + if ((fabs (TotalLoadedLen -DrierPrevLocation)>50 )&&(ReadValue == NOTBUSY)) { - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); - LoadingControlId = 0xFF; - }*/ - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000); + 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; + } + if (numberOfCycles<LoadArmInfo.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,10000); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); + Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0); + } + } + else + { + LOG_ERROR(LoadStages,"Load sequence timeout - drier loading"); + TryAgain = true; + SecondTry = false; + Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); + TimeoutsCounter = 0; + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); + 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(); + } return OK; } uint32_t Thread_Load_Dryer_Loading(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step"); - LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + + if (dryerbufferlength) + LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; if (LoadArmInfo.LoadArmRounds <= 2) - LoadArmInfo.LoadArmRounds = 20; - float numberOfSteps = 0; + LoadArmInfo.LoadArmRounds = 30; //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; - numberOfSteps -= 100; + //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds); + ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; + ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; + + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation); + numberOfCycles = 0; + 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; @@ -498,16 +680,27 @@ 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(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); + 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, Thread_Load_Dryer_Loading_Callback, 100000); - + numberOfSteps-400, Thread_Load_Dryer_Loading_Callback, 10000); +//shlomo //Keep Notation How Many Rotations In The Dryer //LoadArmInfo.LoadArmBackLash = 0; - LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER); return OK; } @@ -526,12 +719,13 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); ThreadJoggingFunc(20); - LoadingControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); + LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,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); @@ -550,6 +744,11 @@ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } + if (SpeedTControlId != 0xFF) + { + RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); + SpeedTControlId = 0xFF; + } MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); @@ -559,21 +758,102 @@ MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); + if (LoadStages!=THREAD_LOAD_END) + { + usnprintf(LoadErrorMsg, 100, "Load sequence stopped by user on stage %d",LoadStages); + LoadStatus = ERROR; + } + LoadStages = THREAD_LOAD_INIT; + load.color = colorON; + return OK; + } + FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT}; + int Screw_Dir = false; + + uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag) + { + if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION) + { + return OK; + } + Screw_Dir = 1-Screw_Dir; + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000); + + return OK; + } + uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) + { + numberOfCycles++; + uint32_t temp = Read_Dryer_ENC_Position(); + Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0); + //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); + if ((abs (temp -DrierPrevLocation)>1000 )&&(ReadValue == NOTBUSY)) + { + DrierPrevLocation = temp; + if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION) + { + return OK; + } + if (numberOfCycles<LoadArmInfo.LoadArmRounds) + { + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, + numberOfSteps, Thread_Load_Dryer_UnLoading_Callback, 10000); + } + else + { + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,10000); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-numberOfCycles); + Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0); + } + } + else + { + LOG_ERROR(LoadStages,"Load sequence timeout - drier Unloading"); + Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0); + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-(numberOfCycles-2));//it takes two cycles to identify a stop of the arm + TimeoutsCounter = 0; + CallbackCounter = 0; + if(PullerControlId != 0xFF) + { + MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); + RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); + PullerControlId = 0xFF; + } + if(WinderControlId != 0xFF) + { + MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); + RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); + WinderControlId = 0xFF; + } + + MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + /*Task_sleep(5) + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ + + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); + LoadStatus = ERROR; + TryAgain = true; + ThreadLoadingReport(); + } return OK; } uint32_t Thread_Load_Dryer_UnLoading(void) { + uint32_t temp; REPORT_MSG(LoadStages, "Thread UnLoad State Machine step"); - LoadArmInfo.LoadArmRounds = 0; + //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); + SetOriginMotorSpeed(22); // 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.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; @@ -593,7 +873,7 @@ MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize); PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR); - Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0); + 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; @@ -614,7 +894,7 @@ MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize); - WinderControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR); + 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); //////////////////////// @@ -624,33 +904,60 @@ 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; + //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0); + //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); +//shlomo + UnloadingStart = msec_millisecondCounter; + 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; + numberOfCycles = 0; + + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); + DrierPrevLocation = temp; + Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmInfo.LoadArmRounds,0); + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); +// 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); + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, + numberOfSteps-400, Thread_Load_Dryer_UnLoading_Callback, 10000); - 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); + Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize; + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000); //Keep Notation How Many Rotations In The Dryer - //LoadArmInfo.LoadArmBackLash = 0; - LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; - FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER); + //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; + load.color = BLINK; return OK; } uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue) { REPORT_MSG(ReadValue,"ThreadLoadStateMachine"); + ThreadLoaded(); if (LoadStages!=ReadValue) LoadStages = ReadValue; switch (ReadValue) { case THREAD_LOAD_INIT: //LoadStages++; - Thread_Load_Init(); + MessageState = 1; + + Thread_Load_Init(ReadValue); 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++; @@ -705,6 +1012,7 @@ 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; @@ -723,19 +1031,90 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) 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); - if (LoadStages > ReadValue) + Report("ThreadLoadButton params",__FILE__,LoadStatus,TryAgain,RpMessage,SecondTry,0); + if (LoadStatus == ERROR) { - ThreadLoadStateMachine(LoadStages+1); + if ((SecondTry == false)&&(TryAgain == true)) + { + LoadStatus = OK; + SecondTry = true; + Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); + ThreadLoadStateMachine(LoadStages); + } + else + { + SecondTry = false; + Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); + ThreadLoadStateMachine(LoadStages+1); + } } else { - ThreadLoadStateMachine(ReadValue); + Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0); + ThreadLoadStateMachine(LoadStages); } 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; + 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 @@ -782,7 +1161,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; - MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, + MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((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 { @@ -794,6 +1173,37 @@ 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) { @@ -801,6 +1211,33 @@ 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__,FeederValue,ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0); + ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0); + WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint; + Report("Winder out",__FILE__,WinderValue,ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0); + ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0); + PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint; + //pooler dancer is right sided: data is opposite + PoolerValue = (-1*PoolerValue); + Report("Puller out",__FILE__,PoolerValue,ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0); + + 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 @@ -835,6 +1272,7 @@ void ThreadLoadRequest(MessageContainer* requestContainer) // return ERROR; //#endif } + void ThreadLoadPollRequest(MessageContainer* requestContainer) { uint8_t* container_buffer; @@ -860,3 +1298,130 @@ 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 return ERROR; + Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); + + //------------------------------------------------------------------------------------------- + responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); + 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 StartThreadLoadingFunc(MessageContainer* requestContainer) +{ + StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + ustrncpy (ThreadLoadingToken, requestContainer->token,36); + //MessageState = 1; + 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 (request->processparameters) + { + dryerbufferlength = request->processparameters->dryerbufferlength; + LoadArmInfo.LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + + } + 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(); + 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; + +} |