SpeedController.cpp

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