Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: LSM9DS1 RangeFinder FastPWM
Dependents: PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more
PositionController.cpp@30:05abc1d2a2b9, 2022-05-14 (annotated)
- Committer:
- pmic
- Date:
- Sat May 14 08:19:21 2022 +0000
- Revision:
- 30:05abc1d2a2b9
- Parent:
- 29:335fb9b01ca7
Updated PM2_Libary
Who changed what in which revision?
User | Revision | Line number | New 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 | 29:335fb9b01ca7 | 15 | setPositionCntrlGain(22.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 | 30:05abc1d2a2b9 | 27 | this->initialRotation = (float)encoderCounter.read()/counts_per_turn; |
pmic | 10:fe74e8909d3f | 28 | actualRotation = initialRotation; |
pmic | 10:fe74e8909d3f | 29 | desiredRotation = initialRotation; |
pmic | 29:335fb9b01ca7 | 30 | |
pmic | 29:335fb9b01ca7 | 31 | motion.setProfileVelocity(max_voltage * kn); |
pmic | 29:335fb9b01ca7 | 32 | 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 | 33 | motion.setProfileAcceleration(maxAcceleration); |
pmic | 29:335fb9b01ca7 | 34 | motion.setProfileDeceleration(maxAcceleration); |
pmic | 8:6b747ad59ff5 | 35 | |
pmic | 10:fe74e8909d3f | 36 | // set up thread |
pmic | 8:6b747ad59ff5 | 37 | thread.start(callback(this, &PositionController::run)); |
pmic | 10:fe74e8909d3f | 38 | ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)}); |
pmic | 8:6b747ad59ff5 | 39 | } |
pmic | 8:6b747ad59ff5 | 40 | |
pmic | 8:6b747ad59ff5 | 41 | PositionController::~PositionController() |
pmic | 8:6b747ad59ff5 | 42 | { |
pmic | 10:fe74e8909d3f | 43 | ticker.detach(); |
pmic | 8:6b747ad59ff5 | 44 | } |
pmic | 8:6b747ad59ff5 | 45 | |
pmic | 10:fe74e8909d3f | 46 | /** |
pmic | 10:fe74e8909d3f | 47 | * Reads the speed in RPM (rotations per minute). |
pmic | 10:fe74e8909d3f | 48 | * @return actual speed in RPM. |
pmic | 10:fe74e8909d3f | 49 | */ |
pmic | 8:6b747ad59ff5 | 50 | float PositionController::getSpeedRPM() |
pmic | 8:6b747ad59ff5 | 51 | { |
pmic | 8:6b747ad59ff5 | 52 | return actualSpeed; |
pmic | 8:6b747ad59ff5 | 53 | } |
pmic | 8:6b747ad59ff5 | 54 | |
pmic | 10:fe74e8909d3f | 55 | /** |
pmic | 10:fe74e8909d3f | 56 | * Reads the speed in RPS (rotations per second). |
pmic | 10:fe74e8909d3f | 57 | * @return actual speed in RPS. |
pmic | 10:fe74e8909d3f | 58 | */ |
pmic | 8:6b747ad59ff5 | 59 | float PositionController::getSpeedRPS() |
pmic | 8:6b747ad59ff5 | 60 | { |
pmic | 8:6b747ad59ff5 | 61 | return actualSpeed/60.0f; |
pmic | 8:6b747ad59ff5 | 62 | } |
pmic | 8:6b747ad59ff5 | 63 | |
pmic | 10:fe74e8909d3f | 64 | /** |
pmic | 10:fe74e8909d3f | 65 | * Sets desired rotation (1 corresponds to 360 deg). |
pmic | 10:fe74e8909d3f | 66 | */ |
pmic | 8:6b747ad59ff5 | 67 | void PositionController::setDesiredRotation(float desiredRotation) |
pmic | 8:6b747ad59ff5 | 68 | { |
pmic | 8:6b747ad59ff5 | 69 | this->desiredRotation = initialRotation + desiredRotation; |
pmic | 8:6b747ad59ff5 | 70 | } |
pmic | 8:6b747ad59ff5 | 71 | |
pmic | 10:fe74e8909d3f | 72 | /** |
pmic | 10:fe74e8909d3f | 73 | * Reads the number of rotations (1 corresponds to 360 deg). |
pmic | 10:fe74e8909d3f | 74 | * @return actual rotations. |
pmic | 10:fe74e8909d3f | 75 | */ |
pmic | 8:6b747ad59ff5 | 76 | float PositionController::getRotation() |
pmic | 8:6b747ad59ff5 | 77 | { |
pmic | 8:6b747ad59ff5 | 78 | return actualRotation - initialRotation; |
pmic | 8:6b747ad59ff5 | 79 | } |
pmic | 8:6b747ad59ff5 | 80 | |
pmic | 10:fe74e8909d3f | 81 | /** |
pmic | 10:fe74e8909d3f | 82 | * Sets the feed-forward gain. |
pmic | 10:fe74e8909d3f | 83 | */ |
pmic | 10:fe74e8909d3f | 84 | void PositionController::setFeedForwardGain(float kn) |
pmic | 10:fe74e8909d3f | 85 | { |
pmic | 10:fe74e8909d3f | 86 | this->kn = kn; |
pmic | 10:fe74e8909d3f | 87 | } |
pmic | 10:fe74e8909d3f | 88 | |
pmic | 10:fe74e8909d3f | 89 | /** |
pmic | 10:fe74e8909d3f | 90 | * Sets the gain of the speed controller (p-controller). |
pmic | 10:fe74e8909d3f | 91 | */ |
pmic | 10:fe74e8909d3f | 92 | void PositionController::setSpeedCntrlGain(float kp) |
pmic | 10:fe74e8909d3f | 93 | { |
pmic | 10:fe74e8909d3f | 94 | this->kp = kp; |
pmic | 10:fe74e8909d3f | 95 | } |
pmic | 10:fe74e8909d3f | 96 | |
pmic | 10:fe74e8909d3f | 97 | /** |
pmic | 10:fe74e8909d3f | 98 | * Sets the gain of the position controller (p-controller). |
pmic | 10:fe74e8909d3f | 99 | */ |
pmic | 10:fe74e8909d3f | 100 | void PositionController::setPositionCntrlGain(float p) |
pmic | 10:fe74e8909d3f | 101 | { |
pmic | 10:fe74e8909d3f | 102 | this->p = p; |
pmic | 10:fe74e8909d3f | 103 | } |
pmic | 10:fe74e8909d3f | 104 | |
pmic | 29:335fb9b01ca7 | 105 | /** |
pmic | 29:335fb9b01ca7 | 106 | * Sets the maximum Velocity in RPS. |
pmic | 29:335fb9b01ca7 | 107 | */ |
pmic | 29:335fb9b01ca7 | 108 | void PositionController::setMaxVelocityRPS(float maxVelocityRPS) |
pmic | 29:335fb9b01ca7 | 109 | { |
pmic | 29:335fb9b01ca7 | 110 | if (maxVelocityRPS*60.0f <= max_speed) |
pmic | 29:335fb9b01ca7 | 111 | motion.setProfileVelocity(maxVelocityRPS*60.0f); |
pmic | 29:335fb9b01ca7 | 112 | else |
pmic | 29:335fb9b01ca7 | 113 | motion.setProfileVelocity(max_speed); |
pmic | 29:335fb9b01ca7 | 114 | } |
pmic | 29:335fb9b01ca7 | 115 | |
pmic | 29:335fb9b01ca7 | 116 | /** |
pmic | 29:335fb9b01ca7 | 117 | * Sets the maximum Velocity in RPM. |
pmic | 29:335fb9b01ca7 | 118 | */ |
pmic | 29:335fb9b01ca7 | 119 | void PositionController::setMaxVelocityRPM(float maxVelocityRPM) |
pmic | 29:335fb9b01ca7 | 120 | { |
pmic | 29:335fb9b01ca7 | 121 | if (maxVelocityRPM <= max_speed) |
pmic | 29:335fb9b01ca7 | 122 | motion.setProfileVelocity(maxVelocityRPM); |
pmic | 29:335fb9b01ca7 | 123 | else |
pmic | 29:335fb9b01ca7 | 124 | motion.setProfileVelocity(max_speed); |
pmic | 29:335fb9b01ca7 | 125 | } |
pmic | 29:335fb9b01ca7 | 126 | |
pmic | 29:335fb9b01ca7 | 127 | /** |
pmic | 29:335fb9b01ca7 | 128 | * Sets the maximum Acceleration in RPS/sec. |
pmic | 29:335fb9b01ca7 | 129 | */ |
pmic | 29:335fb9b01ca7 | 130 | void PositionController::setMaxAccelerationRPS(float maxAccelerationRPS) |
pmic | 29:335fb9b01ca7 | 131 | { |
pmic | 29:335fb9b01ca7 | 132 | motion.setProfileAcceleration(maxAccelerationRPS*60.0f); |
pmic | 29:335fb9b01ca7 | 133 | motion.setProfileDeceleration(maxAccelerationRPS*60.0f); |
pmic | 29:335fb9b01ca7 | 134 | } |
pmic | 29:335fb9b01ca7 | 135 | |
pmic | 29:335fb9b01ca7 | 136 | /** |
pmic | 29:335fb9b01ca7 | 137 | * Sets the maximum Acceleration in RPM/sec. |
pmic | 29:335fb9b01ca7 | 138 | */ |
pmic | 29:335fb9b01ca7 | 139 | void PositionController::setMaxAccelerationRPM(float maxAccelerationRPM) |
pmic | 29:335fb9b01ca7 | 140 | { |
pmic | 29:335fb9b01ca7 | 141 | motion.setProfileAcceleration(maxAccelerationRPM); |
pmic | 29:335fb9b01ca7 | 142 | motion.setProfileDeceleration(maxAccelerationRPM); |
pmic | 29:335fb9b01ca7 | 143 | } |
pmic | 29:335fb9b01ca7 | 144 | |
pmic | 8:6b747ad59ff5 | 145 | void PositionController::run() |
pmic | 8:6b747ad59ff5 | 146 | { |
pmic | 8:6b747ad59ff5 | 147 | while(true) { |
pmic | 8:6b747ad59ff5 | 148 | // wait for the periodic signal |
pmic | 8:6b747ad59ff5 | 149 | ThisThread::flags_wait_any(threadFlag); |
pmic | 8:6b747ad59ff5 | 150 | |
pmic | 8:6b747ad59ff5 | 151 | // calculate actual speed of motors in [rpm] |
pmic | 8:6b747ad59ff5 | 152 | short valueCounter = encoderCounter.read(); |
pmic | 8:6b747ad59ff5 | 153 | short countsInPastPeriod = valueCounter - previousValueCounter; |
pmic | 8:6b747ad59ff5 | 154 | previousValueCounter = valueCounter; |
pmic | 8:6b747ad59ff5 | 155 | actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f); |
pmic | 8:6b747ad59ff5 | 156 | actualRotation = actualRotation + actualSpeed/60.0f*TS; |
pmic | 8:6b747ad59ff5 | 157 | |
pmic | 30:05abc1d2a2b9 | 158 | // trajectory needs to be calculated in rotations*60 so that all units are in RPM |
pmic | 29:335fb9b01ca7 | 159 | motion.incrementToPosition(60.0f * desiredRotation, TS); |
pmic | 29:335fb9b01ca7 | 160 | float desiredRotationMotion = motion.getPosition(); |
pmic | 29:335fb9b01ca7 | 161 | float desiredVelocityMotion = motion.getVelocity(); |
pmic | 29:335fb9b01ca7 | 162 | |
pmic | 8:6b747ad59ff5 | 163 | // calculate motor phase voltages |
pmic | 30:05abc1d2a2b9 | 164 | desiredSpeed = p *(desiredRotationMotion - 60.0f * actualRotation) + 0.0f * desiredVelocityMotion; // using velocity feedforward here will lead to overshoot |
pmic | 29:335fb9b01ca7 | 165 | |
pmic | 8:6b747ad59ff5 | 166 | if (desiredSpeed < -max_speed) desiredSpeed = -max_speed; |
pmic | 8:6b747ad59ff5 | 167 | else if (desiredSpeed > max_speed) desiredSpeed = max_speed; |
pmic | 29:335fb9b01ca7 | 168 | |
pmic | 8:6b747ad59ff5 | 169 | float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn; |
pmic | 8:6b747ad59ff5 | 170 | // calculate, limit and set duty cycles |
pmic | 8:6b747ad59ff5 | 171 | float dutyCycle = 0.5f + 0.5f*voltage/max_voltage; |
pmic | 8:6b747ad59ff5 | 172 | if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE; |
pmic | 8:6b747ad59ff5 | 173 | else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE; |
pmic | 8:6b747ad59ff5 | 174 | pwm.write(static_cast<double>(dutyCycle)); |
pmic | 8:6b747ad59ff5 | 175 | } |
pmic | 8:6b747ad59ff5 | 176 | } |
pmic | 8:6b747ad59ff5 | 177 | |
pmic | 8:6b747ad59ff5 | 178 | void PositionController::sendThreadFlag() |
pmic | 8:6b747ad59ff5 | 179 | { |
pmic | 8:6b747ad59ff5 | 180 | thread.flags_set(threadFlag); |
pmic | 8:6b747ad59ff5 | 181 | } |