Nim leo niiiim
Controller.cpp@0:da791f233257, 2018-05-11 (annotated)
- Committer:
- Kiwicjam
- Date:
- Fri May 11 12:21:19 2018 +0000
- Revision:
- 0:da791f233257
start of rome2 p5;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kiwicjam | 0:da791f233257 | 1 | /* |
Kiwicjam | 0:da791f233257 | 2 | * Controller.cpp |
Kiwicjam | 0:da791f233257 | 3 | * Copyright (c) 2018, ZHAW |
Kiwicjam | 0:da791f233257 | 4 | * All rights reserved. |
Kiwicjam | 0:da791f233257 | 5 | */ |
Kiwicjam | 0:da791f233257 | 6 | |
Kiwicjam | 0:da791f233257 | 7 | #include <cmath> |
Kiwicjam | 0:da791f233257 | 8 | #include "Controller.h" |
Kiwicjam | 0:da791f233257 | 9 | |
Kiwicjam | 0:da791f233257 | 10 | using namespace std; |
Kiwicjam | 0:da791f233257 | 11 | |
Kiwicjam | 0:da791f233257 | 12 | const float Controller::PERIOD = 0.001f; // period of control task, given in [s] |
Kiwicjam | 0:da791f233257 | 13 | const float Controller::PI = 3.14159265f; // the constant PI |
Kiwicjam | 0:da791f233257 | 14 | const float Controller::WHEEL_DISTANCE = 0.170f; // distance between wheels, given in [m] |
Kiwicjam | 0:da791f233257 | 15 | const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m] |
Kiwicjam | 0:da791f233257 | 16 | const float Controller::COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter |
Kiwicjam | 0:da791f233257 | 17 | const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s] |
Kiwicjam | 0:da791f233257 | 18 | const float Controller::KN = 40.0f; // speed constant of motor, given in [rpm/V] |
Kiwicjam | 0:da791f233257 | 19 | const float Controller::KP = 0.2f; // speed controller gain, given in [V/rpm] |
Kiwicjam | 0:da791f233257 | 20 | const float Controller::MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V] |
Kiwicjam | 0:da791f233257 | 21 | const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%) |
Kiwicjam | 0:da791f233257 | 22 | const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%) |
Kiwicjam | 0:da791f233257 | 23 | |
Kiwicjam | 0:da791f233257 | 24 | /** |
Kiwicjam | 0:da791f233257 | 25 | * Creates and initializes a Controller object. |
Kiwicjam | 0:da791f233257 | 26 | * @param pwmLeft a pwm output object to set the duty cycle for the left motor. |
Kiwicjam | 0:da791f233257 | 27 | * @param pwmRight a pwm output object to set the duty cycle for the right motor. |
Kiwicjam | 0:da791f233257 | 28 | * @param counterLeft an encoder counter object to read the position of the left motor. |
Kiwicjam | 0:da791f233257 | 29 | * @param counterRight an encoder counter object to read the position of the right motor. |
Kiwicjam | 0:da791f233257 | 30 | */ |
Kiwicjam | 0:da791f233257 | 31 | Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) { |
Kiwicjam | 0:da791f233257 | 32 | |
Kiwicjam | 0:da791f233257 | 33 | // initialize periphery drivers |
Kiwicjam | 0:da791f233257 | 34 | |
Kiwicjam | 0:da791f233257 | 35 | pwmLeft.period(0.00005f); |
Kiwicjam | 0:da791f233257 | 36 | pwmLeft.write(0.5f); |
Kiwicjam | 0:da791f233257 | 37 | |
Kiwicjam | 0:da791f233257 | 38 | pwmRight.period(0.00005f); |
Kiwicjam | 0:da791f233257 | 39 | pwmRight.write(0.5f); |
Kiwicjam | 0:da791f233257 | 40 | |
Kiwicjam | 0:da791f233257 | 41 | // initialize local variables |
Kiwicjam | 0:da791f233257 | 42 | |
Kiwicjam | 0:da791f233257 | 43 | translationalMotion.setProfileVelocity(1.5f); |
Kiwicjam | 0:da791f233257 | 44 | translationalMotion.setProfileAcceleration(1.5f); |
Kiwicjam | 0:da791f233257 | 45 | translationalMotion.setProfileDeceleration(3.0f); |
Kiwicjam | 0:da791f233257 | 46 | |
Kiwicjam | 0:da791f233257 | 47 | rotationalMotion.setProfileVelocity(3.0f); |
Kiwicjam | 0:da791f233257 | 48 | rotationalMotion.setProfileAcceleration(15.0f); |
Kiwicjam | 0:da791f233257 | 49 | rotationalMotion.setProfileDeceleration(15.0f); |
Kiwicjam | 0:da791f233257 | 50 | |
Kiwicjam | 0:da791f233257 | 51 | translationalVelocity = 0.0f; |
Kiwicjam | 0:da791f233257 | 52 | rotationalVelocity = 0.0f; |
Kiwicjam | 0:da791f233257 | 53 | actualTranslationalVelocity = 0.0f; |
Kiwicjam | 0:da791f233257 | 54 | actualRotationalVelocity = 0.0f; |
Kiwicjam | 0:da791f233257 | 55 | |
Kiwicjam | 0:da791f233257 | 56 | previousValueCounterLeft = counterLeft.read(); |
Kiwicjam | 0:da791f233257 | 57 | previousValueCounterRight = counterRight.read(); |
Kiwicjam | 0:da791f233257 | 58 | |
Kiwicjam | 0:da791f233257 | 59 | speedLeftFilter.setPeriod(PERIOD); |
Kiwicjam | 0:da791f233257 | 60 | speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
Kiwicjam | 0:da791f233257 | 61 | |
Kiwicjam | 0:da791f233257 | 62 | speedRightFilter.setPeriod(PERIOD); |
Kiwicjam | 0:da791f233257 | 63 | speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
Kiwicjam | 0:da791f233257 | 64 | |
Kiwicjam | 0:da791f233257 | 65 | desiredSpeedLeft = 0.0f; |
Kiwicjam | 0:da791f233257 | 66 | desiredSpeedRight = 0.0f; |
Kiwicjam | 0:da791f233257 | 67 | |
Kiwicjam | 0:da791f233257 | 68 | actualSpeedLeft = 0.0f; |
Kiwicjam | 0:da791f233257 | 69 | actualSpeedRight = 0.0f; |
Kiwicjam | 0:da791f233257 | 70 | |
Kiwicjam | 0:da791f233257 | 71 | x = 0.0f; |
Kiwicjam | 0:da791f233257 | 72 | y = 0.0f; |
Kiwicjam | 0:da791f233257 | 73 | alpha = 0.0f; |
Kiwicjam | 0:da791f233257 | 74 | |
Kiwicjam | 0:da791f233257 | 75 | // start periodic task |
Kiwicjam | 0:da791f233257 | 76 | |
Kiwicjam | 0:da791f233257 | 77 | ticker.attach(callback(this, &Controller::run), PERIOD); |
Kiwicjam | 0:da791f233257 | 78 | } |
Kiwicjam | 0:da791f233257 | 79 | |
Kiwicjam | 0:da791f233257 | 80 | /** |
Kiwicjam | 0:da791f233257 | 81 | * Deletes the Controller object and releases all allocated resources. |
Kiwicjam | 0:da791f233257 | 82 | */ |
Kiwicjam | 0:da791f233257 | 83 | Controller::~Controller() { |
Kiwicjam | 0:da791f233257 | 84 | |
Kiwicjam | 0:da791f233257 | 85 | ticker.detach(); |
Kiwicjam | 0:da791f233257 | 86 | } |
Kiwicjam | 0:da791f233257 | 87 | |
Kiwicjam | 0:da791f233257 | 88 | /** |
Kiwicjam | 0:da791f233257 | 89 | * Sets the desired translational velocity of the robot. |
Kiwicjam | 0:da791f233257 | 90 | * @param velocity the desired translational velocity, given in [m/s]. |
Kiwicjam | 0:da791f233257 | 91 | */ |
Kiwicjam | 0:da791f233257 | 92 | void Controller::setTranslationalVelocity(float velocity) { |
Kiwicjam | 0:da791f233257 | 93 | |
Kiwicjam | 0:da791f233257 | 94 | this->translationalVelocity = velocity; |
Kiwicjam | 0:da791f233257 | 95 | } |
Kiwicjam | 0:da791f233257 | 96 | |
Kiwicjam | 0:da791f233257 | 97 | /** |
Kiwicjam | 0:da791f233257 | 98 | * Sets the desired rotational velocity of the robot. |
Kiwicjam | 0:da791f233257 | 99 | * @param velocity the desired rotational velocity, given in [rad/s]. |
Kiwicjam | 0:da791f233257 | 100 | */ |
Kiwicjam | 0:da791f233257 | 101 | void Controller::setRotationalVelocity(float velocity) { |
Kiwicjam | 0:da791f233257 | 102 | |
Kiwicjam | 0:da791f233257 | 103 | this->rotationalVelocity = velocity; |
Kiwicjam | 0:da791f233257 | 104 | } |
Kiwicjam | 0:da791f233257 | 105 | |
Kiwicjam | 0:da791f233257 | 106 | /** |
Kiwicjam | 0:da791f233257 | 107 | * Gets the actual translational velocity of the robot. |
Kiwicjam | 0:da791f233257 | 108 | * @return the actual translational velocity, given in [m/s]. |
Kiwicjam | 0:da791f233257 | 109 | */ |
Kiwicjam | 0:da791f233257 | 110 | float Controller::getActualTranslationalVelocity() { |
Kiwicjam | 0:da791f233257 | 111 | |
Kiwicjam | 0:da791f233257 | 112 | return actualTranslationalVelocity; |
Kiwicjam | 0:da791f233257 | 113 | } |
Kiwicjam | 0:da791f233257 | 114 | |
Kiwicjam | 0:da791f233257 | 115 | /** |
Kiwicjam | 0:da791f233257 | 116 | * Gets the actual rotational velocity of the robot. |
Kiwicjam | 0:da791f233257 | 117 | * @return the actual rotational velocity, given in [rad/s]. |
Kiwicjam | 0:da791f233257 | 118 | */ |
Kiwicjam | 0:da791f233257 | 119 | float Controller::getActualRotationalVelocity() { |
Kiwicjam | 0:da791f233257 | 120 | |
Kiwicjam | 0:da791f233257 | 121 | return actualRotationalVelocity; |
Kiwicjam | 0:da791f233257 | 122 | } |
Kiwicjam | 0:da791f233257 | 123 | |
Kiwicjam | 0:da791f233257 | 124 | /** |
Kiwicjam | 0:da791f233257 | 125 | * Sets the actual x coordinate of the robots position. |
Kiwicjam | 0:da791f233257 | 126 | * @param x the x coordinate of the position, given in [m]. |
Kiwicjam | 0:da791f233257 | 127 | */ |
Kiwicjam | 0:da791f233257 | 128 | void Controller::setX(float x) { |
Kiwicjam | 0:da791f233257 | 129 | |
Kiwicjam | 0:da791f233257 | 130 | this->x = x; |
Kiwicjam | 0:da791f233257 | 131 | } |
Kiwicjam | 0:da791f233257 | 132 | |
Kiwicjam | 0:da791f233257 | 133 | /** |
Kiwicjam | 0:da791f233257 | 134 | * Gets the actual x coordinate of the robots position. |
Kiwicjam | 0:da791f233257 | 135 | * @return the x coordinate of the position, given in [m]. |
Kiwicjam | 0:da791f233257 | 136 | */ |
Kiwicjam | 0:da791f233257 | 137 | float Controller::getX() { |
Kiwicjam | 0:da791f233257 | 138 | |
Kiwicjam | 0:da791f233257 | 139 | return x; |
Kiwicjam | 0:da791f233257 | 140 | } |
Kiwicjam | 0:da791f233257 | 141 | |
Kiwicjam | 0:da791f233257 | 142 | /** |
Kiwicjam | 0:da791f233257 | 143 | * Sets the actual y coordinate of the robots position. |
Kiwicjam | 0:da791f233257 | 144 | * @param y the y coordinate of the position, given in [m]. |
Kiwicjam | 0:da791f233257 | 145 | */ |
Kiwicjam | 0:da791f233257 | 146 | void Controller::setY(float y) { |
Kiwicjam | 0:da791f233257 | 147 | |
Kiwicjam | 0:da791f233257 | 148 | this->y = y; |
Kiwicjam | 0:da791f233257 | 149 | } |
Kiwicjam | 0:da791f233257 | 150 | |
Kiwicjam | 0:da791f233257 | 151 | /** |
Kiwicjam | 0:da791f233257 | 152 | * Gets the actual y coordinate of the robots position. |
Kiwicjam | 0:da791f233257 | 153 | * @return the y coordinate of the position, given in [m]. |
Kiwicjam | 0:da791f233257 | 154 | */ |
Kiwicjam | 0:da791f233257 | 155 | float Controller::getY() { |
Kiwicjam | 0:da791f233257 | 156 | |
Kiwicjam | 0:da791f233257 | 157 | return y; |
Kiwicjam | 0:da791f233257 | 158 | } |
Kiwicjam | 0:da791f233257 | 159 | |
Kiwicjam | 0:da791f233257 | 160 | /** |
Kiwicjam | 0:da791f233257 | 161 | * Sets the actual orientation of the robot. |
Kiwicjam | 0:da791f233257 | 162 | * @param alpha the orientation, given in [rad]. |
Kiwicjam | 0:da791f233257 | 163 | */ |
Kiwicjam | 0:da791f233257 | 164 | void Controller::setAlpha(float alpha) { |
Kiwicjam | 0:da791f233257 | 165 | |
Kiwicjam | 0:da791f233257 | 166 | this->alpha = alpha; |
Kiwicjam | 0:da791f233257 | 167 | } |
Kiwicjam | 0:da791f233257 | 168 | |
Kiwicjam | 0:da791f233257 | 169 | /** |
Kiwicjam | 0:da791f233257 | 170 | * Gets the actual orientation of the robot. |
Kiwicjam | 0:da791f233257 | 171 | * @return the orientation, given in [rad]. |
Kiwicjam | 0:da791f233257 | 172 | */ |
Kiwicjam | 0:da791f233257 | 173 | float Controller::getAlpha() { |
Kiwicjam | 0:da791f233257 | 174 | |
Kiwicjam | 0:da791f233257 | 175 | return alpha; |
Kiwicjam | 0:da791f233257 | 176 | } |
Kiwicjam | 0:da791f233257 | 177 | |
Kiwicjam | 0:da791f233257 | 178 | /** |
Kiwicjam | 0:da791f233257 | 179 | * This method is called periodically by the ticker object and contains the |
Kiwicjam | 0:da791f233257 | 180 | * algorithm of the speed controller. |
Kiwicjam | 0:da791f233257 | 181 | */ |
Kiwicjam | 0:da791f233257 | 182 | void Controller::run() { |
Kiwicjam | 0:da791f233257 | 183 | |
Kiwicjam | 0:da791f233257 | 184 | // calculate the planned velocities using the motion planner |
Kiwicjam | 0:da791f233257 | 185 | |
Kiwicjam | 0:da791f233257 | 186 | translationalMotion.incrementToVelocity(translationalVelocity, PERIOD); |
Kiwicjam | 0:da791f233257 | 187 | rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD); |
Kiwicjam | 0:da791f233257 | 188 | |
Kiwicjam | 0:da791f233257 | 189 | // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model |
Kiwicjam | 0:da791f233257 | 190 | |
Kiwicjam | 0:da791f233257 | 191 | desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI; |
Kiwicjam | 0:da791f233257 | 192 | desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI; |
Kiwicjam | 0:da791f233257 | 193 | |
Kiwicjam | 0:da791f233257 | 194 | // calculate actual speed of motors in [rpm] |
Kiwicjam | 0:da791f233257 | 195 | |
Kiwicjam | 0:da791f233257 | 196 | short valueCounterLeft = counterLeft.read(); |
Kiwicjam | 0:da791f233257 | 197 | short valueCounterRight = counterRight.read(); |
Kiwicjam | 0:da791f233257 | 198 | |
Kiwicjam | 0:da791f233257 | 199 | short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; |
Kiwicjam | 0:da791f233257 | 200 | short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; |
Kiwicjam | 0:da791f233257 | 201 | |
Kiwicjam | 0:da791f233257 | 202 | previousValueCounterLeft = valueCounterLeft; |
Kiwicjam | 0:da791f233257 | 203 | previousValueCounterRight = valueCounterRight; |
Kiwicjam | 0:da791f233257 | 204 | |
Kiwicjam | 0:da791f233257 | 205 | actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f); |
Kiwicjam | 0:da791f233257 | 206 | actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f); |
Kiwicjam | 0:da791f233257 | 207 | |
Kiwicjam | 0:da791f233257 | 208 | // calculate motor phase voltages |
Kiwicjam | 0:da791f233257 | 209 | |
Kiwicjam | 0:da791f233257 | 210 | float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; |
Kiwicjam | 0:da791f233257 | 211 | float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN; |
Kiwicjam | 0:da791f233257 | 212 | |
Kiwicjam | 0:da791f233257 | 213 | // calculate, limit and set duty cycles |
Kiwicjam | 0:da791f233257 | 214 | |
Kiwicjam | 0:da791f233257 | 215 | float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; |
Kiwicjam | 0:da791f233257 | 216 | if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; |
Kiwicjam | 0:da791f233257 | 217 | else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; |
Kiwicjam | 0:da791f233257 | 218 | pwmLeft.write(dutyCycleLeft); |
Kiwicjam | 0:da791f233257 | 219 | |
Kiwicjam | 0:da791f233257 | 220 | float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; |
Kiwicjam | 0:da791f233257 | 221 | if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; |
Kiwicjam | 0:da791f233257 | 222 | else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; |
Kiwicjam | 0:da791f233257 | 223 | pwmRight.write(dutyCycleRight); |
Kiwicjam | 0:da791f233257 | 224 | |
Kiwicjam | 0:da791f233257 | 225 | // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model |
Kiwicjam | 0:da791f233257 | 226 | |
Kiwicjam | 0:da791f233257 | 227 | actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f; |
Kiwicjam | 0:da791f233257 | 228 | actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE; |
Kiwicjam | 0:da791f233257 | 229 | |
Kiwicjam | 0:da791f233257 | 230 | // calculate the actual robot pose |
Kiwicjam | 0:da791f233257 | 231 | |
Kiwicjam | 0:da791f233257 | 232 | float deltaTranslation = actualTranslationalVelocity*PERIOD; |
Kiwicjam | 0:da791f233257 | 233 | float deltaOrientation = actualRotationalVelocity*PERIOD; |
Kiwicjam | 0:da791f233257 | 234 | |
Kiwicjam | 0:da791f233257 | 235 | x += cos(alpha+deltaOrientation)*deltaTranslation; |
Kiwicjam | 0:da791f233257 | 236 | y += sin(alpha+deltaOrientation)*deltaTranslation; |
Kiwicjam | 0:da791f233257 | 237 | float alpha = this->alpha+deltaOrientation; |
Kiwicjam | 0:da791f233257 | 238 | |
Kiwicjam | 0:da791f233257 | 239 | while (alpha > PI) alpha -= 2.0f*PI; |
Kiwicjam | 0:da791f233257 | 240 | while (alpha < -PI) alpha += 2.0f*PI; |
Kiwicjam | 0:da791f233257 | 241 | |
Kiwicjam | 0:da791f233257 | 242 | this->alpha = alpha; |
Kiwicjam | 0:da791f233257 | 243 | } |