/* * Motor.c * * Created on: 15 במרץ 2018 * Author: shlomo */ /************************************************************************************************ * TimerMotor.h * this module is used to operate and control over dispensers and winding, using clock object * the motors which are not connected to the PWM pins and needs microstepping * *************************************************************************************************/ #include #include #include "include.h" #include "motor.h" #include "Modules/thread/thread.h" #include "Modules/Control/control.h" #include #include #include "drivers/SPI/SPI_Comm.h" #include "drivers/FPGA/FPGA_SPI_Comm.h" #include "drivers/FPGA/FPGA_Comm.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "drivers/SSI_Comm/Dancer/Dancer.h" #include "drivers/Flash_ram/MCU_E2Prom.h" #include "AlarmHandling/AlarmHandling.h" ///////////////////////// #include "driverlib/ssi.h" //callback_fptr MotorMoveModuleCallback[NUM_OF_MOTORS]; //callback_fptr MotorSetSpeedModuleCallback[NUM_OF_MOTORS]; //callback_fptr MotorMovetoLimitSwitchCallback[NUM_OF_MOTORS]; callback_fptr MotorCallback[NUM_OF_MOTORS]; ControlCBFunction MotorControlCallback[NUM_OF_MOTORS]; uint32_t DrierZeroPosition = 0; //uint32_t DryerBackLash = 0; //bool DrierBackLashDirection = false; uint32_t Arm_Drier_Center = 0; callback_fptr UnloadingReset = NULL; int32_t ArmSamples[MAX_CONTROL_SAMPLES] = {0}; int ArmSamplePointer = {0}; int StoredavreageSampleValue = 0; uint32_t CallbackCalls = 0; int32_t Initialcurrentposition =0; int cycles = 0; uint32_t MotorMoveCallBackFunction(uint32_t deviceId, uint32_t BusyFlag); uint32_t MotorSetSpeedCallBackFunction(uint32_t deviceId, uint32_t BusyFlag); uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag); uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, uint32_t LimitSwitch); uint32_t MotorRunCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag); //uint32_t MotorMoveControlId[NUM_OF_MOTORS]; //uint32_t MotorSetSpeedControlId[NUM_OF_MOTORS]; //uint32_t MotorMovetoLimitSwitchControlId[NUM_OF_MOTORS]; uint32_t MotorTimeout[NUM_OF_MOTORS]; uint32_t MotorTimeLag[NUM_OF_MOTORS]; uint32_t MotorTimeLimit[NUM_OF_MOTORS]; uint32_t MotorControlId[NUM_OF_MOTORS]; uint32_t StoredMotorPosition[NUM_OF_MOTORS]; #ifdef FOUR_WINDERS FPGA_GPI_ENUM Motor_Id_to_LS_IdDown[NUM_OF_MOTORS] = { // limit switch for normal homing into working mode GPI_LS_DH_CLEAN_LEFT , //MOTO_DH_CLEANHEAD = 0, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_CLEAN_RIGHT, //MOTO_DH_CLEANMECH = 1, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_LID_CLOSED, //MOTO_DH_LID = 2, MAX_GPI, //MOTO_DRYER_DRIVING = 3, GPI_LS_DRYER_LID_CLOSED, //MOTO_DRYER_LID = 4, MAX_GPI, //MOTO_DRYER_LOADARM = 5, GPI_LS_DISPENSER_DOWN_1, //MOTO_DISPENSER_1 = 6, GPI_LS_DISPENSER_DOWN_2, //MOTO_DISPENSER_2 = 7, GPI_LS_DISPENSER_DOWN_3, //MOTO_DISPENSER_3 = 8, GPI_LS_DISPENSER_DOWN_4, //MOTO_DISPENSER_4 = 9, GPI_LS_DISPENSER_DOWN_5, //MOTO_DISPENSER_5 = 10, GPI_LS_DISPENSER_DOWN_6, //MOTO_DISPENSER_6 = 11, GPI_LS_DISPENSER_DOWN_7, //MOTO_DISPENSER_7 = 12, GPI_LS_DISPENSER_DOWN_8, //MOTO_DISPENSER_8 = 13, GPI_LS_SCREW_RIGHT, //MOTO_SCREW = 14, MAX_GPI, //MOTO_WINDER = 15, GPI_LS_LDANCER1_DOWN, //MOTO_LDANCER1 = 16, GPI_LS_LSPARE1, //MOTO_LDANCER2 = 17, MAX_GPI, //MOTO_LDRIVING = 18, GPI_LS_LLOADMOTOR_DOWN, //MOTO_LLOADING = 19, GPI_LS_LPIVOT_DOWN, //MOTO_LPIVOT1 = 20, GPI_LS_RDANCER_UP, //MOTO_RDANCER = 21, MAX_GPI, //MOTO_RDRIVING = 22, GPI_LS_RDANCER_LONG, //MOTO_RLOADARM = 23, GPI_LS_RLOADMOTOR_DOWN, //MOTO_RLOADING = 24, GPI_LS_SPARE1_2, //HARDWARE_MOTOR_TYPE__MOTO_SPARE1_1 = 25, MAX_GPI, //HARDWARE_MOTOR_TYPE__MOTO_SPARE1_2 = 26, GPI_LS_SPARE2_2, //HARDWARE_MOTOR_TYPE__MOTO_SPARE2_1 = 27, MAX_GPI, //HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2 = 28, }; #else FPGA_GPI_ENUM Motor_Id_to_LS_IdDown[NUM_OF_MOTORS] = { // limit switch for normal homing into working mode GPI_LS_DH_CLEAN_LEFT , //MOTO_DH_CLEANHEAD = 0, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_CLEAN_RIGHT, //MOTO_DH_CLEANMECH = 1, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_LID_CLOSED, //MOTO_DH_LID = 2, MAX_GPI, //MOTO_DRYER_DRIVING = 3, GPI_LS_DRYER_LID_CLOSED, //MOTO_DRYER_LID = 4, MAX_GPI, //MOTO_DRYER_LOADARM = 5, GPI_LS_DISPENSER_DOWN_1, //MOTO_DISPENSER_1 = 6, GPI_LS_DISPENSER_DOWN_2, //MOTO_DISPENSER_2 = 7, GPI_LS_DISPENSER_DOWN_3, //MOTO_DISPENSER_3 = 8, GPI_LS_DISPENSER_DOWN_4, //MOTO_DISPENSER_4 = 9, GPI_LS_DISPENSER_DOWN_5, //MOTO_DISPENSER_5 = 10, GPI_LS_DISPENSER_DOWN_6, //MOTO_DISPENSER_6 = 11, GPI_LS_DISPENSER_DOWN_7, //MOTO_DISPENSER_7 = 12, GPI_LS_DISPENSER_DOWN_8, //MOTO_DISPENSER_8 = 13, GPI_LS_SCREW_RIGHT, //MOTO_SCREW = 14, MAX_GPI, //MOTO_WINDER = 15, GPI_LS_LDANCER1_DOWN, //MOTO_LDANCER1 = 16, GPI_LS_LSPARE1, //MOTO_LDANCER2 = 17, MAX_GPI, //MOTO_LDRIVING = 18, GPI_LS_LLOADMOTOR_DOWN, //MOTO_LLOADING = 19, GPI_LS_LPIVOT_DOWN, //MOTO_LPIVOT1 = 20, GPI_LS_RDANCER_DOWN, //MOTO_RDANCER = 21, MAX_GPI, //MOTO_RDRIVING = 22, GPI_LS_RDANCER_LONG, //MOTO_RLOADARM = 23, GPI_LS_RLOADMOTOR_DOWN, //MOTO_RLOADING = 24, }; #endif FPGA_GPI_ENUM Motor_Id_to_LS_IdUp[NUM_OF_MOTORS] = { // limit switch for normal homing out of working mode GPI_LS_DH_CLEAN_LEFT , //MOTO_DH_CLEANHEAD = 0, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_CLEAN_RIGHT, //MOTO_DH_CLEANMECH = 1, Shlomo Correct setting for machine 7 25/3 GPI_LS_DH_LID_OPEN, //MOTO_DH_LID = 2, MAX_GPI, //MOTO_DRYER_DRIVING = 3, GPI_LS_DRYER_LID_OPEN, //MOTO_DRYER_LID = 4, MAX_GPI, //MOTO_DRYER_LOADARM = 5, GPI_LS_DISPENSER_UP_1, //MOTO_DISPENSER_1 = 6, GPI_LS_DISPENSER_UP_2, //MOTO_DISPENSER_2 = 7, GPI_LS_DISPENSER_UP_3, //MOTO_DISPENSER_3 = 8, GPI_LS_DISPENSER_UP_4, //MOTO_DISPENSER_4 = 9, GPI_LS_DISPENSER_UP_5, //MOTO_DISPENSER_5 = 10, GPI_LS_DISPENSER_UP_6, //MOTO_DISPENSER_6 = 11, GPI_LS_DISPENSER_UP_7, //MOTO_DISPENSER_7 = 12, GPI_LS_DISPENSER_UP_8, //MOTO_DISPENSER_8 = 13, GPI_LS_SCREW_LEFT, //MOTO_SCREW = 14, MAX_GPI, //MOTO_WINDER = 15, GPI_LS_LDANCER1_UP, //MOTO_LDANCER1 = 16, GPI_LS_LSPARE2, //MOTO_LDANCER2 = 17, MAX_GPI, //MOTO_LDRIVING = 18, GPI_LS_LLOADMOTOR_UP, //MOTO_LLOADING = 19, GPI_LS_LPIVOT_UP, //MOTO_LPIVOT1 = 20, GPI_LS_RDANCER_LONG, //MOTO_RDANCER = 21, MAX_GPI, //MOTO_RDRIVING = 22, GPI_LS_RLOADRAM_UP, //MOTO_RLOADARM = 23, GPI_LS_RLOADMOTOR_UP, //MOTO_RLOADING = 24, }; MotorHomingDirectionEnum Motor_Id_to_LS_Direction[NUM_OF_MOTORS] = { MotorHomingDirectionDown, //MOTO_DH_CLEANHEAD = 0, MotorHomingDirectionDown, //MOTO_DH_CLEANMECH = 1, MotorHomingDirectionDown, //MOTO_DH_LID = 2, MotorHomingDirectionNoHoming, //MOTO_DRYER_DRIVING = 3, MotorHomingDirectionDown, //MOTO_DRYER_LID = 4, MotorHomingDirectionNoHoming, //MOTO_DRYER_LOADARM = 5, MotorHomingDirectionDown, //MOTO_DISPENSER_1 = 6, MotorHomingDirectionDown, //MOTO_DISPENSER_2 = 7, MotorHomingDirectionDown, //MOTO_DISPENSER_3 = 8, MotorHomingDirectionDown, //MOTO_DISPENSER_4 = 9, MotorHomingDirectionDown, //MOTO_DISPENSER_5 = 10, MotorHomingDirectionDown, //MOTO_DISPENSER_6 = 11, MotorHomingDirectionDown, //MOTO_DISPENSER_7 = 12, MotorHomingDirectionDown, //MOTO_DISPENSER_8 = 13, MotorHomingDirectionUp, //MOTO_SCREW = 14, MotorHomingDirectionNoHoming, //MOTO_WINDER = 15, MotorHomingDirectionDown, //MOTO_LDANCER1 = 16, MotorHomingDirectionDown, //MOTO_LDANCER2 = 17, MotorHomingDirectionNoHoming, //MOTO_LDRIVING = 18, MotorHomingDirectionDown, //MOTO_LLOADING = 19, MotorHomingDirectionUp, //MOTO_LPIVOT1 = 20, MotorHomingDirectionDown, //MOTO_RDANCER = 21, MotorHomingDirectionNoHoming, //MOTO_RDRIVING = 22, MotorHomingDirectionUp, //MOTO_RLOADARM = 23, MotorHomingDirectionDown, //MOTO_RLOADING = 24, }; uint32_t MotorGotoWithCallback (TimerMotors_t MotorId, uint32_t Steps, uint32_t LimitSwitchId,callback_fptr callback,uint32_t timeout) //TODO { //assert (callback); //assert (isValid(deviceId)); //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); MotorCallback[MotorId] = callback; MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = eTenMillisecond; MotorTimeLimit[MotorId] = timeout; MotorStop(MotorId,Hard_Hiz ); Task_sleep(100); MotorGoTo(MotorId,Steps ); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveToLimitSwitchCallBackFunction, eTenMillisecond, FPGA_Read_limit_Switches,(IfTypeMotors*0x100+MotorId), LimitSwitchId, 0 ); MotorControlCallback[MotorId] = MotorMoveToLimitSwitchCallBackFunction; return MotorControlId[MotorId]; } uint32_t MotorGotoWithBusyCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout) //TODO { //assert (callback); //assert (isValid(deviceId)); //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); MotorCallback[MotorId] = callback; //MotorStop(MotorId,Hard_Hiz ); int sign = direction?1:-1; MotorGoTo(MotorId,Steps*sign ); CallbackCalls = 0; MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveCallBackFunction, /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } int32_t AccumulatedArmMovement = 0; int32_t InitialArmLocation = 0; bool ArmDirection = true; int failCounter = 0; uint32_t MotorRunWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Freq, callback_fptr callback,uint32_t timeout) //TODO { ReportWithPackageFilter(GeneralFilter,"MotorRunWithCallback",__FILE__,direction,MotorId,RpMessage,Freq,0); //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); MotorCallback[MotorId] = callback; MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = 20; MotorTimeLimit[MotorId] = timeout; CallbackCalls = 0; if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { InitialArmLocation = Read_Dryer_ENC_Position(0,0); AccumulatedArmMovement = 0; ArmDirection = direction; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierZeroPosition); failCounter = 0; MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,direction); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Freq); } else { return ERROR; } MotorControlId[MotorId] = AddControlCallback(NULL, MotorRunCallBackFunction, /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } uint32_t MotorRunCallBackFunction(uint32_t IfIndex, uint32_t ReadValue) //TODO { uint32_t MotorId,encoder,temp = 0; uint32_t RunningContinues = true; int angle; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = IfIndex&0xFF; CallbackCalls++; if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { encoder = Read_Dryer_ENC_Position(0,0); if (ArmDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize) //unloading - numbers going down { //previous number is bigger than current if (InitialArmLocation>encoder) { temp = InitialArmLocation-encoder; if ((temp>25)&&(temp<1000)) { AccumulatedArmMovement+=temp; InitialArmLocation=encoder; } } else if (InitialArmLocation25)&&(temp<1000)) { AccumulatedArmMovement+=temp; InitialArmLocation=encoder; } } } /*else //loading - going up { //previous number is smaller than current if (InitialArmLocation25)&&(temp<1000)) { AccumulatedArmMovement+=temp; InitialArmLocation=encoder; } } else if (InitialArmLocation>encoder)//rollover { temp = 0x3FFF + encoder -InitialArmLocation; if ((temp>25)&&(temp<1000)) { AccumulatedArmMovement+=temp; InitialArmLocation=encoder; } } }*/ if (((temp>1000)||(temp<20))&&(CallbackCalls>10)) { Report("MotorRunCallBackFunction temp curr prev small",__FILE__,encoder,InitialArmLocation,RpWarning,temp,0); failCounter++; if (failCounter>=8) { Report("arm stopped",__FILE__,failCounter,encoder,RpWarning,temp,0); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); RunningContinues = false; } } else { failCounter = 0; RunningContinues = true; } if (CallbackCalls%50 == 0) { //encoder = Read_Dryer_ENC_Position(0,0); Report("MotorRunCallBackFunction periodic curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); } if (CallbackCalls%200 == 0) { //encoder = Read_Dryer_ENC_Position(0,0); Report("MotorRunCallBackFunction",__FILE__,__LINE__,encoder,RpWarning,CallbackCalls,0); } } else { if (CallbackCalls%100 == 1) { Report("MotorRunCallBackFunction wrong motor",__FILE__,MotorId,MotorTimeout[MotorId],RpWarning,MotorTimeLimit[MotorId],0); return ERROR; } } MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if ((RunningContinues == false)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) { if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { angle = Calculate_Arm_Distance(Arm_Drier_Center,encoder); if (abs(angle)<400) { RunningContinues = false; } else { RunningContinues = true; } Report("arm stopped call",__FILE__,failCounter,encoder,RpWarning,abs(DrierZeroPosition-encoder),0); Report("arm stopped call 2",__FILE__,MotorTimeout[MotorId],RunningContinues,RpWarning,angle,0); } //stop this control loop Report("MotorControlGetnBusyState stop",__FILE__,MotorId,RunningContinues,RpMessage,MotorCallback[MotorId],0); SafeRemoveControlCallback(MotorControlId[MotorId], MotorRunCallBackFunction ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); if ((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0)) { Report("motor timeout",__FILE__,__LINE__,MotorId,RpMessage,MotorTimeout[MotorId],0); RunningContinues = true; } Report("MotorRunCallBackFunction curr prev return",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); //call the module callback if (MotorCallback[MotorId]) { MotorCallback[MotorId](MotorId,RunningContinues); } } return OK; } uint32_t MotorMoveWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout) //TODO { //assert (callback); //assert (isValid(deviceId)); /*if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { printBusy = true; Report("MotorControlGetnBusyState print",__FILE__,__LINE__,MotorDriverResponse[MotorId].Busy,RpMessage,0,0); }*/ ReportWithPackageFilter(GeneralFilter,"MotorMoveWithCallback",__FILE__,direction,MotorId,RpMessage,Steps,0); CallbackCalls = 0; //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); MotorCallback[MotorId] = callback; MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = 20; MotorTimeLimit[MotorId] = timeout; /*if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { InitialArmLocation = Read_Dryer_ENC_Position(0,0); AccumulatedArmMovement = 0; ArmDirection = direction; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierZeroPosition); failCounter = 0; MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].maxfrequency); } else*/ { //MotorStop(MotorId,Hard_Hiz ); MotorMove(MotorId,direction,Steps ); } if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { Task_sleep (100); } MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveCallBackFunction, /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t ArmPosition) //TODO { uint32_t MotorId; uint32_t MotorPosition; int i,avreageSampleValue = 0,avreageSampleSum = 0; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = IfIndex&0xFF; MotorPosition = ArmPosition; CallbackCalls++; if (CallbackCalls%20 == 0) { // REPORT_MSG(MotorPosition,"MotorVerifiedCallBackFunction"); //Report("Move_To_Stopper_Callback",__FILE__,__LINE__,MotorPosition,RpMessage,StoredMotorPosition[MotorId],0); //CallbackCalls = 0; } if (abs(Initialcurrentposition-MotorPosition)<40) { cycles++; Report("Move_To_Stopper_Callback",__FILE__,cycles,MotorPosition,RpMessage,Initialcurrentposition,0); } Read_Dryer_ENC_Position(); //trigger the next call ArmSamples[ArmSamplePointer] = MotorPosition;//(-1 * TranslatedReadValue); ArmSamples[ArmSamplePointer] = MotorPosition;//(-1 * TranslatedReadValue); ArmSamplePointer++; if (ArmSamplePointer >= MAX_CONTROL_SAMPLES) ArmSamplePointer = 0; for (i=0;i=10)) /*Motor did not move in the last time lag*/ // if (abs(MotorPosition - StoredMotorPosition[MotorId])<=10) if (abs(avreageSampleValue-StoredavreageSampleValue)<=2) { //stop this control loop //REPORT_MSG(MotorPosition,"MotorVerifiedCallBackFunction stopped"); Report("Motor to stopper stopped",__FILE__,avreageSampleValue,StoredavreageSampleValue,RpWarning,avreageSampleSum,0); Report("Move_To_Stopper_Callback 1",__FILE__,cycles,MotorPosition,RpMessage,Initialcurrentposition,0); SafeRemoveControlCallback(MotorControlId[MotorId], MotorVerifiedCallBackFunction ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; MotorStop(MotorId,Hard_Hiz ); /* Task_sleep(5); MotorMove(MotorId,DrierBackLashDirection,DryerBackLash ); Report("Motor to stopper move to backlash",__FILE__,MotorId,DryerBackLash,RpWarning,DrierBackLashDirection,0); Task_sleep(100); if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) MotorStop(MotorId,Hard_Stop ); Task_sleep(5); Read_Dryer_ENC_Position(); //trigger the next call Task_sleep(5);*/ // MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CENTER,DrierZeroPosition); Report("MotorVerifiedCallBackFunction",__FILE__,__LINE__,DrierZeroPosition,RpWarning,avreageSampleValue,0); //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,ArmPosition); MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,NULL,1000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); DrierZeroPosition = Read_Dryer_ENC_Position(); //trigger the next call } else { StoredMotorPosition[MotorId] = MotorPosition; StoredavreageSampleValue = avreageSampleValue; } return OK; } uint32_t MotorStopAction (TimerMotors_t MotorId) //TODO { Report("MotorStopAction",__FILE__,MotorId,MotorControlId[MotorId],RpWarning,MotorControlCallback[MotorId] ,0); RemoveControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlId[MotorId] = 0xFF; MotorControlCallback[MotorId] = NULL; MotorStop(MotorId,Hard_Hiz); return OK; } uint32_t MotorMoveToStopper (TimerMotors_t MotorId,bool direction, uint32_t Speed, callback_fptr callback,uint32_t backlash,uint32_t timeout) //TODO { //assert (callback); //assert (isValid(deviceId)); //call driver action to device id with the parameter MotorCallback[MotorId] = callback; MotorStop(MotorId,Hard_Hiz ); Task_sleep(5); MotorSetDirection( MotorId, direction); MotorGetPositionFromFPGA(MotorId); memset(ArmSamples,0,sizeof(ArmSamples)); ArmSamplePointer = 0; Read_Dryer_ENC_Position(); Task_sleep(10); Initialcurrentposition = Read_Dryer_ENC_Position(); cycles = 0; CallbackCalls = 0; StoredMotorPosition[MotorId] = Read_Dryer_ENC_Position(); Report("MotorMoveToStopper initial position",__FILE__,__LINE__,StoredMotorPosition[MotorId],RpWarning,Initialcurrentposition,0); MotorSetSpeed (MotorId, Speed); MotorControlId[MotorId] = AddControlCallback(NULL, MotorVerifiedCallBackFunction, 20,Control_Read_Dryer_Position,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorVerifiedCallBackFunction; return MotorControlId[MotorId]; } uint32_t MotorMoveToDrierPositionCallBackFunction(uint32_t IfIndex, uint32_t ReadValue) //TODO { TimerMotors_t MotorId; //uint32_t currentposition; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = (TimerMotors_t)(IfIndex&0xFF); if (MotorControlId[MotorId] == 0xFF) return ERROR; CallbackCalls++; if (CallbackCalls%100 == 0) { // REPORT_MSG(MotorPosition,"MotorVerifiedCallBackFunction"); Report("MotorMoveToDrierPositionCallBackFunction",__FILE__,MotorId,DrierZeroPosition,RpWarning,ReadValue,0); //CallbackCalls = 0; } Read_Dryer_ENC_Position(); MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if ((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))//timeout { LOG_ERROR(MotorTimeout[MotorId],"Timeout on MotorMoveToDrierPositionCallBackFunction"); } if ((abs(ReadValue - DrierZeroPosition)<2)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) { Report("MotorMoveToDrierPosition end",__FILE__,__LINE__,DrierZeroPosition,RpWarning,ReadValue,0); //stop this control loop SafeRemoveControlCallback(MotorControlId[MotorId], MotorMoveToDrierPositionCallBackFunction); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) MotorStop(MotorId,Hard_Stop ); //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); //call the module callback if (MotorCallback[MotorId]) { Report("MotorMoveToDrierPosition callback",__FILE__,__LINE__,MotorCallback[MotorId],RpWarning,ReadValue,0); MotorCallback[MotorId](MotorId,ReadValue); } } return OK; } uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callback,uint32_t timeout, bool direction,uint32_t speed) { //assert (callback); uint32_t currentposition = 0; // bool direction; //assert (isValid(deviceId)); //call driver action to device id with the parameter MotorCallback[MotorId] = callback; //decide the required direction and speed Read_Dryer_ENC_Position(); MotorStop(MotorId,Hard_Hiz ); Task_sleep(5); Read_Dryer_ENC_Position(); Task_sleep(5); currentposition = Control_Read_Dryer_Position(0,0); MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierZeroPosition); Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,DrierZeroPosition,RpWarning,currentposition,0); Report("MotorMovetoEncoderPosition callback",__FILE__,__LINE__,direction,RpWarning,speed,0); if (abs(currentposition - DrierZeroPosition)<2) { if (callback) callback(MotorId,0); return OK; } //Report("MotorMovetoEncoderPosition direction",__FILE__,__LINE__,direction,RpWarning,15,0); //MotorSetDirection( MotorId, direction); MotorSetDirection(MotorId,direction); MotorSetSpeed (MotorId, speed); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveToDrierPositionCallBackFunction, 2,Control_Read_Dryer_Position,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveToDrierPositionCallBackFunction; MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = 2; MotorTimeLimit[MotorId] = timeout; return MotorControlId[MotorId]; } uint32_t MotorSetSpeedWithCallback (TimerMotors_t MotorId, uint32_t _freq, callback_fptr callback) //TODO { //assert (callback); MotorCallback[MotorId] = callback; MotorControlId[MotorId] = AddControlCallback(NULL, MotorSetSpeedCallBackFunction, eTenMillisecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorSetSpeedCallBackFunction; MotorSetSpeed(MotorId, _freq); return MotorControlId[MotorId]; } uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO { uint32_t MotorId; uint32_t Busy = BusyFlag; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = IfIndex&0xFF; CallbackCalls++; if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { Read_Dryer_ENC_Position(); if (CallbackCalls<50) return OK; if (CallbackCalls%200 == 0) { Report("MotorMoveCallBackFunction",__FILE__,__LINE__,MotorId,RpWarning,Read_Dryer_ENC_Position(0,0),0); } } else { if (CallbackCalls%100 == 1) { Report("MotorMoveCallBackFunction",__FILE__,MotorId,MotorTimeout[MotorId],RpWarning,MotorTimeLimit[MotorId],0); } } MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if ((BusyFlag == NOTBUSY)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) { //stop this control loop Report("MotorControlGetnBusyState stop",__FILE__,MotorId,BusyFlag,RpMessage,MotorTimeout[MotorId],0); SafeRemoveControlCallback(MotorControlId[MotorId], MotorMoveCallBackFunction ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); if ((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0)) { Report("motor timeout",__FILE__,__LINE__,MotorId,RpMessage,MotorTimeout[MotorId],0); Busy = BUSY; } //call the module callback if (MotorCallback[MotorId]) { MotorCallback[MotorId](MotorId,Busy); } } return OK; } uint32_t MotorSetSpeedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO { uint32_t MotorId; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = IfIndex&0xFF; if (BusyFlag == 1) { //stop this control loop SafeRemoveControlCallback(MotorControlId[MotorId], MotorSetSpeedCallBackFunction ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,BusyFlag); } return OK; } bool CloseMagnet = false; bool HoldRightDancer = false; uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, uint32_t LimitSwitch) //TODO { TimerMotors_t MotorId; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = (TimerMotors_t)(IfIndex&0xFF); if (MotorControlId[MotorId] == 0xFF) { SafeRemoveControlCallback(MotorControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction ); return ERROR; } MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if ((LimitSwitch == LIMIT) ||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) { //stop this control loop SafeRemoveControlCallback(MotorControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDANCER)&&(HoldRightDancer == true)) { REPORT_MSG(MotorId,"RDancer Motor at limit"); HoldRightDancer = false; MotorStop(MotorId,Soft_Stop); //TODO in run time limit switch just reverse direction } else if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_DH_LID) && (Head_Type == HEAD_TYPE_ARC)) { REPORT_MSG(MotorId,"Cleaner Motor at limit"); MotorStop(MotorId,Soft_Stop); //TODO in run time limit switch just reverse direction } else { REPORT_MSG(MotorId,"Motor at limit"); MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction } //possibly: start regular control (speed etc) if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DH_LID) { ReportWithPackageFilter(GeneralFilter,"dh limit callback",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpMessage,LimitSwitch,0); } //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_DH_LID)&&(LimitSwitch == LIMIT)) { if (CloseMagnet == true) { MotorMove(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize,200); Task_sleep(200); ActivateHeadMagnet(); // close the lid magnet CloseMagnet=false; ReportWithPackageFilter(GeneralFilter,"calling ActivateHeadMagnet",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpMessage,LimitSwitch,0); } /*else { Report("Diagnostics_Open_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0); Trigger_Head_Actuators_Control(ACTOT, LOW,true); Trigger_Head_Actuators_Control(ACTIN, LOW,true); }*/ } if ((LimitSwitch != LIMIT) &&((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) //timeout { switch (MotorId) { case HARDWARE_MOTOR_TYPE__MOTO_RDANCER: case HARDWARE_MOTOR_TYPE__MOTO_RLOADARM: case HARDWARE_MOTOR_TYPE__MOTO_RLOADING: if(MotorDriverRequest[MotorId].Direction == MotorsCfg[MotorId].directionthreadwize) AlarmHandlingSetAlarm (EVENT_TYPE__RTFU_DOWN_TIMEOUT,ON); //handle alarm detection and operation else AlarmHandlingSetAlarm (EVENT_TYPE__RTFU_UP_TIMEOUT,ON); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_LDANCER1: case HARDWARE_MOTOR_TYPE__MOTO_LDANCER2: case HARDWARE_MOTOR_TYPE__MOTO_LPIVOT1: case HARDWARE_MOTOR_TYPE__MOTO_LLOADING: if(MotorDriverRequest[MotorId].Direction == MotorsCfg[MotorId].directionthreadwize) AlarmHandlingSetAlarm (EVENT_TYPE__LTFU_DOWN_TIMEOUT,ON); //handle alarm detection and operation else AlarmHandlingSetAlarm (EVENT_TYPE__LTFU_UP_TIMEOUT,ON); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_SCREW: AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,ON); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_DH_LID: if(MotorDriverRequest[MotorId].Direction == MotorsCfg[MotorId].directionthreadwize) AlarmHandlingSetAlarm (EVENT_TYPE__DYEING_HEAD_COVER_CLOSE_TIMEOUT,ON); //handle alarm detection and operation else AlarmHandlingSetAlarm (EVENT_TYPE__DYEING_HEAD_COVER_OPEN_TIMEOUT,ON); //handle alarm detection and operation break; default: REPORT_MSG(MotorId,"Motor timeout"); break; } } else { if (LimitSwitch == LIMIT) { switch (MotorId) { case HARDWARE_MOTOR_TYPE__MOTO_RDANCER: case HARDWARE_MOTOR_TYPE__MOTO_RLOADARM: case HARDWARE_MOTOR_TYPE__MOTO_RLOADING: AlarmHandlingSetAlarm (EVENT_TYPE__RTFU_DOWN_TIMEOUT,OFF); //handle alarm detectiOFF and operatiOFF AlarmHandlingSetAlarm (EVENT_TYPE__RTFU_UP_TIMEOUT,OFF); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_LDANCER1: case HARDWARE_MOTOR_TYPE__MOTO_LDANCER2: case HARDWARE_MOTOR_TYPE__MOTO_LPIVOT1: case HARDWARE_MOTOR_TYPE__MOTO_LLOADING: AlarmHandlingSetAlarm (EVENT_TYPE__LTFU_DOWN_TIMEOUT,OFF); //handle alarm detection and operation AlarmHandlingSetAlarm (EVENT_TYPE__LTFU_UP_TIMEOUT,OFF); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_SCREW: AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF); //handle alarm detection and operation break; case HARDWARE_MOTOR_TYPE__MOTO_DH_LID: AlarmHandlingSetAlarm (EVENT_TYPE__DYEING_HEAD_COVER_CLOSE_TIMEOUT,OFF); //handle alarm detection and operation AlarmHandlingSetAlarm (EVENT_TYPE__DYEING_HEAD_COVER_OPEN_TIMEOUT,OFF); //handle alarm detection and operation break; default: REPORT_MSG(MotorId,"Motor timeout solved"); break; } } } //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,LimitSwitch); } return OK; } uint32_t MotorMovetoLimitSwitch (TimerMotors_t MotorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback,uint32_t timeout) //TODO { //assert (callback); assert (LimitSwitchId<=MAX_GPI); ReportWithPackageFilter(GeneralFilter,"MotorMovetoLimitSwitch",__FILE__,direction,MotorId,RpMessage,Freq,0); if (MotorControlId[MotorId] != 0xFF) { RemoveControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; //return ERROR; } //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DH_LID) { if ((Is_PP_Machine()==true)&&(Safety_Incident_Report()==Safety_Event_Occurred)) { ReportWithPackageFilter(GeneralFilter,"cannot open dyeing head lid when safety active",__FILE__,direction,MotorId,RpMessage,Safety_Incident_Report(),0); return ERROR; } if (Head_Type != HEAD_TYPE_ARC) { DeActivateHeadMagnet(); //open the lid magnet ReportWithPackageFilter(GeneralFilter,"calling DeActivateHeadMagnet",__FILE__,timeout,direction,RpMessage,Freq,0); } if (direction == MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize) { if (Head_Type != HEAD_TYPE_ARC) { CloseMagnet = true; } /*if (Head_Type == HEAD_TYPE_FLAT) { Report("Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0); Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN); HeadCard_Actuators_Relocate(); }*/ } } if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDANCER) { if (direction != MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize) { HoldRightDancer = true; } //Report("MotorMovetoLimitSwitch RDANCER",__FILE__,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,direction,RpMessage,HoldRightDancer,0); } MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = 5; //five milliseconds for better accuracy MotorTimeLimit[MotorId] = timeout; if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD)||(HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH)||(HARDWARE_MOTOR_TYPE__MOTO_SCREW)) MotorTimeLag[MotorId] = 2; MotorSetDirection( MotorId, direction); MotorSetSpeed(MotorId, Freq); MotorControlId[MotorId] = AddControlCallback("MoveToLimitSwitch", MotorMoveToLimitSwitchCallBackFunction, MotorTimeLag[MotorId] , FPGA_Read_limit_Switches,(IfTypeMotors*0x100+MotorId), LimitSwitchId, 0 ); if ( MotorControlId[MotorId] == 0xFF) return ERROR; MotorCallback[MotorId] = callback; MotorControlCallback[MotorId] = MotorMoveToLimitSwitchCallBackFunction; return OK; } uint8_t mBreakSensorCounter = 0; uint32_t MotorMoveToBreakSensorCallBackFunction(uint32_t IfIndex, uint32_t LimitSwitch) //TODO { TimerMotors_t MotorId; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = (TimerMotors_t)(IfIndex&0xFF); MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if (MotorControlId[MotorId] == 0xFF) return ERROR; if (LimitSwitch == OK) //thread running identified { mBreakSensorCounter++; if (mBreakSensorCounter > BreakSensordebouncetimemilli) { //stop this control loop SafeRemoveHighControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,LimitSwitch); } } if ((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0)) //timeout { SafeRemoveHighControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,NO_LIMIT); } return OK; } uint32_t MotorMovetoBreakSensor (TimerMotors_t MotorId,bool direction, uint32_t Freq, callback_fptr callback,uint32_t timeout) { // uint32_t MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500, ReadBreakSensor, Thread_Load_HomingCallback,10000); //assert (callback); if (MotorControlId[MotorId] != 0xFF) { RemoveControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; //return ERROR; } //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); mBreakSensorCounter = 0; MotorSetDirection( MotorId, direction); MotorSetSpeed(MotorId, Freq); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveToBreakSensorCallBackFunction, eOneMillisecond , ReadBreakSensor,(IfTypeMotors*0x100+MotorId), 0, 0 ); if ( MotorControlId[MotorId] == 0xFF) return ERROR; MotorCallback[MotorId] = callback; MotorControlCallback[MotorId] = MotorMoveToBreakSensorCallBackFunction; return OK; } bool DancerValueDirection = false; uint32_t DancerId = 0xFF; uint32_t MotorMoveToDancerPositionCallBackFunction(uint32_t IfIndex, uint32_t ReadValue) //TODO { TimerMotors_t MotorId; bool Condition = false; if (IfIndex>>8 != IfTypeMotors) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } MotorId = (TimerMotors_t)(IfIndex&0xFF); if (MotorControlId[MotorId] == 0xFF) return ERROR; if (DancerValueDirection == true) { if (ReadValue < DancersCfg[DancerId].zeropoint) Condition = true; } else { if (ReadValue > DancersCfg[DancerId].zeropoint) Condition = true; } MotorTimeout[MotorId]+=MotorTimeLag[MotorId]; if ((Condition == true)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) //thread running identified { //stop this control loop DancerValueDirection = false; DancerId = 0xFF; SafeRemoveControlCallback(MotorControlId[MotorId],MotorMoveToDancerPositionCallBackFunction ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; MotorStop(MotorId,Hard_Hiz); //TODO in run time limit switch just reverse direction //possibly: start regular control (speed etc) //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback, eOneMillisecond, (IfTypeMotors*0x100+MotorId), deviceId, Parameter ); //call the module callback if (MotorCallback[MotorId]) MotorCallback[MotorId](MotorId,Condition); } return OK; } uint32_t MotorMovetoDancerPosition (TimerMotors_t MotorId,bool direction, uint32_t Freq,uint32_t dancerid,bool dancervaluedirection, callback_fptr callback,uint32_t timeout) { if (MotorControlId[MotorId] != 0xFF) { RemoveControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; } //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); DancerValueDirection = dancervaluedirection; DancerId = dancerid; MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = eTenMillisecond; MotorTimeLimit[MotorId] = timeout; MotorSetDirection( MotorId, direction); MotorSetSpeed(MotorId, Freq); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveToDancerPositionCallBackFunction, eTenMillisecond , Control_Read_Dancer_Position,(IfTypeMotors*0x100+MotorId), DancerId, 0 ); if ( MotorControlId[MotorId] == 0xFF) return ERROR; MotorCallback[MotorId] = callback; MotorControlCallback[MotorId] = MotorMoveToDancerPositionCallBackFunction; return OK; } uint32_t MotorAbortMovetoLimitSwitch (TimerMotors_t MotorId) { ReportWithPackageFilter(GeneralFilter,"MotorAbortMovetoLimitSwitch",__FILE__,__LINE__,MotorId,RpMessage,MotorControlId[MotorId],0); if (MotorControlId[MotorId] != 0xFF) { RemoveControlCallback(MotorControlId[MotorId], MotorControlCallback[MotorId] ); MotorControlCallback[MotorId] = 0; MotorControlId[MotorId] = 0xFF; } //call driver action to device id with the parameter //SetMotorSpeed (deviceId, parameter); if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) { if (UnloadingReset) { UnloadingReset(MotorId, BUSY); UnloadingReset = NULL; } } MotorStop(MotorId,Hard_Hiz); MotorCallback[MotorId] = NULL; return OK; } float Calculate_Arm_Angle(uint32_t Drier_Center,uint32_t Current_Angle) { float angle; float Calc_angle; if (Current_Angle >= Drier_Center) angle = Current_Angle - Drier_Center; else angle = Current_Angle + 0x3FFF - Drier_Center; Calc_angle = (float)(angle/0x3FFF); MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); ReportWithPackageFilter(DiagnosticsFilter,"Calculate_Arm_Angle",__FILE__,(int)(Calc_angle*1000),Current_Angle,RpMessage,Drier_Center,0); return Calc_angle; } int Calculate_Arm_Distance(uint32_t Drier_Center,uint32_t Current_Angle) { int angle; //if (Current_Angle >= Drier_Center) angle = Current_Angle - Drier_Center; //else // angle = Current_Angle + 0x3FFF - Drier_Center; ReportWithPackageFilter(DiagnosticsFilter,"Calculate_Arm_Distance",__FILE__,angle,Current_Angle,RpMessage,Drier_Center,0); return angle; } uint32_t LoadingArmReset_Callback_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset_Callback_Stopper_Callback time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,0,0); if (UnloadingReset) { UnloadingReset(deviceID, BusyFlag); UnloadingReset = NULL; } return OK; } uint32_t LoadingArmReset_Callback(uint32_t MotorId, uint32_t ReadValue) { bool direction; int angle = 0; uint32_t temp = Read_Dryer_ENC_Position(); angle = Calculate_Arm_Distance(Arm_Drier_Center,temp); ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset_Callback",__FILE__,angle,temp,RpMessage,Arm_Drier_Center,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback details",__FILE__,__LINE__,ReadValue,RpMessage,0,0); //if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round if (ReadValue == NOTBUSY) // OK - take another round { ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset_Callback OK",__FILE__,__LINE__,Arm_Drier_Center,RpMessage,ReadValue,0); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); if (abs(angle)<400) //if (fabs(angle)<0.2) { ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Arm_Drier_Center,RpMessage,angle,0); if (angle>0) direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; else direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,LoadingArmReset_Callback_Stopper_Callback,/*3000*/16000,direction,5); } else { ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset_Callback Fail",__FILE__,__LINE__,Arm_Drier_Center,RpMessage,ReadValue,0); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); if (UnloadingReset) { UnloadingReset(MotorId, ReadValue); UnloadingReset = NULL; } } } else //timeout or no movement { ReportWithPackageFilter(DiagnosticsFilter,"Unloading drier - halted",__FILE__,__LINE__,Arm_Drier_Center,RpMessage,0,0); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm ReportWithPackageFilter(DiagnosticsFilter,"Dryer unloading timeout(1) or no movement",__FILE__,temp,Arm_Drier_Center,RpWarning,ReadValue,0); if (UnloadingReset) { UnloadingReset(MotorId, ReadValue); UnloadingReset = NULL; } } return OK; } uint32_t LoadingArmReset (callback_fptr callback,uint32_t timeout) //TODO { uint32_t temp; if (UnloadingReset == NULL) UnloadingReset = callback; else { ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset is active",__FILE__,__LINE__,timeout,RpMessage, 0,0); return ERROR; } MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); Arm_Drier_Center = temp; ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset",__FILE__,__LINE__,timeout,RpMessage, 0,0); MotorRunWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius, LoadingArmReset_Callback, 300000); return OK; } uint32_t LoadingStopArmReset(void) { return MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); } void MotorActionsInit(void) { int i; for (i=0;i