Sebastian Grünwald / PM2_Libary

PM2_Lib

Dependencies:   LSM9DS1 RangeFinder FastPWM

PositionController.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 8:6b747ad59ff5 1 #include "PositionController.h"
pmic 8:6b747ad59ff5 2
pmic 8:6b747ad59ff5 3 const float PositionController::TS = 0.001f; // period of 1 ms
pmic 8:6b747ad59ff5 4 const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
pmic 19:518ed284d98b 5 const float PositionController::MIN_DUTY_CYCLE = 0.01f; // minimum duty-cycle
pmic 19:518ed284d98b 6 const float PositionController::MAX_DUTY_CYCLE = 0.99f; // maximum duty-cycle
pmic 8:6b747ad59ff5 7
pmic 8:6b747ad59ff5 8 PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 8:6b747ad59ff5 9 {
pmic 8:6b747ad59ff5 10 this->counts_per_turn = counts_per_turn;
pmic 10:fe74e8909d3f 11 setFeedForwardGain(kn);
pmic 10:fe74e8909d3f 12 setSpeedCntrlGain(0.1f);
pmic 8:6b747ad59ff5 13 this->max_voltage = max_voltage;
pmic 8:6b747ad59ff5 14 this->max_speed = kn*max_voltage;
pmic 10:fe74e8909d3f 15 setPositionCntrlGain(1300.0f);
pmic 10:fe74e8909d3f 16
pmic 10:fe74e8909d3f 17 // initialise pwm
pmic 10:fe74e8909d3f 18 pwm.period(0.00005); // pwm period of 50 us
pmic 10:fe74e8909d3f 19 pwm.write(0.5); // duty-cycle of 50%
pmic 10:fe74e8909d3f 20
pmic 10:fe74e8909d3f 21 // initialise
pmic 10:fe74e8909d3f 22 previousValueCounter = encoderCounter.read();
pmic 10:fe74e8909d3f 23 speedFilter.setPeriod(TS);
pmic 10:fe74e8909d3f 24 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 10:fe74e8909d3f 25 desiredSpeed = 0.0f;
pmic 10:fe74e8909d3f 26 actualSpeed = 0.0f;
pmic 10:fe74e8909d3f 27 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 10:fe74e8909d3f 28 this->initialRotation = initialRotation;
pmic 10:fe74e8909d3f 29 actualRotation = initialRotation;
pmic 10:fe74e8909d3f 30 desiredRotation = initialRotation;
pmic 8:6b747ad59ff5 31
pmic 10:fe74e8909d3f 32 // set up thread
pmic 10:fe74e8909d3f 33 thread.start(callback(this, &PositionController::run));
pmic 10:fe74e8909d3f 34 ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 10:fe74e8909d3f 35 }
pmic 8:6b747ad59ff5 36
pmic 10:fe74e8909d3f 37 PositionController::PositionController(float counts_per_turn, float kn, float kp, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 10:fe74e8909d3f 38 {
pmic 10:fe74e8909d3f 39 this->counts_per_turn = counts_per_turn;
pmic 10:fe74e8909d3f 40 setFeedForwardGain(kn);
pmic 10:fe74e8909d3f 41 setSpeedCntrlGain(kp);
pmic 10:fe74e8909d3f 42 this->max_voltage = max_voltage;
pmic 10:fe74e8909d3f 43 this->max_speed = kn*max_voltage;
pmic 10:fe74e8909d3f 44 setPositionCntrlGain(1300.0f);
pmic 10:fe74e8909d3f 45
pmic 10:fe74e8909d3f 46 // initialise pwm
pmic 10:fe74e8909d3f 47 pwm.period(0.00005); // pwm period of 50 us
pmic 10:fe74e8909d3f 48 pwm.write(0.5); // duty-cycle of 50%
pmic 10:fe74e8909d3f 49
pmic 10:fe74e8909d3f 50 // initialise
pmic 8:6b747ad59ff5 51 previousValueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 52 speedFilter.setPeriod(TS);
pmic 8:6b747ad59ff5 53 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 8:6b747ad59ff5 54 desiredSpeed = 0.0f;
pmic 8:6b747ad59ff5 55 actualSpeed = 0.0f;
pmic 8:6b747ad59ff5 56 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 8:6b747ad59ff5 57 this->initialRotation = initialRotation;
pmic 8:6b747ad59ff5 58 actualRotation = initialRotation;
pmic 8:6b747ad59ff5 59 desiredRotation = initialRotation;
pmic 8:6b747ad59ff5 60
pmic 10:fe74e8909d3f 61 // set up thread
pmic 8:6b747ad59ff5 62 thread.start(callback(this, &PositionController::run));
pmic 10:fe74e8909d3f 63 ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 8:6b747ad59ff5 64 }
pmic 8:6b747ad59ff5 65
pmic 8:6b747ad59ff5 66 PositionController::PositionController(float counts_per_turn, float kn, float kp, float p, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 8:6b747ad59ff5 67 {
pmic 8:6b747ad59ff5 68 this->counts_per_turn = counts_per_turn;
pmic 10:fe74e8909d3f 69 setFeedForwardGain(kn);
pmic 10:fe74e8909d3f 70 setSpeedCntrlGain(kp);
pmic 8:6b747ad59ff5 71 this->max_voltage = max_voltage;
pmic 8:6b747ad59ff5 72 this->max_speed = kn*max_voltage;
pmic 10:fe74e8909d3f 73 setPositionCntrlGain(p);
pmic 8:6b747ad59ff5 74
pmic 10:fe74e8909d3f 75 // initialise pwm
pmic 10:fe74e8909d3f 76 pwm.period(0.00005); // pwm period of 50 us
pmic 10:fe74e8909d3f 77 pwm.write(0.5); // duty-cycle of 50%
pmic 8:6b747ad59ff5 78
pmic 10:fe74e8909d3f 79 // initialise
pmic 8:6b747ad59ff5 80 previousValueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 81 speedFilter.setPeriod(TS);
pmic 8:6b747ad59ff5 82 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 8:6b747ad59ff5 83 desiredSpeed = 0.0f;
pmic 10:fe74e8909d3f 84 actualSpeed = 0.0f;
pmic 8:6b747ad59ff5 85 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 8:6b747ad59ff5 86 this->initialRotation = initialRotation;
pmic 8:6b747ad59ff5 87 actualRotation = initialRotation;
pmic 8:6b747ad59ff5 88 desiredRotation = initialRotation;
pmic 8:6b747ad59ff5 89
pmic 10:fe74e8909d3f 90 // set up thread
pmic 8:6b747ad59ff5 91 thread.start(callback(this, &PositionController::run));
pmic 10:fe74e8909d3f 92 ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 8:6b747ad59ff5 93 }
pmic 8:6b747ad59ff5 94
pmic 8:6b747ad59ff5 95 PositionController::~PositionController()
pmic 8:6b747ad59ff5 96 {
pmic 10:fe74e8909d3f 97 ticker.detach();
pmic 8:6b747ad59ff5 98 }
pmic 8:6b747ad59ff5 99
pmic 10:fe74e8909d3f 100 /**
pmic 10:fe74e8909d3f 101 * Reads the speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 102 * @return actual speed in RPM.
pmic 10:fe74e8909d3f 103 */
pmic 8:6b747ad59ff5 104 float PositionController::getSpeedRPM()
pmic 8:6b747ad59ff5 105 {
pmic 8:6b747ad59ff5 106 return actualSpeed;
pmic 8:6b747ad59ff5 107 }
pmic 8:6b747ad59ff5 108
pmic 10:fe74e8909d3f 109 /**
pmic 10:fe74e8909d3f 110 * Reads the speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 111 * @return actual speed in RPS.
pmic 10:fe74e8909d3f 112 */
pmic 8:6b747ad59ff5 113 float PositionController::getSpeedRPS()
pmic 8:6b747ad59ff5 114 {
pmic 8:6b747ad59ff5 115 return actualSpeed/60.0f;
pmic 8:6b747ad59ff5 116 }
pmic 8:6b747ad59ff5 117
pmic 10:fe74e8909d3f 118 /**
pmic 10:fe74e8909d3f 119 * Sets desired rotation (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 120 */
pmic 8:6b747ad59ff5 121 void PositionController::setDesiredRotation(float desiredRotation)
pmic 8:6b747ad59ff5 122 {
pmic 8:6b747ad59ff5 123 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 124 }
pmic 8:6b747ad59ff5 125
pmic 10:fe74e8909d3f 126 /**
pmic 10:fe74e8909d3f 127 * Sets desired rotation (1 corresponds to 360 deg) and max. rotational speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 128 */
pmic 8:6b747ad59ff5 129 void PositionController::setDesiredRotation(float desiredRotation, float maxSpeedRPS)
pmic 8:6b747ad59ff5 130 {
pmic 10:fe74e8909d3f 131 float maxSpeedRPM = fabs(maxSpeedRPS * 60.0f);
pmic 10:fe74e8909d3f 132 if(maxSpeedRPM > kn * max_voltage) {
pmic 10:fe74e8909d3f 133 maxSpeedRPM = kn * max_voltage;
pmic 10:fe74e8909d3f 134 }
pmic 10:fe74e8909d3f 135 this->max_speed = maxSpeedRPM;
pmic 8:6b747ad59ff5 136 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 137 }
pmic 8:6b747ad59ff5 138
pmic 10:fe74e8909d3f 139 /**
pmic 10:fe74e8909d3f 140 * Reads the number of rotations (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 141 * @return actual rotations.
pmic 10:fe74e8909d3f 142 */
pmic 8:6b747ad59ff5 143 float PositionController::getRotation()
pmic 8:6b747ad59ff5 144 {
pmic 8:6b747ad59ff5 145 return actualRotation - initialRotation;
pmic 8:6b747ad59ff5 146 }
pmic 8:6b747ad59ff5 147
pmic 10:fe74e8909d3f 148 /**
pmic 10:fe74e8909d3f 149 * Sets the feed-forward gain.
pmic 10:fe74e8909d3f 150 */
pmic 10:fe74e8909d3f 151 void PositionController::setFeedForwardGain(float kn)
pmic 10:fe74e8909d3f 152 {
pmic 10:fe74e8909d3f 153 this->kn = kn;
pmic 10:fe74e8909d3f 154 }
pmic 10:fe74e8909d3f 155
pmic 10:fe74e8909d3f 156 /**
pmic 10:fe74e8909d3f 157 * Sets the gain of the speed controller (p-controller).
pmic 10:fe74e8909d3f 158 */
pmic 10:fe74e8909d3f 159 void PositionController::setSpeedCntrlGain(float kp)
pmic 10:fe74e8909d3f 160 {
pmic 10:fe74e8909d3f 161 this->kp = kp;
pmic 10:fe74e8909d3f 162 }
pmic 10:fe74e8909d3f 163
pmic 10:fe74e8909d3f 164 /**
pmic 10:fe74e8909d3f 165 * Sets the gain of the position controller (p-controller).
pmic 10:fe74e8909d3f 166 */
pmic 10:fe74e8909d3f 167 void PositionController::setPositionCntrlGain(float p)
pmic 10:fe74e8909d3f 168 {
pmic 10:fe74e8909d3f 169 this->p = p;
pmic 10:fe74e8909d3f 170 }
pmic 10:fe74e8909d3f 171
pmic 8:6b747ad59ff5 172 void PositionController::run()
pmic 8:6b747ad59ff5 173 {
pmic 8:6b747ad59ff5 174 while(true) {
pmic 8:6b747ad59ff5 175 // wait for the periodic signal
pmic 8:6b747ad59ff5 176 ThisThread::flags_wait_any(threadFlag);
pmic 8:6b747ad59ff5 177
pmic 8:6b747ad59ff5 178 // calculate actual speed of motors in [rpm]
pmic 8:6b747ad59ff5 179 short valueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 180 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 8:6b747ad59ff5 181 previousValueCounter = valueCounter;
pmic 8:6b747ad59ff5 182 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 8:6b747ad59ff5 183 actualRotation = actualRotation + actualSpeed/60.0f*TS;
pmic 8:6b747ad59ff5 184
pmic 8:6b747ad59ff5 185 // calculate motor phase voltages
pmic 8:6b747ad59ff5 186 desiredSpeed = p*(desiredRotation - actualRotation);
pmic 8:6b747ad59ff5 187 if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
pmic 8:6b747ad59ff5 188 else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
pmic 8:6b747ad59ff5 189 float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
pmic 8:6b747ad59ff5 190 // calculate, limit and set duty cycles
pmic 8:6b747ad59ff5 191 float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
pmic 8:6b747ad59ff5 192 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 8:6b747ad59ff5 193 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 8:6b747ad59ff5 194 pwm.write(static_cast<double>(dutyCycle));
pmic 8:6b747ad59ff5 195 }
pmic 8:6b747ad59ff5 196 }
pmic 8:6b747ad59ff5 197
pmic 8:6b747ad59ff5 198 void PositionController::sendThreadFlag()
pmic 8:6b747ad59ff5 199 {
pmic 8:6b747ad59ff5 200 thread.flags_set(threadFlag);
pmic 8:6b747ad59ff5 201 }