diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
3 files changed, 101 insertions, 13 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 230d06601..e927ae436 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -9,6 +9,7 @@ #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include "PMR/Hardware/HardwareWinder.pb-c.h" #include "PMR/Hardware/HardwarePidControl.pb-c.h" +#include "PMR/Hardware/HardwareConfiguration.pb-c.h" #include "PMR/Printing/JobSpool.pb-c.h" #include "PMR/Printing/JobSpoolType.pb-c.h" @@ -73,7 +74,7 @@ extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM]; uint32_t InternalWinderConfigMessage(HardwareWinder* request); -uint32_t MotorsConfigMessage(HardwareMotor * request); +uint32_t MotorsConfigMessage( HardwareConfiguration* HW_request); uint32_t InternalWindingConfigMessage(JobSpool* request); uint32_t ThreadInitialTestStub(HardwareMotor * request); uint32_t MotorPidRequestMessage(HardwarePidControl* request); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 66d6baed5..9ef7f3d0d 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -62,6 +62,7 @@ uint32_t ControlId = 0xFF; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages); uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue); + uint32_t Thread_Load_Dryer_UnLoading(void); typedef struct { @@ -465,6 +466,74 @@ REPORT_MSG(LoadStages,"Loading Ended"); return OK; } + uint32_t Thread_Load_Dryer_UnLoading(void) + { + REPORT_MSG(LoadStages, "Thread UnLoad State Machine step"); + LoadArmInfo.LoadArmRounds = 0; + uint32_t numberOfSteps = 0; + //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 + SetOriginMotorSpeed(20); +// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; +// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; + + numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1; + MotorControlConfig[POOLER_MOTOR].m_params.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 = 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); + + ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),FEEDER_DANCER,POOLER_MOTOR); +//////////////////////// + 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 = 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_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize); + + ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),FEEDER_DANCER,WINDER_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("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); + + //Keep Notation How Many Rotations In The Dryer + //LoadArmInfo.LoadArmBackLash = 0; + LoadArmInfo.LoadArmRounds = 0xFF; + FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath); + return OK; + } uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue) { @@ -481,7 +550,8 @@ break; case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY //LoadStages++; - Thread_Load_Set_Load_Arm_To_Start_Position(); + //Thread_Load_Set_Load_Arm_To_Start_Position(); + Thread_Load_Dryer_UnLoading(); break; case THREAD_LOAD_CENTER_HEAD_ROCKERS: //LoadStages++; @@ -570,11 +640,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) return 0xFFFFFFFF; } index = IfIndex&0xFF; - /*if (index == POOLER_MOTOR) + if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - }*/ + } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -589,10 +659,10 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) MotorControlConfig[index].m_mesuredParam = NormalizedError; MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); - /*if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit + if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); - }*/ + } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) { diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 529e4d260..78203b236 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -40,11 +40,20 @@ int32_t BreakSensordebouncetimemilli = BREAK_SENSOR_LIMIT; HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0}; //******************************************************************************************************************** -uint32_t MotorsConfigMessage(HardwareMotor * request) +uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest) { uint32_t status = PASSED; - TimerMotors_t Motor_i; - Motor_i = request->hardwaremotortype; + TimerMotors_t Motor_i,MotorId; + MOTDRIVER_TYPE DriverType; + HardwareMotor *request; + if (HWrequest == NULL) + return ERROR; + for (MotorId = 0; MotorId < HWrequest->n_motors ; MotorId++) + { + request = HWrequest->motors[MotorId]; + if (request == NULL) + return ERROR; + Motor_i = request->hardwaremotortype; MotorsCfg[Motor_i].configword = request->configword; MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype; MotorsCfg[Motor_i].minfrequency = request->minfrequency; @@ -80,13 +89,21 @@ uint32_t MotorsConfigMessage(HardwareMotor * request) MotorsCfg[Motor_i].fnslpdec = request->fnslpdec; MotorsCfg[Motor_i].fsspd = request->fsspd; - status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]); + MotorsCfg[Motor_i].gatecfg1 = request->gatecfg1; + MotorsCfg[Motor_i].gatecfg2 = request->gatecfg2; + MotorsCfg[Motor_i].tvalhold = request->tvalhold; + MotorsCfg[Motor_i].tvalrun = request->tvalrun ; + MotorsCfg[Motor_i].tvalacc = request->tvalacc ; + MotorsCfg[Motor_i].tvaldec = request->tvaldec ; + MotorsCfg[Motor_i].tfast = request->tfast ; + MotorsCfg[Motor_i].tonmin = request->tonmin ; + MotorsCfg[Motor_i].toffmin = request->toffmin ; + + status = MotorConfig( Motor_i, &MotorsCfg[Motor_i]); // if (Motor_i == MOTOR_RDRIVING) // ThreadInitialTestStub(request); - + } return status; -// } -// else return Motor_i; } uint32_t MotorPidRequestMessage(HardwarePidControl* request) |