diff options
| author | Shlomo Hecht <shlomo@twine-s.com> | 2020-01-27 09:19:11 +0200 |
|---|---|---|
| committer | Shlomo Hecht <shlomo@twine-s.com> | 2020-01-27 09:19:11 +0200 |
| commit | ddda6089bff56e80703c8d2dce297919edc58bf1 (patch) | |
| tree | 7702c5cf169124d522eacc7f1a9e0878373baedd /Software/Embedded_SW/Embedded/Modules/Thread | |
| parent | 1d4d327571d4c0c9f4e17411551bd4dae1e2aed0 (diff) | |
| parent | bf2f3245339b9fd9148a2ad25b5ba3320e970cc1 (diff) | |
| download | Tango-ddda6089bff56e80703c8d2dce297919edc58bf1.tar.gz Tango-ddda6089bff56e80703c8d2dce297919edc58bf1.zip | |
Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
6 files changed, 896 insertions, 257 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 56fb79c23..d1d23598f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -82,6 +82,7 @@ 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); 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; + +} diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 9d57cd29f..78092ae2b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -61,11 +61,13 @@ static bool KeepWindingCone = false; static uint32_t WindingConeLocation; 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 @@ -105,10 +107,17 @@ uint32_t InternalWindingConfigMessage(JobSpool* request) InternalWinderCfg.spoolbackingrate = request->backingrate; InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; InternalWinderCfg.SpoolBottomBackingRate = request->bottombackingrate; - InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*2;//request->rotationsperpassage; + InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*3;//request->rotationsperpassage; + if (request->rotationsperpassage > 6.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.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.spoolbackingrate,(int)InternalWinderCfg.SpoolBottomBackingRate,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000)); Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.diameter,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0); return status; @@ -137,13 +146,13 @@ uint32_t Winder_Prepare(void *JobDetails) * report ready to the job STM */ - /*if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) + if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); PrepareReady(Module_Winder,ModuleFail); AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true); return ERROR; - }*/ + } #ifdef READ_SCREW_ENCODER ScrewLocationRun[0] = 0; @@ -278,7 +287,6 @@ 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; @@ -287,14 +295,8 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { //uint32_t Steps; double temp,tempScrewSpeed; - double screw_horizontal_speed = 0; - double RotationsPerSecond; - //double Averagewinderspeed = 0; - -// { -// TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter]; -// WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR]; -// TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter]; + //double screw_horizontal_speed = 0; + //double RotationsPerSecond; if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION) { if (WinderMotorSpeedRollOver == false) @@ -304,21 +306,10 @@ 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++; @@ -342,16 +333,16 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 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); + 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]; #endif if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out { @@ -377,24 +368,6 @@ 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; @@ -457,7 +430,6 @@ 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; @@ -470,7 +442,6 @@ 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; } @@ -547,7 +518,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId) } if (SegmentId == 0) { - ScrewControlId = AddControlCallback(Screw100msecDirectionChange, eHundredMillisecond,TemplateDataReadCBFunction,0,0,0); + ScrewControlId = AddControlCallback("screw control",Screw100msecDirectionChange, eHundredMillisecond,TemplateDataReadCBFunction,0,0,0); } PreSegmentReady(Module_Winder,ModuleDone); @@ -583,7 +554,7 @@ uint32_t WinderDistanceToSpoolState(void ) double DTS_Time = (dryerbufferCentimeters/dyeingspeed)*1000;//distance to spool time in milliseconds REPORT_MSG ((int)DTS_Time,"WinderDistanceToSpoolState"); - ScrewDTSControlId = AddControlCallback(ScrewDTSCallback, DTS_Time-WinderBackToBaseTime,TemplateDataReadCBFunction,0,0,0); + ScrewDTSControlId = AddControlCallback("screw DTS",ScrewDTSCallback, DTS_Time-WinderBackToBaseTime,TemplateDataReadCBFunction,0,0,0); REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback start"); return OK; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 99117ca5d..420b782b5 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -42,6 +42,8 @@ uint32_t StoreDancerConfigMessage(void); uint32_t LoadDancerConfigMessage(void); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); +extern float NumberOfRotationPerPassage; //debug for rotation per passage trials + uint32_t Winder_Init(void); uint32_t Winder_Prepare(void *JobDetails); uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId); @@ -50,22 +52,22 @@ uint32_t Winder_End(void); typedef enum { - /*01*/ THREAD_LOAD_INIT, - /*02*/ THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - /*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_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - /*07*/ THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND - /*08*/ THREAD_LOAD_CLOSE_ROCKERS, - /*09*/ THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD - /*10*/ THREAD_LOAD_CLOSE_LIDS, - /*11*/ THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION - /*12*/ THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 + /*00*/ THREAD_LOAD_INIT, + /*01*/ THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, + /*05*/ THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE + /*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, + /*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 - /*13*/ THREAD_LOAD_RESUME_HEATING, - /*14*/ THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING - /*15*/ THREAD_LOAD_END + /*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; uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue); bool ThreadLoadingActive(void); @@ -74,6 +76,10 @@ 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 Thread_Load_End(void); #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 88d62a495..fb3b78752 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -11,7 +11,7 @@ #include "PMR/Hardware/HardwareDancer.pb-c.h" #include "PMR/Hardware/HardwareWinder.pb-c.h" #include "PMR/Hardware/HardwareBreakSensor.pb-c.h" -#include <PMR/Diagnostics/EventType.pb-c.h> +#include "PMR/Diagnostics/EventType.pb-c.h" #include "Modules/AlarmHandling/AlarmHandling.h" #include "PMR/Printing/JobSpool.pb-c.h" @@ -23,6 +23,7 @@ #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}; @@ -105,6 +106,10 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest) MotorsCfg[Motor_i].toffmin = request->toffmin ; 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); } @@ -148,11 +153,12 @@ char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg"; uint32_t StoreDancerConfigMessage() { - FRESULT Fresult = FR_OK; + uint32_t status = OK; + // FRESULT Fresult = FR_OK; HardwareConfiguration DancerConfig; HardwareDancer Dancers[MAX_SYSTEM_DANCERS]; - uint8_t* response_buffer; - size_t response_size = 0; + // uint8_t* response_buffer; + // size_t response_size = 0; int Dancer_i; hardware_configuration__init(&DancerConfig); @@ -164,15 +170,16 @@ uint32_t StoreDancerConfigMessage() 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.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0); DancerConfig.n_dancers++; DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint; } - MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); - MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint); - 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); + 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) { @@ -187,43 +194,23 @@ uint32_t StoreDancerConfigMessage() my_free(response_buffer); - - return Fresult; +*/ + return status; } uint32_t LoadDancerConfigMessage() { - void* buffer = NULL; - uint32_t Bytes = 0; - FRESULT Fresult = FR_OK; - - HardwareConfiguration *DancerConfig; - int Dancer_i; - HardwareDancer DancersCfg1[MAX_SYSTEM_DANCERS] = {0}; - HardwareDancer DancersCfg2[MAX_SYSTEM_DANCERS] = {0}; - - memcpy(DancersCfg1,(void *)DANCERS_MAP_IN_FLASH,sizeof(DancersCfg1)); + uint32_t status = OK; - Fresult = FileRead(DancerConfigPath, &Bytes, &buffer); - if (Fresult == FR_OK) - { - DancerConfig = hardware_configuration__unpack(NULL, Bytes, buffer); - for (Dancer_i = 0; Dancer_i < DancerConfig->n_dancers ; Dancer_i++) - { - DancersCfg2[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint; - } - hardware_configuration__free_unpacked(DancerConfig,NULL); - free (buffer); - } 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); - Report("Dancer 0 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg1[0].zeropoint,RpWarning,(int)DancersCfg2[0].zeropoint,0); - Report("Dancer 1 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[1].zeropoint,(int)DancersCfg1[1].zeropoint,RpWarning,(int)DancersCfg2[1].zeropoint,0); - Report("Dancer 2 Store data flash, internal flash, eeprom ",__FILE__,DancersCfg[2].zeropoint,(int)DancersCfg1[2].zeropoint,RpWarning,(int)DancersCfg2[2].zeropoint,0); + 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); - return Fresult; + 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 47fc37081..2484b9444 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -49,6 +49,7 @@ uint32_t PoolerSpeedControlId=0xFF; double DancerError[NUM_OF_DANCERS] = {0.0}; double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; +double InitialDryerSpeed = 0.0; uint32_t JobCounter = 0; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; @@ -116,8 +117,8 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea if (Current_Read < Previous_Read) { + ReportWithPackageFilter(ThreadFilter,"Length rollover",__FILE__,__LINE__,(int)Current_Read,RpWarning,(int)Previous_Read,0); Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1; - Report("Length rollover",__FILE__,__LINE__,(int)Current_Read,RpWarning,(int)Previous_Read,0); } else Time_Pass = Current_Read - Previous_Read; @@ -142,7 +143,7 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; } -char Lenstr[150]; +char Lenstr[160]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -157,7 +158,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - LOG_ERROR (IfIndex, "Wrong Interface type"); + ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -165,7 +166,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) // return OK; if (index != FEEDER_MOTOR) { - LOG_ERROR (IfIndex, "Wrong Motor"); + ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); return 0xFFFFFFFF; } CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); @@ -191,7 +192,7 @@ 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); - Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); length = 0; } @@ -223,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); - Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -247,7 +248,7 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - LOG_ERROR (IfIndex, "Wrong Interface type"); + ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -255,7 +256,7 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) // return OK; if (index != POOLER_MOTOR) { - LOG_ERROR (IfIndex, "Wrong Motor"); + ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); return 0xFFFFFFFF; } PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); @@ -281,7 +282,7 @@ 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); - Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); length = 0; } @@ -299,9 +300,12 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { if (PrepareState == true) { +//#ifdef Use_Head_Card +// strcpy(Lenstr,"Heating up"); +//#else //later - add temperatures TemperatureListString(Lenstr); - +//#endif SendJobProgress(0.0,0,false, Lenstr); } else @@ -314,7 +318,7 @@ uint32_t PoolerThreadLengthCBFunction(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); - Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -339,7 +343,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { - LOG_ERROR (IfIndex, "Wrong Interface type"); + ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -371,7 +375,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) int index; if (IfIndex>>8 != IfTypeThread) { - LOG_ERROR (IfIndex, "Wrong Interface type"); + ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -420,7 +424,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]); - Report(time,__FILE__,__LINE__,111,RpError,111,0); + ReportWithPackageFilter(ThreadFilter,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); @@ -442,6 +446,7 @@ char TMessage[150]; //char time[150]; uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; +bool FirstCalcInJob = true; uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -449,7 +454,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; //read value is the dancer angle - int i,index=MAX_THREAD_MOTORS_NUM; + int i,index=MAX_THREAD_MOTORS_NUM,len; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; @@ -464,7 +469,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) #endif if (IfIndex>>8 != IfTypeThread) { - LOG_ERROR (IfIndex, "Wrong Interface type"); + ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; @@ -477,7 +482,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) if (ReadValue < 10) { MotorFailedSample[index]++; - Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + ReportWithPackageFilter(ThreadFilter,"Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; } if (ReadValue == 0x3FFF) @@ -486,13 +491,17 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) if (dancerinvalid == false) { dancerinvalid = true; - LOG_ERROR(index, "Dancer value invalid."); + ReportWithPackageFilter(ThreadFilter,"Dancer value invalid.",__FILE__,__LINE__,(int)IfIndex,RpError,(int)ReadValue,0); } return OK; } KeepReadValue = ReadValue; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) + if (abs(TranslatedReadValue) > 0x2000) + { + TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment + } + if ((index == POOLER_MOTOR)||((index == FEEDER_MOTOR)&&(DancersCfg[DancerId].assemblydirectionright == true))) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); @@ -532,7 +541,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); - LOG_ERROR(index, "ReadBreakSensor Error"); + ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return OK; } //passed limit }//ReadBreakSensor()==ERROR @@ -540,7 +549,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { if (BreakSensorCounter) { - LOG_ERROR(BreakSensorCounter, "ReadBreakSensor Spike"); + ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); } BreakSensorCounter = 0; } @@ -558,7 +567,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); - LOG_ERROR(index, "thread speed too low Error"); + ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return OK; } } @@ -572,7 +581,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { keepdata = false; usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); - Report(TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); + ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); //JobAbortedByUser = true; ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); @@ -592,27 +601,28 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true); break; }*/ - LOG_ERROR (DancerId, "Dancer Failure"); + ReportWithPackageFilter(ThreadFilter,"Dancer Failure",__FILE__,DancerId,(int)avreageSampleValue,RpError,(int)JobCounter,0); 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 = 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 - { - MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); - } - else + /*else { //KeepNormalizedError = NormalizedError; - } + }*/ /*if ((JobCounter % 100) == 0) { //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit //{ - // Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); + // ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); //} /`*if (JobCounter >= 3000) { @@ -623,23 +633,46 @@ 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; - //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); + //ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue; }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; + if (FirstCalcInJob == true) + { + if (index == FEEDER_MOTOR) + { + FirstCalcInJob = false; + len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", + ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), + (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); + + } + } + #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 { - CurrentControlledSpeed[index] = calculated_speed; - MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); + if (calculated_speed>5.0) + { + 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); + } + /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit { - Report("MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); + ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); }*/ #ifdef TEST_PID_THREAD int len; @@ -658,7 +691,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) 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); - Report(TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); //Task_sleep(100); //if (controlIndex++>=MAX_THREAD_CONTROL_LOG) // controlIndex = 0; @@ -711,26 +744,56 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { - int Motor_i, HW_Motor_Id, Pid_Id; + int Motor_i,i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; CurrentSegmentId = 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); + EnableLubrication = JobTicket->enablelubrication; EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; + 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) + { + 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,RpFatalError,LIMIT,0); + //JobEndReason = JOB_LIDS_OPEN; + //PrepareReady(Module_Thread,ModuleFail); + //return ERROR; + } //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) @@ -754,12 +817,22 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) 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 - +////////////////////////////////////////////////// + for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) + { + if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + MotorSamples[Motor_i][i] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0); //reset the samples value for control beginning + else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) + MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; + //MotorSpeedSamples[Motor_i][i] = 0; + } + MotorSamplePointer[Motor_i] = 0; +///////////////////////////////////////////////////// MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { - Report("Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (SpeedControlId != 0xFF) { RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); @@ -767,33 +840,33 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } 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 { - Report("Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (PoolerSpeedControlId != 0xFF) { if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - LOG_ERROR(Motor_i,"Remove Control Failed"); + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); PoolerSpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); - PoolerSpeedControlId = AddControlCallback(PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } 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) - LOG_ERROR(Motor_i,"Remove Control Failed"); + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } #ifndef TEST_PID_THREAD - ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); - //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); #endif } if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled @@ -801,31 +874,31 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) - LOG_ERROR(Motor_i,"Remove Control Failed"); + 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(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #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 { - Report("Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); + 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) - LOG_ERROR(Motor_i,"Remove Control Failed"); + 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(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } // if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled // AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); - if (Motor_i == 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 + if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled continue; } #ifdef TEST_PID_THREAD @@ -833,9 +906,6 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) #endif PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions - InitialProcess = true; - initialpos = 0xFFFF; - Poolerinitialpos = 0xFFFF; return OK; } @@ -852,11 +922,13 @@ void SetOriginMotorSpeed(float process_speed) / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius); //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; + InitialDryerSpeed = 0.0; CurrentControlledSpeed[Motor_i] = (int) motor_speed; - //Report("Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0); + if (process_speed > 1) + ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)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) @@ -865,6 +937,28 @@ void ThreadPreSegmentEnded(void) REPORT_MSG (0,"First ThreadPreSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } +int DrierDivider = 20; +uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag) +{ + InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider); + 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); + + + return OK; +} //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) @@ -875,10 +969,10 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) float process_speed = dyeingspeed; if (dyeingspeed == 0) { - LOG_ERROR (dyeingspeed," job speed zero"); + ReportWithPackageFilter(ThreadFilter,"job speed zero.",__FILE__,__LINE__,(int)dyeingspeed,RpError,(int)SegmentId,0); return ERROR; } - REPORT_MSG (dyeingspeed," ThreadPreSegmentState"); + ReportWithPackageFilter(ThreadFilter,"ThreadPreSegmentState",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)SegmentId,0); 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); @@ -886,7 +980,18 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) PrepareState = false; #ifndef TEST_PID_THREAD // set the new speed in the dryer motor to the speed of the new segment - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); + 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,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); + InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; + MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); + ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0); #endif #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); @@ -1004,10 +1109,10 @@ char Endstr[150]; { int Motor_i; ThreadControlActive = false; - + uint32_t status = OK,tempCtl; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); - Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); ThreadUpdateProcessLength (0.0,(void *)NULL); //TotalProcessedLength = 0.0; @@ -1018,27 +1123,31 @@ char Endstr[150]; if (SpeedControlId != 0xFF) { if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK) - LOG_ERROR(SpeedControlId,"RemoveControl Failed"); + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)SpeedControlId,0); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) - LOG_ERROR(PoolerSpeedControlId,"RemoveControl Failed"); + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); PoolerSpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { + tempCtl = ControlIdtoMotorId[Motor_i]; if (ControlIdtoMotorId[Motor_i] != 0xFF) { - if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK) + status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + if(status == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else - LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed"); + ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0); } + //ReportWithPackageFilter(ThreadFilter,"Remove Control",__FILE__,Motor_i,(int)status,RpError,(int)tempCtl,0); + } - Task_sleep(10); + Task_sleep(100); for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); |