Marco Oehler / Mbed OS Lab3

ROME2 Lab3

Controller.cpp@2:fc9e2aebf9d5, 2020-03-25 (annotated)

Committer:
oehlemar
Date:
Wed Mar 25 14:15:52 2020 +0000
Revision:
2:fc9e2aebf9d5
Parent:
0:6a4d3264c067 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:6a4d3264c067 1 /*
oehlemar 0:6a4d3264c067 2 * Controller.cpp
oehlemar 0:6a4d3264c067 3 * Copyright (c) 2020, ZHAW
oehlemar 0:6a4d3264c067 4 * All rights reserved.
oehlemar 0:6a4d3264c067 5 */
oehlemar 0:6a4d3264c067 6
oehlemar 0:6a4d3264c067 7 #include "Controller.h"
oehlemar 0:6a4d3264c067 8
oehlemar 0:6a4d3264c067 9 using namespace std;
oehlemar 0:6a4d3264c067 10
oehlemar 0:6a4d3264c067 11 const float Controller::PERIOD = 0.001f; // period of 1 ms
oehlemar 0:6a4d3264c067 12 const float Controller::PI = 3.14159265f; // the constant PI
oehlemar 0:6a4d3264c067 13 const float Controller::WHEEL_DISTANCE = 0.185f; // distance between wheels, given in [m]
oehlemar 0:6a4d3264c067 14 const float Controller::WHEEL_RADIUS = 0.038f; // radius of wheels, given in [m]
oehlemar 0:6a4d3264c067 15 const float Controller::COUNTS_PER_TURN = 86016.0f; // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
oehlemar 0:6a4d3264c067 16 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
oehlemar 0:6a4d3264c067 17 const float Controller::KN = 45.0f; // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
oehlemar 0:6a4d3264c067 18 const float Controller::KP = 0.1f; // speed control parameter
oehlemar 0:6a4d3264c067 19 const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
oehlemar 0:6a4d3264c067 20 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
oehlemar 0:6a4d3264c067 21 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
oehlemar 0:6a4d3264c067 22
oehlemar 0:6a4d3264c067 23 /**
oehlemar 0:6a4d3264c067 24 * Creates and initialises the robot controller.
oehlemar 0:6a4d3264c067 25 * @param pwmLeft a reference to the pwm output for the left motor.
oehlemar 0:6a4d3264c067 26 * @param pwmRight a reference to the pwm output for the right motor.
oehlemar 0:6a4d3264c067 27 * @param counterLeft a reference to the encoder counter of the left motor.
oehlemar 0:6a4d3264c067 28 * @param counterRight a reference to the encoder counter of the right motor.
oehlemar 0:6a4d3264c067 29 */
oehlemar 0:6a4d3264c067 30 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
oehlemar 0:6a4d3264c067 31
oehlemar 0:6a4d3264c067 32 // initialise pwm outputs
oehlemar 0:6a4d3264c067 33
oehlemar 0:6a4d3264c067 34 pwmLeft.period(0.00005f); // pwm period of 50 us
oehlemar 0:6a4d3264c067 35 pwmLeft = 0.5f; // duty-cycle of 50%
oehlemar 0:6a4d3264c067 36
oehlemar 0:6a4d3264c067 37 pwmRight.period(0.00005f); // pwm period of 50 us
oehlemar 0:6a4d3264c067 38 pwmRight = 0.5f; // duty-cycle of 50%
oehlemar 0:6a4d3264c067 39
oehlemar 0:6a4d3264c067 40 // initialise local variables
oehlemar 0:6a4d3264c067 41
oehlemar 0:6a4d3264c067 42 translationalMotion.setProfileVelocity(2.0f);
oehlemar 0:6a4d3264c067 43 translationalMotion.setProfileAcceleration(2.0f);
oehlemar 0:6a4d3264c067 44 translationalMotion.setProfileDeceleration(4.0f);
oehlemar 0:6a4d3264c067 45
oehlemar 0:6a4d3264c067 46 rotationalMotion.setProfileVelocity(6.0f);
oehlemar 0:6a4d3264c067 47 rotationalMotion.setProfileAcceleration(12.0f);
oehlemar 0:6a4d3264c067 48 rotationalMotion.setProfileDeceleration(12.0f);
oehlemar 0:6a4d3264c067 49
oehlemar 0:6a4d3264c067 50 translationalVelocity = 0.0f;
oehlemar 0:6a4d3264c067 51 rotationalVelocity = 0.0f;
oehlemar 0:6a4d3264c067 52 actualTranslationalVelocity = 0.0f;
oehlemar 0:6a4d3264c067 53 actualRotationalVelocity = 0.0f;
oehlemar 0:6a4d3264c067 54
oehlemar 0:6a4d3264c067 55 previousValueCounterLeft = counterLeft.read();
oehlemar 0:6a4d3264c067 56 previousValueCounterRight = counterRight.read();
oehlemar 0:6a4d3264c067 57
oehlemar 0:6a4d3264c067 58 speedLeftFilter.setPeriod(PERIOD);
oehlemar 0:6a4d3264c067 59 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
oehlemar 0:6a4d3264c067 60
oehlemar 0:6a4d3264c067 61 speedRightFilter.setPeriod(PERIOD);
oehlemar 0:6a4d3264c067 62 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
oehlemar 0:6a4d3264c067 63
oehlemar 0:6a4d3264c067 64 desiredSpeedLeft = 0.0f;
oehlemar 0:6a4d3264c067 65 desiredSpeedRight = 0.0f;
oehlemar 0:6a4d3264c067 66
oehlemar 0:6a4d3264c067 67 actualSpeedLeft = 0.0f;
oehlemar 0:6a4d3264c067 68 actualSpeedRight = 0.0f;
oehlemar 0:6a4d3264c067 69
oehlemar 0:6a4d3264c067 70 x = 0.0f;
oehlemar 0:6a4d3264c067 71 y = 0.0f;
oehlemar 0:6a4d3264c067 72 alpha = 0.0f;
oehlemar 0:6a4d3264c067 73
oehlemar 0:6a4d3264c067 74 // start the periodic task
oehlemar 0:6a4d3264c067 75
oehlemar 0:6a4d3264c067 76 ticker.attach(callback(this, &Controller::run), PERIOD);
oehlemar 0:6a4d3264c067 77 }
oehlemar 0:6a4d3264c067 78
oehlemar 0:6a4d3264c067 79 /**
oehlemar 0:6a4d3264c067 80 * Deletes this Controller object.
oehlemar 0:6a4d3264c067 81 */
oehlemar 0:6a4d3264c067 82 Controller::~Controller() {
oehlemar 0:6a4d3264c067 83
oehlemar 0:6a4d3264c067 84 ticker.detach(); // stop the periodic task
oehlemar 0:6a4d3264c067 85 }
oehlemar 0:6a4d3264c067 86
oehlemar 0:6a4d3264c067 87 /**
oehlemar 0:6a4d3264c067 88 * Sets the desired translational velocity of the robot.
oehlemar 0:6a4d3264c067 89 * @param velocity the desired translational velocity, given in [m/s].
oehlemar 0:6a4d3264c067 90 */
oehlemar 0:6a4d3264c067 91 void Controller::setTranslationalVelocity(float velocity) {
oehlemar 0:6a4d3264c067 92
oehlemar 0:6a4d3264c067 93 this->translationalVelocity = velocity;
oehlemar 0:6a4d3264c067 94 }
oehlemar 0:6a4d3264c067 95
oehlemar 0:6a4d3264c067 96 /**
oehlemar 0:6a4d3264c067 97 * Sets the desired rotational velocity of the robot.
oehlemar 0:6a4d3264c067 98 * @param velocity the desired rotational velocity, given in [rad/s].
oehlemar 0:6a4d3264c067 99 */
oehlemar 0:6a4d3264c067 100 void Controller::setRotationalVelocity(float velocity) {
oehlemar 0:6a4d3264c067 101
oehlemar 0:6a4d3264c067 102 this->rotationalVelocity = velocity;
oehlemar 0:6a4d3264c067 103 }
oehlemar 0:6a4d3264c067 104
oehlemar 0:6a4d3264c067 105 /**
oehlemar 0:6a4d3264c067 106 * Gets the actual translational velocity of the robot.
oehlemar 0:6a4d3264c067 107 * @return the actual translational velocity, given in [m/s].
oehlemar 0:6a4d3264c067 108 */
oehlemar 0:6a4d3264c067 109 float Controller::getActualTranslationalVelocity() {
oehlemar 0:6a4d3264c067 110
oehlemar 0:6a4d3264c067 111 return actualTranslationalVelocity;
oehlemar 0:6a4d3264c067 112 }
oehlemar 0:6a4d3264c067 113
oehlemar 0:6a4d3264c067 114 /**
oehlemar 0:6a4d3264c067 115 * Gets the actual rotational velocity of the robot.
oehlemar 0:6a4d3264c067 116 * @return the actual rotational velocity, given in [rad/s].
oehlemar 0:6a4d3264c067 117 */
oehlemar 0:6a4d3264c067 118 float Controller::getActualRotationalVelocity() {
oehlemar 0:6a4d3264c067 119
oehlemar 0:6a4d3264c067 120 return actualRotationalVelocity;
oehlemar 0:6a4d3264c067 121 }
oehlemar 0:6a4d3264c067 122
oehlemar 0:6a4d3264c067 123 /**
oehlemar 0:6a4d3264c067 124 * Sets the desired speed of the left motor.
oehlemar 0:6a4d3264c067 125 * @param desiredSpeedLeft desired speed given in [rpm].
oehlemar 0:6a4d3264c067 126 */
oehlemar 0:6a4d3264c067 127 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
oehlemar 0:6a4d3264c067 128
oehlemar 0:6a4d3264c067 129 this->desiredSpeedLeft = desiredSpeedLeft;
oehlemar 0:6a4d3264c067 130 }
oehlemar 0:6a4d3264c067 131
oehlemar 0:6a4d3264c067 132 /**
oehlemar 0:6a4d3264c067 133 * Sets the desired speed of the right motor.
oehlemar 0:6a4d3264c067 134 * @param desiredSpeedRight desired speed given in [rpm].
oehlemar 0:6a4d3264c067 135 */
oehlemar 0:6a4d3264c067 136 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
oehlemar 0:6a4d3264c067 137
oehlemar 0:6a4d3264c067 138 this->desiredSpeedRight = desiredSpeedRight;
oehlemar 0:6a4d3264c067 139 }
oehlemar 0:6a4d3264c067 140
oehlemar 0:6a4d3264c067 141 /**
oehlemar 0:6a4d3264c067 142 * Gets the actual speed of the left motor.
oehlemar 0:6a4d3264c067 143 * @return the actual speed given in [rpm].
oehlemar 0:6a4d3264c067 144 */
oehlemar 0:6a4d3264c067 145 float Controller::getActualSpeedLeft() {
oehlemar 0:6a4d3264c067 146
oehlemar 0:6a4d3264c067 147 return actualSpeedLeft;
oehlemar 0:6a4d3264c067 148 }
oehlemar 0:6a4d3264c067 149
oehlemar 0:6a4d3264c067 150 /**
oehlemar 0:6a4d3264c067 151 * Gets the actual speed of the right motor.
oehlemar 0:6a4d3264c067 152 * @return the actual speed given in [rpm].
oehlemar 0:6a4d3264c067 153 */
oehlemar 0:6a4d3264c067 154 float Controller::getActualSpeedRight() {
oehlemar 0:6a4d3264c067 155
oehlemar 0:6a4d3264c067 156 return actualSpeedRight;
oehlemar 0:6a4d3264c067 157 }
oehlemar 0:6a4d3264c067 158
oehlemar 0:6a4d3264c067 159 /**
oehlemar 0:6a4d3264c067 160 * Sets the actual x coordinate of the robots position.
oehlemar 0:6a4d3264c067 161 * @param x the x coordinate of the position, given in [m].
oehlemar 0:6a4d3264c067 162 */
oehlemar 0:6a4d3264c067 163 void Controller::setX(float x) {
oehlemar 0:6a4d3264c067 164
oehlemar 0:6a4d3264c067 165 this->x = x;
oehlemar 0:6a4d3264c067 166 }
oehlemar 0:6a4d3264c067 167
oehlemar 0:6a4d3264c067 168 /**
oehlemar 0:6a4d3264c067 169 * Gets the actual x coordinate of the robots position.
oehlemar 0:6a4d3264c067 170 * @return the x coordinate of the position, given in [m].
oehlemar 0:6a4d3264c067 171 */
oehlemar 0:6a4d3264c067 172 float Controller::getX() {
oehlemar 0:6a4d3264c067 173
oehlemar 0:6a4d3264c067 174 return x;
oehlemar 0:6a4d3264c067 175 }
oehlemar 0:6a4d3264c067 176
oehlemar 0:6a4d3264c067 177 /**
oehlemar 0:6a4d3264c067 178 * Sets the actual y coordinate of the robots position.
oehlemar 0:6a4d3264c067 179 * @param y the y coordinate of the position, given in [m].
oehlemar 0:6a4d3264c067 180 */
oehlemar 0:6a4d3264c067 181 void Controller::setY(float y) {
oehlemar 0:6a4d3264c067 182
oehlemar 0:6a4d3264c067 183 this->y = y;
oehlemar 0:6a4d3264c067 184 }
oehlemar 0:6a4d3264c067 185
oehlemar 0:6a4d3264c067 186 /**
oehlemar 0:6a4d3264c067 187 * Gets the actual y coordinate of the robots position.
oehlemar 0:6a4d3264c067 188 * @return the y coordinate of the position, given in [m].
oehlemar 0:6a4d3264c067 189 */
oehlemar 0:6a4d3264c067 190 float Controller::getY() {
oehlemar 0:6a4d3264c067 191
oehlemar 0:6a4d3264c067 192 return y;
oehlemar 0:6a4d3264c067 193 }
oehlemar 0:6a4d3264c067 194
oehlemar 0:6a4d3264c067 195 /**
oehlemar 0:6a4d3264c067 196 * Sets the actual orientation of the robot.
oehlemar 0:6a4d3264c067 197 * @param alpha the orientation, given in [rad].
oehlemar 0:6a4d3264c067 198 */
oehlemar 0:6a4d3264c067 199 void Controller::setAlpha(float alpha) {
oehlemar 0:6a4d3264c067 200
oehlemar 0:6a4d3264c067 201 this->alpha = alpha;
oehlemar 0:6a4d3264c067 202 }
oehlemar 0:6a4d3264c067 203
oehlemar 0:6a4d3264c067 204 /**
oehlemar 0:6a4d3264c067 205 * Gets the actual orientation of the robot.
oehlemar 0:6a4d3264c067 206 * @return the orientation, given in [rad].
oehlemar 0:6a4d3264c067 207 */
oehlemar 0:6a4d3264c067 208 float Controller::getAlpha() {
oehlemar 0:6a4d3264c067 209
oehlemar 0:6a4d3264c067 210 return alpha;
oehlemar 0:6a4d3264c067 211 }
oehlemar 0:6a4d3264c067 212
oehlemar 0:6a4d3264c067 213 /**
oehlemar 0:6a4d3264c067 214 * This is an internal method of the controller that is running periodically.
oehlemar 0:6a4d3264c067 215 */
oehlemar 0:6a4d3264c067 216 void Controller::run() {
oehlemar 0:6a4d3264c067 217
oehlemar 0:6a4d3264c067 218 // calculate the planned velocities using the motion planner
oehlemar 0:6a4d3264c067 219
oehlemar 0:6a4d3264c067 220 translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
oehlemar 0:6a4d3264c067 221 rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
oehlemar 0:6a4d3264c067 222
oehlemar 0:6a4d3264c067 223 // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
oehlemar 0:6a4d3264c067 224
oehlemar 0:6a4d3264c067 225 desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
oehlemar 0:6a4d3264c067 226 desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
oehlemar 0:6a4d3264c067 227
oehlemar 0:6a4d3264c067 228 // calculate the actual speed of the motors in [rpm]
oehlemar 0:6a4d3264c067 229
oehlemar 0:6a4d3264c067 230 short valueCounterLeft = counterLeft.read();
oehlemar 0:6a4d3264c067 231 short valueCounterRight = counterRight.read();
oehlemar 0:6a4d3264c067 232
oehlemar 0:6a4d3264c067 233 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
oehlemar 0:6a4d3264c067 234 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
oehlemar 0:6a4d3264c067 235
oehlemar 0:6a4d3264c067 236 previousValueCounterLeft = valueCounterLeft;
oehlemar 0:6a4d3264c067 237 previousValueCounterRight = valueCounterRight;
oehlemar 0:6a4d3264c067 238
oehlemar 0:6a4d3264c067 239 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
oehlemar 0:6a4d3264c067 240 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
oehlemar 0:6a4d3264c067 241
oehlemar 0:6a4d3264c067 242 // calculate desired motor voltages Uout
oehlemar 0:6a4d3264c067 243
oehlemar 0:6a4d3264c067 244 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
oehlemar 0:6a4d3264c067 245 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
oehlemar 0:6a4d3264c067 246
oehlemar 0:6a4d3264c067 247 // calculate, limit and set the duty-cycle
oehlemar 0:6a4d3264c067 248
oehlemar 0:6a4d3264c067 249 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
oehlemar 0:6a4d3264c067 250 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
oehlemar 0:6a4d3264c067 251 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
oehlemar 0:6a4d3264c067 252 pwmLeft = dutyCycleLeft;
oehlemar 0:6a4d3264c067 253
oehlemar 0:6a4d3264c067 254 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
oehlemar 0:6a4d3264c067 255 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
oehlemar 0:6a4d3264c067 256 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
oehlemar 0:6a4d3264c067 257 pwmRight = dutyCycleRight;
oehlemar 0:6a4d3264c067 258
oehlemar 0:6a4d3264c067 259 // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
oehlemar 0:6a4d3264c067 260
oehlemar 0:6a4d3264c067 261 actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
oehlemar 0:6a4d3264c067 262 actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
oehlemar 0:6a4d3264c067 263
oehlemar 0:6a4d3264c067 264 // calculate the actual robot pose
oehlemar 0:6a4d3264c067 265
oehlemar 0:6a4d3264c067 266 float deltaTranslation = translationalMotion.velocity*PERIOD; // with a real robot: actualTranslationalVelocity*PERIOD
oehlemar 0:6a4d3264c067 267 float deltaOrientation = rotationalMotion.velocity*PERIOD; // with a real robot: actualRotationalVelocity*PERIOD
oehlemar 0:6a4d3264c067 268
oehlemar 0:6a4d3264c067 269 float sinAlpha = sin(alpha+deltaOrientation);
oehlemar 0:6a4d3264c067 270 float cosAlpha = cos(alpha+deltaOrientation);
oehlemar 0:6a4d3264c067 271
oehlemar 0:6a4d3264c067 272 x += cosAlpha*deltaTranslation;
oehlemar 0:6a4d3264c067 273 y += sinAlpha*deltaTranslation;
oehlemar 0:6a4d3264c067 274 float alpha = this->alpha+deltaOrientation;
oehlemar 0:6a4d3264c067 275
oehlemar 0:6a4d3264c067 276 while (alpha > PI) alpha -= 2.0f*PI;
oehlemar 0:6a4d3264c067 277 while (alpha < -PI) alpha += 2.0f*PI;
oehlemar 0:6a4d3264c067 278
oehlemar 0:6a4d3264c067 279 this->alpha = alpha;
oehlemar 0:6a4d3264c067 280 }
oehlemar 0:6a4d3264c067 281