diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded')
14 files changed, 635 insertions, 287 deletions
diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c index bf2c4e98d..a607fedcb 100644 --- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c +++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c @@ -20,7 +20,7 @@ typedef struct } TangoVersion_t; -TangoVersion_t _gTangoVersion = {1,4,6,44}; +TangoVersion_t _gTangoVersion = {1,5,0,1}; #define BUILD_DATE __DATE__ char Dat[50] = BUILD_DATE; char _gTangoName [MAX_STRING_LEN] = "Tango01 ";//e diff --git a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.c b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.c index b151bea09..3a3aadd3b 100644 --- a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.c +++ b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.c @@ -52,7 +52,7 @@ void progress_request__free_unpacked assert(message->base.descriptor == &progress_request__descriptor); protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); } -static const ProtobufCFieldDescriptor progress_request__field_descriptors[2] = +static const ProtobufCFieldDescriptor progress_request__field_descriptors[5] = { { "", @@ -78,15 +78,54 @@ static const ProtobufCFieldDescriptor progress_request__field_descriptors[2] = 0, /* flags */ 0,NULL,NULL /* reserved1,reserved2, etc */ }, + { + "", + 3, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT32, + offsetof(ProgressRequest, has_param1), + offsetof(ProgressRequest, param1), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "", + 4, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT32, + offsetof(ProgressRequest, has_param2), + offsetof(ProgressRequest, param2), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "", + 5, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT32, + offsetof(ProgressRequest, has_param3), + offsetof(ProgressRequest, param3), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, }; static const unsigned progress_request__field_indices_by_name[] = { 0, /* field[0] = Amount */ 1, /* field[1] = Delay */ + 2, /* field[2] = Param1 */ + 3, /* field[3] = Param2 */ + 4, /* field[4] = Param3 */ }; static const ProtobufCIntRange progress_request__number_ranges[1 + 1] = { { 1, 0 }, - { 0, 2 } + { 0, 5 } }; const ProtobufCMessageDescriptor progress_request__descriptor = { @@ -96,7 +135,7 @@ const ProtobufCMessageDescriptor progress_request__descriptor = "", "", sizeof(ProgressRequest), - 2, + 5, progress_request__field_descriptors, progress_request__field_indices_by_name, 1, progress_request__number_ranges, diff --git a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.h b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.h index 1ce2b6d6b..da8eac4f1 100644 --- a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.h +++ b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressRequest.pb-c.h @@ -30,10 +30,16 @@ struct _ProgressRequest int32_t amount; protobuf_c_boolean has_delay; int32_t delay; + protobuf_c_boolean has_param1; + uint32_t param1; + protobuf_c_boolean has_param2; + uint32_t param2; + protobuf_c_boolean has_param3; + uint32_t param3; }; #define PROGRESS_REQUEST__INIT \ { PROTOBUF_C_MESSAGE_INIT (&progress_request__descriptor) \ - , 0, 0, 0, 0 } + , 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } /* ProgressRequest methods */ diff --git a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.c b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.c index 537f8e763..ff0667e61 100644 --- a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.c +++ b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.c @@ -52,7 +52,7 @@ void progress_response__free_unpacked assert(message->base.descriptor == &progress_response__descriptor); protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); } -static const ProtobufCFieldDescriptor progress_response__field_descriptors[1] = +static const ProtobufCFieldDescriptor progress_response__field_descriptors[2] = { { "", @@ -66,14 +66,27 @@ static const ProtobufCFieldDescriptor progress_response__field_descriptors[1] = 0, /* flags */ 0,NULL,NULL /* reserved1,reserved2, etc */ }, + { + "", + 2, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT32, + offsetof(ProgressResponse, has_info), + offsetof(ProgressResponse, info), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, }; static const unsigned progress_response__field_indices_by_name[] = { + 1, /* field[1] = Info */ 0, /* field[0] = Progress */ }; static const ProtobufCIntRange progress_response__number_ranges[1 + 1] = { { 1, 0 }, - { 0, 1 } + { 0, 2 } }; const ProtobufCMessageDescriptor progress_response__descriptor = { @@ -83,7 +96,7 @@ const ProtobufCMessageDescriptor progress_response__descriptor = "", "", sizeof(ProgressResponse), - 1, + 2, progress_response__field_descriptors, progress_response__field_indices_by_name, 1, progress_response__number_ranges, diff --git a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.h b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.h index de5201ee5..bd1a3141a 100644 --- a/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.h +++ b/Software/Embedded_SW/Embedded/Communication/PMR/Stubs/ProgressResponse.pb-c.h @@ -28,10 +28,12 @@ struct _ProgressResponse ProtobufCMessage base; protobuf_c_boolean has_progress; double progress; + protobuf_c_boolean has_info; + uint32_t info; }; #define PROGRESS_RESPONSE__INIT \ { PROTOBUF_C_MESSAGE_INIT (&progress_response__descriptor) \ - , 0, 0 } + , 0, 0, 0, 0 } /* ProgressResponse methods */ diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c index f8e8d7b9e..6e38cc68a 100644 --- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c +++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c @@ -443,6 +443,7 @@ bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch) case I2C_HEADCARD_COVER_LS_FRONT: case I2C_HEADCARD_ARC_LS_ACTUATOR: LM_Status = !(Head_I2C_EXP4_0x46.bits.INPUT_LS_FRONT_ARC_ACT); + LM_Status -= 1; break; case I2C_HEADCARD_COVER_LS_REAR: case I2C_HEADCARD_COVER_LS_ARC: diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h index e6947a8d4..00cada255 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h @@ -312,6 +312,7 @@ uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType); uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps); uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout); +uint32_t MotorRunWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Freq, callback_fptr callback,uint32_t timeout); uint32_t MotorGotoWithCallback (TimerMotors_t MotorId, uint32_t Steps, uint32_t LimitSwitchId,callback_fptr callback,uint32_t timeout); //TODO uint32_t MotorGotoWithBusyCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout); //TODO @@ -352,7 +353,12 @@ uint32_t MotorMovetoBreakSensor (TimerMotors_t _motorId,bool direction, uint32_t uint32_t MotorMovetoDancerPosition (TimerMotors_t _motorId,bool direction, uint32_t Freq,uint32_t DancerId,bool dancervaluedirection, callback_fptr callback,uint32_t timeout); uint32_t MotorAbortMovetoLimitSwitch (TimerMotors_t _motorId); uint32_t MotorMoveToStopper (TimerMotors_t _motorId,bool direction, uint32_t Speed, callback_fptr callback,uint32_t backlash,uint32_t timeout); -uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callback,uint32_t timeout, bool direction); +uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callback,uint32_t timeout, bool direction,uint32_t speed); +uint32_t LoadingArmReset (callback_fptr callback,uint32_t timeout); +uint32_t LoadingStopArmReset (void); + +float Calculate_Arm_Angle(uint32_t Drier_Center,uint32_t Current_Angle); +int Calculate_Arm_Distance(uint32_t Drier_Center,uint32_t Current_Angle); uint32_t MotorStopAction (TimerMotors_t MotorId); //Stop the controlled action of a motor diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c index f33edee50..9e8ef25fc 100644 --- a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c +++ b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c @@ -39,11 +39,26 @@ //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; +/*int CallbackCalls=0; +int FirstCall = 0; +#define MAX_CONTROL_SAMPLES 10*/ +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]; @@ -206,6 +221,172 @@ uint32_t MotorGotoWithBusyCallback (TimerMotors_t MotorId,bool direction, uint32 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; + 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 BusyFlag) //TODO +{ + uint32_t MotorId,encoder,temp = 0; + uint32_t Busy = BusyFlag; + 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 (InitialArmLocation<encoder)//rollover + { + temp = 0x3FFF + InitialArmLocation - encoder; + if ((temp>25)&&(temp<1000)) + { + AccumulatedArmMovement+=temp; + InitialArmLocation=encoder; + } + } + } + else //loading - going up + { + //previous number is smaller than current + if (InitialArmLocation<encoder) + { + temp = encoder-InitialArmLocation; + if ((temp>25)&&(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("MotorRunCallBackFunctionMotorRunCallBackFunction temp curr prev small",__FILE__,encoder,InitialArmLocation,RpWarning,temp,0); + failCounter++; + if (failCounter>=20) + { + Report("arm stopped",__FILE__,failCounter,encoder,RpWarning,temp,0); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Hiz); + BusyFlag = NOTBUSY; + } + } + else + { + failCounter = 0; + BusyFlag = BUSY; + } + if (CallbackCalls%50 == 0) + { + //encoder = Read_Dryer_ENC_Position(0,0); + Report("MotorRunCallBackFunctionMotorRunCallBackFunction periodic curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); + } + if (CallbackCalls%200 == 0) + { + //encoder = Read_Dryer_ENC_Position(0,0); + Report("MotorRunCallBackFunctionMotorRunCallBackFunction",__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 ((BusyFlag == NOTBUSY)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) + { + if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) + { + angle = Calculate_Arm_Distance(Arm_Drier_Center,temp); + + if ((angle<400)||(angle>16000)) + { + BusyFlag = NOTBUSY; + } + else + { + BusyFlag = BUSY; + } + Report("arm stopped call",__FILE__,failCounter,encoder,RpWarning,abs(DrierZeroPosition-encoder),0); + } + //stop this control loop + Report("MotorControlGetnBusyState stop",__FILE__,MotorId,BusyFlag,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); + Busy = BUSY; + } + Report("MotorRunCallBackFunctionMotorRunCallBackFunction curr prev return",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); + //call the module callback + if (MotorCallback[MotorId]) + { + MotorCallback[MotorId](MotorId,Busy); + } + + } + return OK; +} + uint32_t MotorMoveWithCallback (TimerMotors_t MotorId,bool direction, uint32_t Steps, callback_fptr callback,uint32_t timeout) //TODO { @@ -224,31 +405,28 @@ uint32_t MotorMoveWithCallback (TimerMotors_t MotorId,bool direction, uint32_t S MotorTimeout[MotorId] = 0; MotorTimeLag[MotorId] = 20; MotorTimeLimit[MotorId] = timeout; - if (MotorId ==HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) + /*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); } - - //MotorStop(MotorId,Hard_Hiz ); - MotorMove(MotorId,direction,Steps ); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveCallBackFunction, /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveCallBackFunction; return MotorControlId[MotorId]; } -uint32_t DryerBackLash = 0; -uint32_t DrierZeroPosition = 0; -bool DrierBackLashDirection = false; -int CallbackCalls=0; -int FirstCall = 0; -#define MAX_CONTROL_SAMPLES 10 -int32_t ArmSamples[MAX_CONTROL_SAMPLES] = {0}; -int ArmSamplePointer = {0}; -int StoredavreageSampleValue = 0; - -int32_t Initialcurrentposition =0; -int cycles = 0; uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t ArmPosition) //TODO { uint32_t MotorId; @@ -262,6 +440,7 @@ uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t ArmPosition) / MotorId = IfIndex&0xFF; MotorPosition = ArmPosition; CallbackCalls++; + if (CallbackCalls%20 == 0) { // REPORT_MSG(MotorPosition,"MotorVerifiedCallBackFunction"); @@ -276,11 +455,6 @@ uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t ArmPosition) / Read_Dryer_ENC_Position(); //trigger the next call ArmSamples[ArmSamplePointer] = MotorPosition;//(-1 * TranslatedReadValue); - if (FirstCall<5) - { - FirstCall++; - return OK; - } ArmSamples[ArmSamplePointer] = MotorPosition;//(-1 * TranslatedReadValue); ArmSamplePointer++; if (ArmSamplePointer >= MAX_CONTROL_SAMPLES) @@ -320,13 +494,13 @@ uint32_t MotorVerifiedCallBackFunction(uint32_t IfIndex, uint32_t ArmPosition) / Task_sleep(5);*/ // MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CENTER,DrierZeroPosition); - Report("MotorVerifiedCallBackFunction",__FILE__,__LINE__,DrierZeroPosition,RpWarning,DryerBackLash,0); + 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); + 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 } @@ -359,12 +533,8 @@ uint32_t MotorMoveToStopper (TimerMotors_t MotorId,bool direction, uint32_t Spee MotorStop(MotorId,Hard_Hiz ); Task_sleep(5); - DryerBackLash = backlash; - DrierBackLashDirection = 1-direction; MotorSetDirection( MotorId, direction); - CallbackCalls = 0; - FirstCall = 0; MotorGetPositionFromFPGA(MotorId); memset(ArmSamples,0,sizeof(ArmSamples)); ArmSamplePointer = 0; @@ -422,11 +592,14 @@ uint32_t MotorMoveToDrierPositionCallBackFunction(uint32_t IfIndex, uint32_t Rea //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 MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callback,uint32_t timeout, bool direction,uint32_t speed) { //assert (callback); uint32_t currentposition = 0; @@ -446,6 +619,7 @@ uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callb 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__,callback,RpWarning,speed,0); if (abs(currentposition - DrierZeroPosition)<5) { if (callback) @@ -456,7 +630,7 @@ uint32_t MotorMovetoEncoderPosition (TimerMotors_t MotorId, callback_fptr callb //MotorSetDirection( MotorId, direction); MotorSetDirection(MotorId,direction); - MotorSetSpeed (MotorId, 30); + MotorSetSpeed (MotorId, speed); MotorControlId[MotorId] = AddControlCallback(NULL, MotorMoveToDrierPositionCallBackFunction, 2,Control_Read_Dryer_Position,(IfTypeMotors*0x100+MotorId), MotorId, 0 ); MotorControlCallback[MotorId] = MotorMoveToDrierPositionCallBackFunction; MotorTimeout[MotorId] = 0; @@ -480,78 +654,22 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t MotorId, uint32_t _freq, callb } uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO { - uint32_t MotorId,encoder,temp = 0,tt; + 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) { - encoder = Read_Dryer_ENC_Position(0,0); - tt=InitialArmLocation; - 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 (InitialArmLocation<encoder)//rollover - { - temp = 0x3FFF + InitialArmLocation - encoder; - if ((temp>25)&&(temp<1000)) - { - AccumulatedArmMovement+=temp; - InitialArmLocation=encoder; - InitialArmLocation=encoder; - } - } - } - else //loading - going up - { - //previous number is smaller than current - if (InitialArmLocation<encoder) - { - temp = encoder-InitialArmLocation; - if ((temp>25)&&(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) - { - Report("MotorMoveCallBackFunction temp curr prev total",__FILE__,encoder,tt,RpWarning,temp,0); - }*/ - if (CallbackCalls%50 == 0) - { - //encoder = Read_Dryer_ENC_Position(0,0); - Report("MotorMoveCallBackFunction periodic curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); - } if (CallbackCalls%200 == 0) { - //encoder = Read_Dryer_ENC_Position(0,0); - Report("MotorMoveCallBackFunction",__FILE__,__LINE__,encoder,RpWarning,CallbackCalls,0); + Report("MotorMoveCallBackFunction",__FILE__,__LINE__,MotorId,RpWarning,Read_Dryer_ENC_Position(0,0),0); } } else @@ -565,10 +683,6 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO if ((BusyFlag == NOTBUSY)||((MotorTimeout[MotorId]>=MotorTimeLimit[MotorId])&&(MotorTimeLimit[MotorId]>0))) { - /*if (MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM) - { - printBusy = false; - }*/ //stop this control loop Report("MotorControlGetnBusyState stop",__FILE__,MotorId,BusyFlag,RpMessage,MotorTimeout[MotorId],0); SafeRemoveControlCallback(MotorControlId[MotorId], MotorMoveCallBackFunction ); @@ -581,7 +695,6 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO Report("motor timeout",__FILE__,__LINE__,MotorId,RpMessage,MotorTimeout[MotorId],0); Busy = BUSY; } - Report("MotorMoveCallBackFunction curr prev total",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0); //call the module callback if (MotorCallback[MotorId]) { @@ -780,7 +893,9 @@ uint32_t MotorMovetoLimitSwitch (TimerMotors_t MotorId,bool direction, uint32_t ReportWithPackageFilter(GeneralFilter,"calling DeActivateHeadMagnet",__FILE__,timeout,direction,RpMessage,Freq,0); if (direction == MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize) { - CloseMagnet = true; + if (Head_Type != HEAD_TYPE_ARC) { + CloseMagnet = true; + } /*if (Head_Type == HEAD_TYPE_FLAT) { Report("Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0); @@ -977,6 +1092,14 @@ uint32_t MotorAbortMovetoLimitSwitch (TimerMotors_t MotorId) } //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; @@ -984,7 +1107,121 @@ uint32_t MotorAbortMovetoLimitSwitch (TimerMotors_t MotorId) 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__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,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 ((angle<400)||(angle>16000)) + //if (fabs(angle)<0.2) + { + ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Arm_Drier_Center,RpMessage,angle,0); + if (angle<400) + 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,10); + + } + 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); + //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm + + ReportWithPackageFilter(DiagnosticsFilter,"Drier 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; diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c index b40bbe4c7..9eba08678 100644 --- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c @@ -133,7 +133,7 @@ uint32_t LoadArmRounds; uint32_t D_numberOfSteps = 0; float D_numberOfCycles = 0; -double D_DrierPrevLocation = 0; +uint32_t D_DrierPrevLocation = 0; uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer) @@ -465,30 +465,33 @@ return OK; /******************************************************************************** * Drier Loading Arm Homing ********************************************************************************/ -uint32_t Drier_Center = 0; -float Calculate_Arm_Angle(uint32_t Current_Angle) -{ - uint32_t angle; - float Calc_angle; - if (Current_Angle >= Drier_Center) - angle = Current_Angle - Drier_Center; - else - angle = Current_Angle + 0x3FFF - Drier_Center; - Calc_angle = 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; -} +uint32_t Drier_Center_read = 0; uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { MessageContainer responseContainer; MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT; + uint32_t angle, temp = Read_Dryer_ENC_Position(); + angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp); + + if ((angle<14000 )||(ReadValue == BUSY)) // OK - take another round + { + responseContainer.has_error = true; + responseContainer.error = ERROR_CODE__GENERAL_ERROR; + + } ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Stopper time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,0,0); + if (HomingControlId[deviceID] != 0xff) + { + ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0); + RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport); + HomingControlId[deviceID] = 0xff; + } //NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + ThreadLoadingRestartReport(); responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size); responseContainer.has_continuous = true; responseContainer.continuous = true; @@ -501,45 +504,50 @@ uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_ return OK; } -uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) +/*uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { + bool direction; + D_numberOfCycles++; uint32_t temp = Read_Dryer_ENC_Position(); ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0); - if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round -// if (ReadValue == NOTBUSY) // OK - take another round + //if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round + if (ReadValue == NOTBUSY) // OK - take another round { - D_DrierPrevLocation = temp; ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading cycles",__FILE__,D_numberOfCycles,LoadArmRounds,RpMessage,0,0); - if (D_numberOfCycles<LoadArmRounds) + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + if (abs(temp -Drier_Center_read)<200) { - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps, Diagnostics_Dryer_UnLoading_Callback, 10000); - } - else //done enough cycles, go to the center point - { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,15000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles); - ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Drier_Center_read,RpMessage,abs(temp -Drier_Center_read),0); + if (temp<Drier_Center_read) + 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,Diagnostics_Set_Load_Arm_To_Stopper_Callback,3000,direction,10); + } + else + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); + } else //timeout or no movement { ReportWithPackageFilter(DiagnosticsFilter,"Unloading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm + //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm ReportWithPackageFilter(DiagnosticsFilter,"Drier unloading timeout(1) or no movement",__FILE__,temp,D_DrierPrevLocation,RpWarning,ReadValue,0); + Diagnostics_Set_Load_Arm_To_Stopper_Callback(MotorId, ReadValue); } return OK; -} +}*/ uint32_t Diagnostics_Dryer_UnLoading(void) { - uint32_t temp; + D_DrierPrevLocation = Read_Dryer_ENC_Position(); - D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - D_numberOfCycles = 0; + LoadingArmReset(Diagnostics_Set_Load_Arm_To_Stopper_Callback,300000); + /*uint32_t temp; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped @@ -549,16 +557,21 @@ uint32_t Diagnostics_Dryer_UnLoading(void) if (LoadArmRounds <= 2) MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); + D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + D_numberOfCycles = 0; + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); - Drier_Center = temp; + Drier_Center_read = temp; D_DrierPrevLocation = temp; ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_DrierPrevLocation,RpMessage, LoadArmRounds,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); + // status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), // MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Diagnostics_Set_Load_Arm_To_Stopper_Callback,0,1000); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps-800, Diagnostics_Dryer_UnLoading_Callback, 10000); - + //MotorRunWithCallback + MotorRunWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius, Diagnostics_Dryer_UnLoading_Callback, 300000); +*/ return OK; } uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) @@ -566,7 +579,7 @@ uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint MessageContainer responseContainer; MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT; - ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,0,RpMessage,CallbackCounter,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[MotorId], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size); @@ -584,12 +597,15 @@ uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { D_numberOfCycles++; + uint32_t angle, temp = Read_Dryer_ENC_Position(); - ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)D_numberOfCycles,(int)D_DrierPrevLocation,RpMessage,ReadValue,0); + angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp); + ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)angle,(int)ReadValue,RpMessage,temp,0); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles); - if (ReadValue == NOTBUSY) + if ((angle>14000 )&&(ReadValue == NOTBUSY)) // OK - take another round { + D_DrierPrevLocation = temp; //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); if (D_numberOfCycles<LoadArmRounds) @@ -599,14 +615,14 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue } else { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); } } else { ReportWithPackageFilter(DiagnosticsFilter,"Loading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); } return OK; @@ -620,17 +636,17 @@ uint32_t Diagnostics_Dryer_Loading(void) LoadArmRounds = (int)dryerbufferlength; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&D_DrierPrevLocation); //MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); - //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - Drier_Center = D_DrierPrevLocation; + ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,D_DrierPrevLocation,D_numberOfCycles,RpMessage,LoadArmRounds,0); + Drier_Center_read = D_DrierPrevLocation; D_numberOfCycles = 0; D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - + D_numberOfSteps -= 100; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); //6 seconds per round //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps-800, Diagnostics_Dryer_Loading_Callback, 10000); + D_numberOfSteps, Diagnostics_Dryer_Loading_Callback, 10000); return OK; } diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.c b/Software/Embedded_SW/Embedded/Modules/General/buttons.c index 328ff3203..569071c3a 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/buttons.c +++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.c @@ -402,7 +402,10 @@ uint32_t ButtonLoadCBFunction(uint32_t IfIndex, uint32_t ReadValue) case LONGPB: ReportWithPackageFilter(GeneralFilter,"------------ thread loading LONGPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0); if (ThreadLoadingActive() == false) - ThreadLoadButton(0); //INIT + { + ReportWithPackageFilter(GeneralFilter,"------------ NOT starting loading from button -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0); + ThreadLoadButton(0); //INIT CHANGE HERE FOR 1.4.6.44 + } else Thread_Load_End(); //END break; diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c index 3c7248204..a6b7bff86 100644 --- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c @@ -218,13 +218,12 @@ void initializeArrays(void) HeaterId2PT100Id[HEATER_TYPE__HeaterZone1] = HEAD_PT100_ZONE_1_0X80_0; HeaterId2PT100Id[HEATER_TYPE__HeaterZone2] = HEAD_PT100_ZONE_2_0X80_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone3] = HEAD_PT100_ZONE_3_0X82_0; - /* - HeaterId2PT100Id[HEATER_TYPE__HeaterZone4] = HEAD_PT100_ZONE_4_0X82_1; + //HeaterId2PT100Id[HEATER_TYPE__HeaterZone4] = HEAD_PT100_ZONE_4_0X82_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone5] = HEAD_PT100_ZONE_5_0X84_0; - HeaterId2PT100Id[HEATER_TYPE__HeaterZone6] = HEAD_PT100_ZONE_6_0X84_1;*/ + //HeaterId2PT100Id[HEATER_TYPE__HeaterZone6] = HEAD_PT100_ZONE_6_0X84_1;*/ HeaterId2PT100Id[HEATER_TYPE__MixerHeater] = HEAD_PT100_MIXER_0X8E_0; - /*HeaterId2PT100Id[HEATER_TYPE__HeaterZone7] = HEAD_PT100_ZONE_7_0X86_0; - HeaterId2PT100Id[HEATER_TYPE__HeaterZone8] = HEAD_PT100_ZONE_8_0X86_1; + HeaterId2PT100Id[HEATER_TYPE__HeaterZone7] = HEAD_PT100_ZONE_7_0X86_0; + /*HeaterId2PT100Id[HEATER_TYPE__HeaterZone8] = HEAD_PT100_ZONE_8_0X86_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone9] = HEAD_PT100_ZONE_9_0X88_0; HeaterId2PT100Id[HEATER_TYPE__HeaterZone10] = HEAD_PT100_ZONE_10_0X88_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone11] = HEAD_PT100_ZONE_11_0X8A_0; @@ -239,13 +238,12 @@ void initializeArrays(void) HeaterId2CurrentId[HEATER_TYPE__HeaterZone1] = HEAD_CURRENT_ZONE_1; HeaterId2CurrentId[HEATER_TYPE__HeaterZone2] = HEAD_CURRENT_ZONE_2; HeaterId2CurrentId[HEATER_TYPE__HeaterZone3] = HEAD_CURRENT_ZONE_3; - /* - HeaterId2CurrentId[HEATER_TYPE__HeaterZone4] = HEAD_CURRENT_ZONE_4; + //HeaterId2CurrentId[HEATER_TYPE__HeaterZone4] = HEAD_CURRENT_ZONE_4; HeaterId2CurrentId[HEATER_TYPE__HeaterZone5] = HEAD_CURRENT_ZONE_5; - HeaterId2CurrentId[HEATER_TYPE__HeaterZone6] = HEAD_CURRENT_ZONE_6;*/ + //HeaterId2CurrentId[HEATER_TYPE__HeaterZone6] = HEAD_CURRENT_ZONE_6; HeaterId2CurrentId[HEATER_TYPE__MixerHeater] = HEAD_CURRENT_MIXER; - /*HeaterId2CurrentId[HEATER_TYPE__HeaterZone7] = HEAD_CURRENT_ZONE_7; - HeaterId2CurrentId[HEATER_TYPE__HeaterZone8] = HEAD_CURRENT_ZONE_8; + HeaterId2CurrentId[HEATER_TYPE__HeaterZone7] = HEAD_CURRENT_ZONE_7; + /*HeaterId2CurrentId[HEATER_TYPE__HeaterZone8] = HEAD_CURRENT_ZONE_8; HeaterId2CurrentId[HEATER_TYPE__HeaterZone9] = HEAD_CURRENT_ZONE_9; HeaterId2CurrentId[HEATER_TYPE__HeaterZone10] = HEAD_CURRENT_ZONE_10; HeaterId2CurrentId[HEATER_TYPE__HeaterZone11] = HEAD_CURRENT_ZONE_11; diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c index 8856d873a..5da52f590 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c @@ -66,6 +66,7 @@ extern uint16_t Head_Fan_Tach[2]; extern bool doorState; bool RestartNeeded = false; +uint32_t temp1,temp2; void Stub_ProgressRequest(MessageContainer* requestContainer) { @@ -1105,6 +1106,42 @@ void Stub_ProgressRequest(MessageContainer* requestContainer) response.has_progress = true; } else + if(request->amount == 0xB11) //fast refresh for pressure + { + temp1 = Read_Dryer_ENC_Position(); + temp2 = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius/4; + + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Hiz); + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,200); + //6 seconds per round + Report("Arm movement test",__FILE__,request->delay,request->delay/2*2,RpWarning,(int)request->delay%2,0); + + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, request->delay%2,request->delay/2*2, NULL, 12000); + + response.progress = temp2; + response.has_progress = true; + } + else + if(request->amount == 0xB12) //fast refresh for pressure + { + + temp2 = Calculate_Arm_Distance(temp1,Read_Dryer_ENC_Position()); + Report("Arm movement test calc",__FILE__,temp2,temp1,RpWarning,Read_Dryer_ENC_Position(),0); + + response.progress = temp2; + response.has_progress = true; + } + else + if(request->amount == 0xB13) //TryThreadLoadingRequest + { + TryThreadLoadingFunc(NULL); + Report("TryThreadLoadingFunc",__FILE__,temp2,temp1,RpWarning,Read_Dryer_ENC_Position(),0); + + response.progress = 0xB13; + response.has_progress = true; + } + else if(request->amount == 0xC3) //suspend I2C task { if (request->delay == 0) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index cd7c146fe..d16683967 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -126,6 +126,7 @@ uint32_t Thread_Load_Dryer_UnLoading(void); uint32_t ThreadLoadingReport(void); + uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage bool ThreadLoaded(void); bool ThreadLoadingActive(void) { @@ -245,38 +246,41 @@ } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { + uint32_t angle, temp = Read_Dryer_ENC_Position(); - /*if(PullerControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - }*/ - if(WinderControlId != 0xFF) + ReportWithPackageFilter(DiagnosticsFilter,"Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,(int)numberOfCycles,(int)DrierPrevLocation,RpMessage,temp,0); + + angle = Calculate_Arm_Distance(DrierPrevLocation,temp); + + DrierPrevLocation = temp; + if (CallbackCounter) { - MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); - RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); - WinderControlId = 0xFF; + CallbackCounter--; } + if ((angle>14000 )&&(BusyFlag == NOTBUSY)) // OK - take another round + { + Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); - MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - /*Task_sleep(5) - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ + Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + NumberOfDrierLoaderCycles=0; + //storeLoadArmParameters(); + LoadStages++; - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); + ThreadLoadStateMachine(LoadStages); + } + else + { + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM]); + Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); + + LoadStatus = ERROR; + ThreadLoadingReport(); + TryAgain = true; + //ThreadLoadButton(LoadStages); + } //move to exact location? // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - CallbackCounter = 0; - Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); - - Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - NumberOfDrierLoaderCycles=0; - //storeLoadArmParameters(); - LoadStages++; - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home - - ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) @@ -484,6 +488,8 @@ 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); + Report("Thread_Load_Close dancers tension",__FILE__,__LINE__,(int)windertension,RpMessage,(int)pullertension,0); + // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,¤t); @@ -666,7 +672,7 @@ } else { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } @@ -826,6 +832,7 @@ ThreadLoadControlId = 0xFF; } + LoadingStopArmReset(); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING); @@ -860,7 +867,7 @@ return OK; } - uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) + /*uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { bool direction; numberOfCycles++; @@ -884,7 +891,7 @@ } else //done enough cycles, go to the center point { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } @@ -902,7 +909,7 @@ direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; else direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction,10); } else @@ -911,12 +918,6 @@ 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); @@ -925,9 +926,6 @@ } 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; @@ -940,79 +938,24 @@ //ThreadLoadButton(LoadStages); } return OK; - } + }*/ uint32_t Thread_Load_Dryer_UnLoading(void) { uint32_t temp; REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading"); //LoadArmRounds = 0; //uint32_t numberOfSteps = 0; - //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 - SetOriginMotorSpeed(22); -// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; -// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; - -// Rockers are up already, so puller handling is not needed - //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1; - MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1; - MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime; - MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain; - MotorControlConfig[POOLER_MOTOR].m_params.Ki = 0;//MotorsControl[POOLER_MOTOR].integraltime; - MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp; - MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue; - MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon; - MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype; - MotorControlConfig[POOLER_MOTOR].m_ingnoreValue = MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0; - MotorControlConfig[POOLER_MOTOR].m_integral = 0; - MotorControlConfig[POOLER_MOTOR].m_isEnabled = true; - MotorControlConfig[POOLER_MOTOR].m_isReady = true; - MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0; - MotorControlConfig[POOLER_MOTOR].m_preError = 0; - MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize); - - 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);*/ -//////////////////////// - MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1; - MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1; - MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime; - MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain; - MotorControlConfig[WINDER_MOTOR].m_params.Ki = 0;//MotorsControl[WINDER_MOTOR].integraltime; - MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp; - MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue; - MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon; - MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype; - MotorControlConfig[WINDER_MOTOR].m_ingnoreValue = MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0; - MotorControlConfig[WINDER_MOTOR].m_integral = 0; - MotorControlConfig[WINDER_MOTOR].m_isEnabled = true; - MotorControlConfig[WINDER_MOTOR].m_isReady = true; - MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0; - MotorControlConfig[WINDER_MOTOR].m_preError = 0; - MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize); - - 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); - -//////////////////////// - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); CallbackCounter++; - //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - // numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); - //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); - //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); -//shlomo - UnloadingStart = msec_millisecondCounter; - numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - numberOfCycles = 0; - if (SecondTry == true) + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + DrierCenterLocation = DrierPrevLocation; + DrierPrevLocation = temp; + UnloadingStart = msec_millisecondCounter; + //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + //numberOfCycles = 0; + LoadingArmReset(Thread_Load_Set_Load_Arm_To_Stopper_Callback,300000); + /*if (SecondTry == true) { MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped @@ -1028,8 +971,8 @@ { LoadArmRounds = (int)dryerbufferlength; } - } - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + }*/ + /*MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); DrierCenterLocation = DrierPrevLocation; DrierPrevLocation = temp; Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0); @@ -1046,6 +989,8 @@ //Keep Notation How Many Rotations In The Dryer //LoadArmRounds = (int)dryerbufferlength; + * + */ load.color = BLINK; return OK; } @@ -1172,6 +1117,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) LoadStatus = OK; SecondTry = true; load.color = BLINK; + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); ThreadLoadStateMachine(LoadStages); } @@ -1180,6 +1127,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) SecondTry = false; Report("Calling State machine 2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); load.color = BLINK; + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages+1); } else //((SecondTry == false)&&(TryAgain == false))??????? @@ -1191,6 +1140,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) else { Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0); + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages); SecondTry = false; } @@ -1521,22 +1472,53 @@ uint32_t ThreadLoadingReport(void) return OK; } +bool ThreadLoadingRestartFlag = false; +uint32_t ThreadLoadingRestartReport(void) //sending after a failure in the finalizing stage +{ + MessageContainer responseContainer; + StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; + + if (ThreadLoadingToken[0] == 0) + return OK; + ThreadLoadingRestartFlag = true; + + response.has_state = true; + response.state = THREAD_LOADING_STATE__ReadyForLoading; + response.errorreason = DefaultErrSrt; + Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); + //------------------------------------------------------------------------------------------- + responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, false, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); + responseContainer.has_continuous = true; + responseContainer.continuous = true; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + + return OK; + +} uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) { - TryThreadLoadingRequest *request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + TryThreadLoadingRequest *request; TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT; MessageContainer responseContainer; - MessageState = 2; + //MessageState = 2; + if (requestContainer) + request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ThreadLoadingReport(); Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0); - responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_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 (requestContainer) + { + responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size); + responseContainer.continuous = false; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + } ThreadLoadButton(LoadStages); return OK; @@ -1544,7 +1526,8 @@ uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) { - ThreadLoadingReport(); + if(ThreadLoadingActive()) + ThreadLoadingReport(); return OK; } @@ -1567,13 +1550,16 @@ uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) MessageContainer responseContainer; MessageState = 2; ThreadLoadingReport(); - if (request->processparameters) + if (ThreadLoadingRestartFlag == false) { - dryerbufferlength = request->processparameters->dryerbufferlength; - LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + if (request->processparameters) + { + dryerbufferlength = request->processparameters->dryerbufferlength; + LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + } + Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); } - 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; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 275e7e5b7..d7bfb1b6e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -162,7 +162,7 @@ int FeederSpeedIndex = 0,Speed_i; void ThreadUpdateProcessLength (double length, void *Funcptr) { - REPORT_MSG(length,"ThreadUpdateProcessLength"); + ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; @@ -337,7 +337,10 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) }**/ //} - +#ifdef FOUR_WINDERS + if (CurrentControlledSpeed[WINDER_MOTOR]>100) + length = dyeingspeed/10; +#endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION @@ -642,11 +645,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { c++; } - if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)) + //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR)) + /*if (index >= WINDER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - } + }*/ #endif if (index == POOLER_MOTOR) { @@ -738,12 +742,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; //if (0)//(JobCounter % 1000 == 0) #ifdef FOUR_WINDERS - if (JobCounter % 500 < 4)//(FirstCalcInJob == true) + if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) { if (index >= WINDER_MOTOR) { // FirstCalcInJob = false; - len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index, + len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1, TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); |