PositionController.cpp

00001 #include "PositionController.h"
00002 
00003 const float PositionController::TS = 0.001f;                       // period of 1 ms
00004 const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
00005 const float PositionController::MIN_DUTY_CYCLE = 0.01f;            // minimum duty-cycle
00006 const float PositionController::MAX_DUTY_CYCLE = 0.99f;            // maximum duty-cycle
00007 
00008 PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
00009 {
00010     this->counts_per_turn = counts_per_turn;
00011     setFeedForwardGain(kn);
00012     setSpeedCntrlGain(0.1f);
00013     this->max_voltage = max_voltage;
00014     this->max_speed   = kn*max_voltage;
00015     setPositionCntrlGain(22.0f);
00016 
00017     // initialise pwm
00018     pwm.period(0.00005); // pwm period of 50 us
00019     pwm.write(0.5);      // duty-cycle of 50%
00020 
00021     // initialise
00022     previousValueCounter = encoderCounter.read();
00023     speedFilter.setPeriod(TS);
00024     speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00025     desiredSpeed = 0.0f;
00026     actualSpeed = 0.0f;
00027     this->initialRotation = (float)encoderCounter.read()/counts_per_turn;
00028     actualRotation  = initialRotation;
00029     desiredRotation = initialRotation;
00030     
00031     motion.setProfileVelocity(max_voltage * kn);
00032     float maxAcceleration = 22.0f * max_voltage * kn * 0.4f; // pmic, 13.05.2022, only 40%, based on simple measurement, max_voltage * kn is gearratio
00033     motion.setProfileAcceleration(maxAcceleration);
00034     motion.setProfileDeceleration(maxAcceleration);
00035 
00036     // set up thread
00037     thread.start(callback(this, &PositionController::run));
00038     ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
00039 }
00040 
00041 PositionController::~PositionController()
00042 {
00043     ticker.detach();
00044 }
00045 
00046 /**
00047  * Reads the speed in RPM (rotations per minute).
00048  * @return actual speed in RPM.
00049  */
00050 float PositionController::getSpeedRPM()
00051 {
00052     return actualSpeed;
00053 }
00054 
00055 /**
00056  * Reads the speed in RPS (rotations per second).
00057  * @return actual speed in RPS.
00058  */
00059 float PositionController::getSpeedRPS()
00060 {
00061     return actualSpeed/60.0f;
00062 }
00063 
00064 /**
00065  * Sets desired rotation (1 corresponds to 360 deg).
00066  */
00067 void PositionController::setDesiredRotation(float desiredRotation)
00068 {
00069     this->desiredRotation = initialRotation + desiredRotation;
00070 }
00071 
00072 /**
00073  * Reads the number of rotations (1 corresponds to 360 deg).
00074  * @return actual rotations.
00075  */
00076 float PositionController::getRotation()
00077 {
00078     return actualRotation - initialRotation;
00079 }
00080 
00081 /**
00082  * Sets the feed-forward gain.
00083  */
00084 void PositionController::setFeedForwardGain(float kn)
00085 {
00086     this->kn = kn;
00087 }
00088 
00089 /**
00090  * Sets the gain of the speed controller (p-controller).
00091  */
00092 void PositionController::setSpeedCntrlGain(float kp)
00093 {
00094     this->kp = kp;
00095 }
00096 
00097 /**
00098  * Sets the gain of the position controller (p-controller).
00099  */
00100 void PositionController::setPositionCntrlGain(float p)
00101 {
00102     this->p = p;
00103 }
00104 
00105 /**
00106  * Sets the maximum Velocity in RPS.
00107  */
00108 void PositionController::setMaxVelocityRPS(float maxVelocityRPS)
00109 {
00110     if (maxVelocityRPS*60.0f <= max_speed)
00111         motion.setProfileVelocity(maxVelocityRPS*60.0f);
00112     else
00113         motion.setProfileVelocity(max_speed);
00114 }
00115 
00116 /**
00117  * Sets the maximum Velocity in RPM.
00118  */
00119 void PositionController::setMaxVelocityRPM(float maxVelocityRPM)
00120 {
00121     if (maxVelocityRPM <= max_speed)
00122         motion.setProfileVelocity(maxVelocityRPM);
00123     else
00124         motion.setProfileVelocity(max_speed);
00125 }
00126 
00127 /**
00128  * Sets the maximum Acceleration in RPS/sec.
00129  */
00130 void PositionController::setMaxAccelerationRPS(float maxAccelerationRPS)
00131 {
00132     motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
00133     motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
00134 }
00135 
00136 /**
00137  * Sets the maximum Acceleration in RPM/sec.
00138  */
00139 void PositionController::setMaxAccelerationRPM(float maxAccelerationRPM)
00140 {
00141     motion.setProfileAcceleration(maxAccelerationRPM);
00142     motion.setProfileDeceleration(maxAccelerationRPM);
00143 }
00144 
00145 void PositionController::run()
00146 {
00147     while(true) {
00148         // wait for the periodic signal
00149         ThisThread::flags_wait_any(threadFlag);
00150 
00151         // calculate actual speed of motors in [rpm]
00152         short valueCounter = encoderCounter.read();
00153         short countsInPastPeriod = valueCounter - previousValueCounter;
00154         previousValueCounter = valueCounter;
00155         actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
00156         actualRotation = actualRotation + actualSpeed/60.0f*TS;
00157 
00158         // trajectory needs to be calculated in rotations*60 so that all units are in RPM
00159         motion.incrementToPosition(60.0f * desiredRotation, TS);       
00160         float desiredRotationMotion = motion.getPosition();
00161         float desiredVelocityMotion = motion.getVelocity();
00162 
00163         // calculate motor phase voltages
00164         desiredSpeed  = p *(desiredRotationMotion - 60.0f * actualRotation) + 0.0f * desiredVelocityMotion; // using velocity feedforward here will lead to overshoot
00165         
00166         if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
00167         else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
00168         
00169         float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
00170         // calculate, limit and set duty cycles
00171         float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
00172         if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
00173         else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
00174         pwm.write(static_cast<double>(dutyCycle));
00175     }
00176 }
00177 
00178 void PositionController::sendThreadFlag()
00179 {
00180     thread.flags_set(threadFlag);
00181 }