From 17c5673ea81fbc79043b8e08301d9b0b1f3457b2 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 18 Apr 2018 13:58:47 +0300 Subject: Control fixes (Encoders, Motors and more) and FPGA preparations --- .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 4 +- .../Embedded/Modules/Thread/Thread_Winder.c | 11 +++-- .../Embedded/Modules/Thread/Thread_init.c | 2 +- .../Embedded/Modules/Thread/Thread_print.c | 53 ++++++++++++---------- 4 files changed, 39 insertions(+), 31 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index c737b9ea8..b31c74830 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -50,7 +50,7 @@ extern InternalWinderConfigStruc InternalWinderCfg; extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; #define MAX_CONTROL_SAMPLES 6 -extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; +extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM]; @@ -64,7 +64,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t DancerConfigMessage(HardwareDancer * request); uint32_t ThreadPrepareState(void *JobDetails); - +uint32_t Winder_Prepare(void); #endif //MODULES_THREAD_THREAD_H_ diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index da0844c3c..6a05b83ce 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -105,16 +105,21 @@ uint32_t Winder_Presegment(void *JobDetails) uint32_t Winder_End(void) { //stop screw - return StopMotor (MOTOR_SCREW,1); + return StopMotor (MOTOR_SCREW,Hard_Hiz); } void Winder_ScrewHomeLimitSwitchInterrupt(void) { uint32_t status; if (Winder_ScrewHoming) { - StopMotor(MOTOR_SCREW,3); //stop ASAP + StopMotor(MOTOR_SCREW,Hard_Stop); //stop ASAP } - status = MotorSetDirection(MOTOR_SCREW,0);//make sur to move the cart out + status = MotorSetDirection(MOTOR_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out +} +void Winder_ScrewOutLimitSwitchInterrupt(void) +{ + uint32_t status; + status = MotorSetDirection(MOTOR_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out } uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps) { diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 8c4b83ec6..1727c650a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -14,7 +14,7 @@ HardwareMotor MotorsCfg[NUM_OF_MOTORS]={0}; HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0}; -uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0}; +int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0}; int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0}; double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0}; InternalWinderConfigStruc InternalWinderCfg = {0}; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 07fecf313..fb9740dec 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -27,14 +27,14 @@ 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}; - +uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; typedef struct { bool m_isEnabled; - uint32_t m_SetParam; - uint32_t m_mesuredParam; + int32_t m_SetParam; + float m_mesuredParam; float m_preError; float m_integral; float m_calculatedError; @@ -54,7 +54,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue) //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; for (i=0;i= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i= MotorControlConfig[index].m_params.MAX) + /*if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX) { MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX; } if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN) { MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN; - } + }*/ - MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError, MotorsCfg[index].microstep); + MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index], MotorsCfg[ThreadMotorIdToMotorId[index]].microstep); //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError); } @@ -165,11 +166,12 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; 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.MAX = 1; + MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; 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.epsilon = 0.01; MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; @@ -181,23 +183,25 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); #ifdef DEBUG_TEST_FUNCTIONS - 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 + if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); + /*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); + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); 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); + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_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 continue; //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[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)) - AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); + AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } Winder_Prepare(); //set 3 dancers to the profile positions - ControlStart(); + return OK; } @@ -217,11 +221,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails) 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); - + //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed; - MotorControlConfig[Motor_i].m_SetParam = motor_speed; } - + ControlStart(); // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR], MotorsCfg[MOTOR_DRYER_DRIVING].microstep); MotorSetSpeed(MOTOR_RLOADING, 1, 1); -- cgit v1.3.1