diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 68 |
1 files changed, 50 insertions, 18 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 7bc38c936..1eb49ef6d 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -28,6 +28,8 @@ TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW}; DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; +int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; + typedef struct { bool m_isEnabled; @@ -81,25 +83,42 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue) return OK; } -uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) +uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue) { +//#define MAX_CONTROL_SAMPLES 6 +//extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; +//extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; + //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; - uint32_t TranslatedReadValue = ReadValue - 8740; + int Pid_Id; + int32_t TranslatedReadValue, avreageSampleValue = 0; + double NormalizedError; for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) - if (ThreadMotorIdToDancerId[i] == deviceID) + if (ThreadMotorIdToDancerId[i] == DancerId) { index = i; break; } if (index==MAX_THREAD_MOTORS_NUM) { - LOG_ERROR (deviceID, "No motor for device"); + LOG_ERROR (DancerId, "No motor for device"); return 0xFFFFFFFF; } + if(MotorControlConfig[index].m_isEnabled ) { - MotorControlConfig[index].m_mesuredParam = TranslatedReadValue; + Pid_Id = ThreadMotorIdToControlId[index]; + TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; + MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue; + MotorSamplePointer[index]++; + if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; + for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++) + avreageSampleValue += MotorSamples[index][i]; + avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode; + NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; + + MotorControlConfig[index].m_mesuredParam = avreageSampleValue; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX) @@ -111,7 +130,7 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN; } - MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError, MotorsCfg[index].minmicrostep); + MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError, MotorsCfg[index].microstep); //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError); } @@ -140,16 +159,17 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { - int Motor_i, HW_Motor_Id; + int Motor_i, HW_Motor_Id, Pid_Id; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; - MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq; - MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq; - MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd; - MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp; - MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].ki; + Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ + MotorControlConfig[Motor_i].m_params.MAX = MotorsControl[Pid_Id].outputproportionalpowerlimit*OriginalMotorSpd_2PPS[Motor_i]; + MotorControlConfig[Motor_i].m_params.MIN = 0; + MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; + MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; + MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; @@ -158,12 +178,14 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) MotorControlConfig[Motor_i].m_mesuredParam = 0; MotorControlConfig[Motor_i].m_preError = 0; MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage + + MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); #ifdef DEBUG_TEST_FUNCTIONS - if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0); // continue; AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); - else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) + else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); #else if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled @@ -184,13 +206,23 @@ uint32_t ThreadPreSegmentState(void *JobDetails) { //set the speed only before the first segment, speed is constant accros job JobTicket* JobTicket = JobDetails; -// int process_speed = JobTicket->processparameters->dyeingspeed; - int process_speed = 500; + int Motor_i, HW_Motor_Id; + + int process_speed = JobTicket->processparameters->dyeingspeed; + + process_speed = 10; //debug - int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here + for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + { + HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; + //(Speed*uStep*PPR)/((2*PI*motor_Radius) + double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround * MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); + + OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed; + } // set the new speed in the dryer motor to the speed of the new segment - MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed, MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep); + MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR], MotorsCfg[MOTOR_DRYER_DRIVING].microstep); MotorSetSpeed(MOTOR_RLOADING, 1, 1); MotorSetSpeed(MOTOR_LLOADING, 1,1); |