Michael Ernst Peter / PM2_Libary

Libary for PM2.

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

SpeedController.cpp@32:bb074bb17d39, 2022-06-22 (annotated)

Committer:
pmic
Date:
Wed Jun 22 14:05:13 2022 +0000
Revision:
32:bb074bb17d39
Parent:
29:335fb9b01ca7 
Updated SensorBar so it is not blocking the program if it is not connected

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 29:335fb9b01ca7 26
pmic 29:335fb9b01ca7 27 motion.setProfileVelocity(max_voltage * kn);
pmic 29:335fb9b01ca7 28 float maxAcceleration = 22.0f * max_voltage * kn * 0.4f; // pmic, 13.05.2022, only 40%, based on simple measurement, max_voltage * kn is gearratio
pmic 29:335fb9b01ca7 29 motion.setProfileAcceleration(maxAcceleration);
pmic 29:335fb9b01ca7 30 motion.setProfileDeceleration(maxAcceleration);
pmic 3:8b42e643b294 31
pmic 10:fe74e8909d3f 32 // set up thread
pmic 3:8b42e643b294 33 thread.start(callback(this, &SpeedController::run));
pmic 10:fe74e8909d3f 34 ticker.attach(callback(this, &SpeedController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 3:8b42e643b294 35 }
pmic 3:8b42e643b294 36
pmic 3:8b42e643b294 37 SpeedController::~SpeedController()
pmic 3:8b42e643b294 38 {
pmic 10:fe74e8909d3f 39 ticker.detach();
pmic 3:8b42e643b294 40 }
pmic 3:8b42e643b294 41
pmic 10:fe74e8909d3f 42 /**
pmic 10:fe74e8909d3f 43 * Sets the desired speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 44 */
pmic 4:9c003c402033 45 void SpeedController::setDesiredSpeedRPM(float desiredSpeed)
pmic 3:8b42e643b294 46 {
pmic 3:8b42e643b294 47 this->desiredSpeed = desiredSpeed;
pmic 3:8b42e643b294 48 }
pmic 3:8b42e643b294 49
pmic 10:fe74e8909d3f 50 /**
pmic 10:fe74e8909d3f 51 * Reads the speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 52 * @return actual speed in RPM.
pmic 10:fe74e8909d3f 53 */
pmic 4:9c003c402033 54 float SpeedController::getSpeedRPM()
pmic 3:8b42e643b294 55 {
pmic 3:8b42e643b294 56 return actualSpeed;
pmic 3:8b42e643b294 57 }
pmic 3:8b42e643b294 58
pmic 10:fe74e8909d3f 59 /**
pmic 10:fe74e8909d3f 60 * Sets the desired speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 61 */
pmic 4:9c003c402033 62 void SpeedController::setDesiredSpeedRPS(float desiredSpeed)
pmic 4:9c003c402033 63 {
pmic 4:9c003c402033 64 this->desiredSpeed = desiredSpeed*60.0f;
pmic 4:9c003c402033 65 }
pmic 4:9c003c402033 66
pmic 10:fe74e8909d3f 67 /**
pmic 10:fe74e8909d3f 68 * Reads the speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 69 * @return actual speed in RPS.
pmic 10:fe74e8909d3f 70 */
pmic 4:9c003c402033 71 float SpeedController::getSpeedRPS()
pmic 4:9c003c402033 72 {
pmic 4:9c003c402033 73 return actualSpeed/60.0f;
pmic 4:9c003c402033 74 }
pmic 4:9c003c402033 75
pmic 10:fe74e8909d3f 76 /**
pmic 10:fe74e8909d3f 77 * Sets the feed-forward gain.
pmic 10:fe74e8909d3f 78 */
pmic 10:fe74e8909d3f 79 void SpeedController::setFeedForwardGain(float kn)
pmic 10:fe74e8909d3f 80 {
pmic 10:fe74e8909d3f 81 this->kn = kn;
pmic 10:fe74e8909d3f 82 }
pmic 10:fe74e8909d3f 83
pmic 10:fe74e8909d3f 84 /**
pmic 10:fe74e8909d3f 85 * Sets the gain of the speed controller (p-controller).
pmic 10:fe74e8909d3f 86 */
pmic 10:fe74e8909d3f 87 void SpeedController::setSpeedCntrlGain(float kp)
pmic 10:fe74e8909d3f 88 {
pmic 10:fe74e8909d3f 89 this->kp = kp;
pmic 10:fe74e8909d3f 90 }
pmic 10:fe74e8909d3f 91
pmic 29:335fb9b01ca7 92 /**
pmic 29:335fb9b01ca7 93 * Sets the maximum Velocity in RPS.
pmic 29:335fb9b01ca7 94 */
pmic 29:335fb9b01ca7 95 void SpeedController::setMaxVelocityRPS(float maxVelocityRPS)
pmic 29:335fb9b01ca7 96 {
pmic 29:335fb9b01ca7 97 if (maxVelocityRPS*60.0f <= max_voltage * kn)
pmic 29:335fb9b01ca7 98 motion.setProfileVelocity(maxVelocityRPS*60.0f);
pmic 29:335fb9b01ca7 99 else
pmic 29:335fb9b01ca7 100 motion.setProfileVelocity(max_voltage * kn);
pmic 29:335fb9b01ca7 101 }
pmic 29:335fb9b01ca7 102
pmic 29:335fb9b01ca7 103 /**
pmic 29:335fb9b01ca7 104 * Sets the maximum Velocity in RPM.
pmic 29:335fb9b01ca7 105 */
pmic 29:335fb9b01ca7 106 void SpeedController::setMaxVelocityRPM(float maxVelocityRPM)
pmic 29:335fb9b01ca7 107 {
pmic 29:335fb9b01ca7 108 if (maxVelocityRPM <= max_voltage * kn)
pmic 29:335fb9b01ca7 109 motion.setProfileVelocity(maxVelocityRPM);
pmic 29:335fb9b01ca7 110 else
pmic 29:335fb9b01ca7 111 motion.setProfileVelocity(max_voltage * kn);
pmic 29:335fb9b01ca7 112 }
pmic 29:335fb9b01ca7 113
pmic 29:335fb9b01ca7 114 /**
pmic 29:335fb9b01ca7 115 * Sets the maximum Acceleration in RPS/sec.
pmic 29:335fb9b01ca7 116 */
pmic 29:335fb9b01ca7 117 void SpeedController::setMaxAccelerationRPS(float maxAccelerationRPS)
pmic 29:335fb9b01ca7 118 {
pmic 29:335fb9b01ca7 119 motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 120 motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 121 }
pmic 29:335fb9b01ca7 122
pmic 29:335fb9b01ca7 123 /**
pmic 29:335fb9b01ca7 124 * Sets the maximum Acceleration in RPM/sec.
pmic 29:335fb9b01ca7 125 */
pmic 29:335fb9b01ca7 126 void SpeedController::setMaxAccelerationRPM(float maxAccelerationRPM)
pmic 29:335fb9b01ca7 127 {
pmic 29:335fb9b01ca7 128 motion.setProfileAcceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 129 motion.setProfileDeceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 130 }
pmic 29:335fb9b01ca7 131
pmic 3:8b42e643b294 132 void SpeedController::run()
pmic 3:8b42e643b294 133 {
pmic 3:8b42e643b294 134 while(true) {
pmic 3:8b42e643b294 135 // wait for the periodic signal
pmic 3:8b42e643b294 136 ThisThread::flags_wait_any(threadFlag);
pmic 3:8b42e643b294 137
pmic 3:8b42e643b294 138 // calculate actual speed of motors in [rpm]
pmic 3:8b42e643b294 139 short valueCounter = encoderCounter.read();
pmic 4:9c003c402033 140 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 3:8b42e643b294 141 previousValueCounter = valueCounter;
pmic 6:41dd03654c44 142 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 6:41dd03654c44 143 // actualAngle = actualAngle + actualSpeed/60.0f*TS;
pmic 3:8b42e643b294 144
pmic 29:335fb9b01ca7 145 motion.incrementToVelocity(desiredSpeed, TS);
pmic 29:335fb9b01ca7 146 float desiredSpeedMotion = motion.getVelocity();
pmic 29:335fb9b01ca7 147
pmic 3:8b42e643b294 148 // calculate motor phase voltages
pmic 29:335fb9b01ca7 149 float voltage = kp*(desiredSpeedMotion - actualSpeed) + desiredSpeedMotion/kn;
pmic 3:8b42e643b294 150
pmic 3:8b42e643b294 151 // calculate, limit and set duty cycles
pmic 6:41dd03654c44 152 float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
pmic 3:8b42e643b294 153 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 3:8b42e643b294 154 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 5:6cd242a61e4c 155 pwm.write(static_cast<double>(dutyCycle));
pmic 3:8b42e643b294 156 }
pmic 3:8b42e643b294 157 }
pmic 3:8b42e643b294 158
pmic 3:8b42e643b294 159 void SpeedController::sendThreadFlag()
pmic 3:8b42e643b294 160 {
pmic 3:8b42e643b294 161 thread.flags_set(threadFlag);
pmic 3:8b42e643b294 162 }