Sebastian Grünwald / PM2_Libary

PM2_Lib

Dependencies:   LSM9DS1 RangeFinder FastPWM

SpeedController.cpp@23:1926540ebbec, 2022-03-23 (annotated)

Committer:
pmic
Date:
Wed Mar 23 09:46:07 2022 +0000
Revision:
23:1926540ebbec
Parent:
19:518ed284d98b 
Renamed ReEnalbe function to Enable in Servo class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 3:8b42e643b294 1 #include "SpeedController.h"
pmic 3:8b42e643b294 2
pmic 6:41dd03654c44 3 const float SpeedController::TS = 0.001f; // period of 1 ms
pmic 6:41dd03654c44 4 const float SpeedController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
pmic 19:518ed284d98b 5 const float SpeedController::MIN_DUTY_CYCLE = 0.01f; // minimum duty-cycle
pmic 19:518ed284d98b 6 const float SpeedController::MAX_DUTY_CYCLE = 0.99f; // maximum duty-cycle
pmic 3:8b42e643b294 7
pmic 6:41dd03654c44 8 SpeedController::SpeedController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 3:8b42e643b294 9 {
pmic 6:41dd03654c44 10 this->counts_per_turn = counts_per_turn;
pmic 10:fe74e8909d3f 11 setFeedForwardGain(kn);
pmic 10:fe74e8909d3f 12 setSpeedCntrlGain(0.1f);
pmic 6:41dd03654c44 13 this->max_voltage = max_voltage;
pmic 3:8b42e643b294 14
pmic 10:fe74e8909d3f 15 // initialise pwm
pmic 10:fe74e8909d3f 16 pwm.period(0.00005); // pwm period of 50 us
pmic 10:fe74e8909d3f 17 pwm.write(0.5); // duty-cycle of 50%
pmic 3:8b42e643b294 18
pmic 10:fe74e8909d3f 19 // initialise
pmic 3:8b42e643b294 20 previousValueCounter = encoderCounter.read();
pmic 6:41dd03654c44 21 speedFilter.setPeriod(TS);
pmic 3:8b42e643b294 22 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 3:8b42e643b294 23 desiredSpeed = 0.0f;
pmic 3:8b42e643b294 24 actualSpeed = 0.0f;
pmic 4:9c003c402033 25 // actualAngle = 0.0f;
pmic 3:8b42e643b294 26
pmic 10:fe74e8909d3f 27 // set up thread
pmic 3:8b42e643b294 28 thread.start(callback(this, &SpeedController::run));
pmic 10:fe74e8909d3f 29 ticker.attach(callback(this, &SpeedController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 3:8b42e643b294 30 }
pmic 3:8b42e643b294 31
pmic 3:8b42e643b294 32 SpeedController::~SpeedController()
pmic 3:8b42e643b294 33 {
pmic 10:fe74e8909d3f 34 ticker.detach();
pmic 3:8b42e643b294 35 }
pmic 3:8b42e643b294 36
pmic 10:fe74e8909d3f 37 /**
pmic 10:fe74e8909d3f 38 * Sets the desired speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 39 */
pmic 4:9c003c402033 40 void SpeedController::setDesiredSpeedRPM(float desiredSpeed)
pmic 3:8b42e643b294 41 {
pmic 3:8b42e643b294 42 this->desiredSpeed = desiredSpeed;
pmic 3:8b42e643b294 43 }
pmic 3:8b42e643b294 44
pmic 10:fe74e8909d3f 45 /**
pmic 10:fe74e8909d3f 46 * Reads the speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 47 * @return actual speed in RPM.
pmic 10:fe74e8909d3f 48 */
pmic 4:9c003c402033 49 float SpeedController::getSpeedRPM()
pmic 3:8b42e643b294 50 {
pmic 3:8b42e643b294 51 return actualSpeed;
pmic 3:8b42e643b294 52 }
pmic 3:8b42e643b294 53
pmic 10:fe74e8909d3f 54 /**
pmic 10:fe74e8909d3f 55 * Sets the desired speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 56 */
pmic 4:9c003c402033 57 void SpeedController::setDesiredSpeedRPS(float desiredSpeed)
pmic 4:9c003c402033 58 {
pmic 4:9c003c402033 59 this->desiredSpeed = desiredSpeed*60.0f;
pmic 4:9c003c402033 60 }
pmic 4:9c003c402033 61
pmic 10:fe74e8909d3f 62 /**
pmic 10:fe74e8909d3f 63 * Reads the speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 64 * @return actual speed in RPS.
pmic 10:fe74e8909d3f 65 */
pmic 4:9c003c402033 66 float SpeedController::getSpeedRPS()
pmic 4:9c003c402033 67 {
pmic 4:9c003c402033 68 return actualSpeed/60.0f;
pmic 4:9c003c402033 69 }
pmic 4:9c003c402033 70
pmic 10:fe74e8909d3f 71 /**
pmic 10:fe74e8909d3f 72 * Sets the feed-forward gain.
pmic 10:fe74e8909d3f 73 */
pmic 10:fe74e8909d3f 74 void SpeedController::setFeedForwardGain(float kn)
pmic 10:fe74e8909d3f 75 {
pmic 10:fe74e8909d3f 76 this->kn = kn;
pmic 10:fe74e8909d3f 77 }
pmic 10:fe74e8909d3f 78
pmic 10:fe74e8909d3f 79 /**
pmic 10:fe74e8909d3f 80 * Sets the gain of the speed controller (p-controller).
pmic 10:fe74e8909d3f 81 */
pmic 10:fe74e8909d3f 82 void SpeedController::setSpeedCntrlGain(float kp)
pmic 10:fe74e8909d3f 83 {
pmic 10:fe74e8909d3f 84 this->kp = kp;
pmic 10:fe74e8909d3f 85 }
pmic 10:fe74e8909d3f 86
pmic 3:8b42e643b294 87 void SpeedController::run()
pmic 3:8b42e643b294 88 {
pmic 3:8b42e643b294 89 while(true) {
pmic 3:8b42e643b294 90 // wait for the periodic signal
pmic 3:8b42e643b294 91 ThisThread::flags_wait_any(threadFlag);
pmic 3:8b42e643b294 92
pmic 3:8b42e643b294 93 // calculate actual speed of motors in [rpm]
pmic 3:8b42e643b294 94 short valueCounter = encoderCounter.read();
pmic 4:9c003c402033 95 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 3:8b42e643b294 96 previousValueCounter = valueCounter;
pmic 6:41dd03654c44 97 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 6:41dd03654c44 98 // actualAngle = actualAngle + actualSpeed/60.0f*TS;
pmic 3:8b42e643b294 99
pmic 3:8b42e643b294 100 // calculate motor phase voltages
pmic 6:41dd03654c44 101 float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
pmic 3:8b42e643b294 102
pmic 3:8b42e643b294 103 // calculate, limit and set duty cycles
pmic 6:41dd03654c44 104 float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
pmic 3:8b42e643b294 105 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 3:8b42e643b294 106 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 5:6cd242a61e4c 107 pwm.write(static_cast<double>(dutyCycle));
pmic 3:8b42e643b294 108 }
pmic 3:8b42e643b294 109 }
pmic 3:8b42e643b294 110
pmic 3:8b42e643b294 111 void SpeedController::sendThreadFlag()
pmic 3:8b42e643b294 112 {
pmic 3:8b42e643b294 113 thread.flags_set(threadFlag);
pmic 3:8b42e643b294 114 }