ROME_Praktikum / Mbed 2 deprecated Rome_P_3

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:   mbed

Controller.cpp@0:20ec9d702676, 2020-03-31 (annotated)

Committer:
Jacqueline
Date:
Tue Mar 31 11:58:30 2020 +0000
Revision:
0:20ec9d702676
Praktikum_3

Who changed what in which revision?

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