Marco Oehler / Mbed 2 deprecated Lab2

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:1a972ed770da, 2020-03-09 (annotated)

Committer:
oehlemar
Date:
Mon Mar 09 16:23:04 2020 +0000
Revision:
0:1a972ed770da
LAB2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:1a972ed770da 1 /*
oehlemar 0:1a972ed770da 2 * Controller.cpp
oehlemar 0:1a972ed770da 3 * Copyright (c) 2020, ZHAW
oehlemar 0:1a972ed770da 4 * All rights reserved.
oehlemar 0:1a972ed770da 5 */
oehlemar 0:1a972ed770da 6
oehlemar 0:1a972ed770da 7 #include "Controller.h"
oehlemar 0:1a972ed770da 8
oehlemar 0:1a972ed770da 9 using namespace std;
oehlemar 0:1a972ed770da 10
oehlemar 0:1a972ed770da 11 const float Controller::PERIOD = 0.001f; // period of 1 ms
oehlemar 0:1a972ed770da 12 const float Controller::PI = 3.14159265f; // the constant PI
oehlemar 0:1a972ed770da 13 const float Controller::WHEEL_DISTANCE = 0.185f; // distance between wheels, given in [m]
oehlemar 0:1a972ed770da 14 const float Controller::WHEEL_RADIUS = 0.038f; // radius of wheels, given in [m]
oehlemar 0:1a972ed770da 15 const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
oehlemar 0:1a972ed770da 16 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
oehlemar 0:1a972ed770da 17 const float Controller::KN = 40.0f; // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
oehlemar 0:1a972ed770da 18 const float Controller::KP = 0.25f; // speed control parameter
oehlemar 0:1a972ed770da 19 const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
oehlemar 0:1a972ed770da 20 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
oehlemar 0:1a972ed770da 21 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
oehlemar 0:1a972ed770da 22
oehlemar 0:1a972ed770da 23 /**
oehlemar 0:1a972ed770da 24 * Creates and initialises the robot controller.
oehlemar 0:1a972ed770da 25 * @param pwmLeft a reference to the pwm output for the left motor.
oehlemar 0:1a972ed770da 26 * @param pwmRight a reference to the pwm output for the right motor.
oehlemar 0:1a972ed770da 27 * @param counterLeft a reference to the encoder counter of the left motor.
oehlemar 0:1a972ed770da 28 * @param counterRight a reference to the encoder counter of the right motor.
oehlemar 0:1a972ed770da 29 */
oehlemar 0:1a972ed770da 30 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
oehlemar 0:1a972ed770da 31
oehlemar 0:1a972ed770da 32 // initialise pwm outputs
oehlemar 0:1a972ed770da 33
oehlemar 0:1a972ed770da 34 pwmLeft.period(0.00005f); // pwm period of 50 us
oehlemar 0:1a972ed770da 35 pwmLeft = 0.5f; // duty-cycle of 50%
oehlemar 0:1a972ed770da 36
oehlemar 0:1a972ed770da 37 pwmRight.period(0.00005f); // pwm period of 50 us
oehlemar 0:1a972ed770da 38 pwmRight = 0.5f; // duty-cycle of 50%
oehlemar 0:1a972ed770da 39
oehlemar 0:1a972ed770da 40 // initialise local variables
oehlemar 0:1a972ed770da 41
oehlemar 0:1a972ed770da 42 translationalMotion.setProfileVelocity(1.0f);
oehlemar 0:1a972ed770da 43 translationalMotion.setProfileAcceleration(2.0f);
oehlemar 0:1a972ed770da 44 translationalMotion.setProfileDeceleration(3.0f);
oehlemar 0:1a972ed770da 45
oehlemar 0:1a972ed770da 46 rotationalMotion.setProfileVelocity(1.5f);
oehlemar 0:1a972ed770da 47 rotationalMotion.setProfileAcceleration(20.0f);
oehlemar 0:1a972ed770da 48 rotationalMotion.setProfileDeceleration(20.0f);
oehlemar 0:1a972ed770da 49
oehlemar 0:1a972ed770da 50 translationalVelocity = 0.0f;
oehlemar 0:1a972ed770da 51 rotationalVelocity = 0.0f;
oehlemar 0:1a972ed770da 52 actualTranslationalVelocity = 0.0f;
oehlemar 0:1a972ed770da 53 actualRotationalVelocity = 0.0f;
oehlemar 0:1a972ed770da 54
oehlemar 0:1a972ed770da 55 previousValueCounterLeft = counterLeft.read();
oehlemar 0:1a972ed770da 56 previousValueCounterRight = counterRight.read();
oehlemar 0:1a972ed770da 57
oehlemar 0:1a972ed770da 58 speedLeftFilter.setPeriod(PERIOD);
oehlemar 0:1a972ed770da 59 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
oehlemar 0:1a972ed770da 60
oehlemar 0:1a972ed770da 61 speedRightFilter.setPeriod(PERIOD);
oehlemar 0:1a972ed770da 62 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
oehlemar 0:1a972ed770da 63
oehlemar 0:1a972ed770da 64 desiredSpeedLeft = 0.0f;
oehlemar 0:1a972ed770da 65 desiredSpeedRight = 0.0f;
oehlemar 0:1a972ed770da 66
oehlemar 0:1a972ed770da 67 actualSpeedLeft = 0.0f;
oehlemar 0:1a972ed770da 68 actualSpeedRight = 0.0f;
oehlemar 0:1a972ed770da 69
oehlemar 0:1a972ed770da 70 // start the periodic task
oehlemar 0:1a972ed770da 71
oehlemar 0:1a972ed770da 72 ticker.attach(callback(this, &Controller::run), PERIOD);
oehlemar 0:1a972ed770da 73 }
oehlemar 0:1a972ed770da 74
oehlemar 0:1a972ed770da 75 /**
oehlemar 0:1a972ed770da 76 * Deletes this Controller object.
oehlemar 0:1a972ed770da 77 */
oehlemar 0:1a972ed770da 78 Controller::~Controller() {
oehlemar 0:1a972ed770da 79
oehlemar 0:1a972ed770da 80 ticker.detach(); // stop the periodic task
oehlemar 0:1a972ed770da 81 }
oehlemar 0:1a972ed770da 82
oehlemar 0:1a972ed770da 83 /**
oehlemar 0:1a972ed770da 84 * Sets the desired translational velocity of the robot.
oehlemar 0:1a972ed770da 85 * @param velocity the desired translational velocity, given in [m/s].
oehlemar 0:1a972ed770da 86 */
oehlemar 0:1a972ed770da 87 void Controller::setTranslationalVelocity(float velocity) {
oehlemar 0:1a972ed770da 88
oehlemar 0:1a972ed770da 89 this->translationalVelocity = velocity;
oehlemar 0:1a972ed770da 90 }
oehlemar 0:1a972ed770da 91
oehlemar 0:1a972ed770da 92 /**
oehlemar 0:1a972ed770da 93 * Sets the desired rotational velocity of the robot.
oehlemar 0:1a972ed770da 94 * @param velocity the desired rotational velocity, given in [rad/s].
oehlemar 0:1a972ed770da 95 */
oehlemar 0:1a972ed770da 96 void Controller::setRotationalVelocity(float velocity) {
oehlemar 0:1a972ed770da 97
oehlemar 0:1a972ed770da 98 this->rotationalVelocity = velocity;
oehlemar 0:1a972ed770da 99 }
oehlemar 0:1a972ed770da 100
oehlemar 0:1a972ed770da 101 /**
oehlemar 0:1a972ed770da 102 * Gets the actual translational velocity of the robot.
oehlemar 0:1a972ed770da 103 * @return the actual translational velocity, given in [m/s].
oehlemar 0:1a972ed770da 104 */
oehlemar 0:1a972ed770da 105 float Controller::getActualTranslationalVelocity() {
oehlemar 0:1a972ed770da 106
oehlemar 0:1a972ed770da 107 return actualTranslationalVelocity;
oehlemar 0:1a972ed770da 108 }
oehlemar 0:1a972ed770da 109
oehlemar 0:1a972ed770da 110 /**
oehlemar 0:1a972ed770da 111 * Gets the actual rotational velocity of the robot.
oehlemar 0:1a972ed770da 112 * @return the actual rotational velocity, given in [rad/s].
oehlemar 0:1a972ed770da 113 */
oehlemar 0:1a972ed770da 114 float Controller::getActualRotationalVelocity() {
oehlemar 0:1a972ed770da 115
oehlemar 0:1a972ed770da 116 return actualRotationalVelocity;
oehlemar 0:1a972ed770da 117 }
oehlemar 0:1a972ed770da 118
oehlemar 0:1a972ed770da 119 /**
oehlemar 0:1a972ed770da 120 * Sets the desired speed of the left motor.
oehlemar 0:1a972ed770da 121 * @param desiredSpeedLeft desired speed given in [rpm].
oehlemar 0:1a972ed770da 122 */
oehlemar 0:1a972ed770da 123 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
oehlemar 0:1a972ed770da 124
oehlemar 0:1a972ed770da 125 this->desiredSpeedLeft = desiredSpeedLeft;
oehlemar 0:1a972ed770da 126 }
oehlemar 0:1a972ed770da 127
oehlemar 0:1a972ed770da 128 /**
oehlemar 0:1a972ed770da 129 * Sets the desired speed of the right motor.
oehlemar 0:1a972ed770da 130 * @param desiredSpeedRight desired speed given in [rpm].
oehlemar 0:1a972ed770da 131 */
oehlemar 0:1a972ed770da 132 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
oehlemar 0:1a972ed770da 133
oehlemar 0:1a972ed770da 134 this->desiredSpeedRight = desiredSpeedRight;
oehlemar 0:1a972ed770da 135 }
oehlemar 0:1a972ed770da 136
oehlemar 0:1a972ed770da 137 /**
oehlemar 0:1a972ed770da 138 * Gets the actual speed of the left motor.
oehlemar 0:1a972ed770da 139 * @return the actual speed given in [rpm].
oehlemar 0:1a972ed770da 140 */
oehlemar 0:1a972ed770da 141 float Controller::getActualSpeedLeft() {
oehlemar 0:1a972ed770da 142
oehlemar 0:1a972ed770da 143 return actualSpeedLeft;
oehlemar 0:1a972ed770da 144 }
oehlemar 0:1a972ed770da 145
oehlemar 0:1a972ed770da 146 /**
oehlemar 0:1a972ed770da 147 * Gets the actual speed of the right motor.
oehlemar 0:1a972ed770da 148 * @return the actual speed given in [rpm].
oehlemar 0:1a972ed770da 149 */
oehlemar 0:1a972ed770da 150 float Controller::getActualSpeedRight() {
oehlemar 0:1a972ed770da 151
oehlemar 0:1a972ed770da 152 return actualSpeedRight;
oehlemar 0:1a972ed770da 153 }
oehlemar 0:1a972ed770da 154
oehlemar 0:1a972ed770da 155 /**
oehlemar 0:1a972ed770da 156 * This is an internal method of the controller that is running periodically.
oehlemar 0:1a972ed770da 157 */
oehlemar 0:1a972ed770da 158 void Controller::run() {
oehlemar 0:1a972ed770da 159
oehlemar 0:1a972ed770da 160 // calculate the planned velocities using the motion planner
oehlemar 0:1a972ed770da 161 translationalMotion.incrementToVelocity(translationalVelocity, PERIOD); // Schnelligkeit 0.5 m/s, Periode 0.001 Sek.
oehlemar 0:1a972ed770da 162 rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
oehlemar 0:1a972ed770da 163
oehlemar 0:1a972ed770da 164 // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
oehlemar 0:1a972ed770da 165 desiredSpeedLeft = translationalVelocity - (WHEEL_DISTANCE/2)*rotationalVelocity;
oehlemar 0:1a972ed770da 166 desiredSpeedRight = translationalVelocity + (WHEEL_DISTANCE/2)*rotationalVelocity;
oehlemar 0:1a972ed770da 167
oehlemar 0:1a972ed770da 168 // calculate the actual speed of the motors in [rpm]
oehlemar 0:1a972ed770da 169 desiredSpeedLeft = (60/(2*PI*WHEEL_RADIUS))*desiredSpeedLeft;
oehlemar 0:1a972ed770da 170 desiredSpeedRight = (-60/(2*PI*WHEEL_RADIUS))*desiredSpeedRight;
oehlemar 0:1a972ed770da 171
oehlemar 0:1a972ed770da 172 short valueCounterLeft = counterLeft.read();
oehlemar 0:1a972ed770da 173 short valueCounterRight = counterRight.read();
oehlemar 0:1a972ed770da 174
oehlemar 0:1a972ed770da 175 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
oehlemar 0:1a972ed770da 176 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
oehlemar 0:1a972ed770da 177
oehlemar 0:1a972ed770da 178 previousValueCounterLeft = valueCounterLeft;
oehlemar 0:1a972ed770da 179 previousValueCounterRight = valueCounterRight;
oehlemar 0:1a972ed770da 180
oehlemar 0:1a972ed770da 181 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
oehlemar 0:1a972ed770da 182 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
oehlemar 0:1a972ed770da 183
oehlemar 0:1a972ed770da 184 // calculate desired motor voltages Uout
oehlemar 0:1a972ed770da 185
oehlemar 0:1a972ed770da 186 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
oehlemar 0:1a972ed770da 187 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
oehlemar 0:1a972ed770da 188
oehlemar 0:1a972ed770da 189 // calculate, limit and set the duty-cycle
oehlemar 0:1a972ed770da 190
oehlemar 0:1a972ed770da 191 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
oehlemar 0:1a972ed770da 192 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
oehlemar 0:1a972ed770da 193 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
oehlemar 0:1a972ed770da 194 pwmLeft = dutyCycleLeft;
oehlemar 0:1a972ed770da 195
oehlemar 0:1a972ed770da 196 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
oehlemar 0:1a972ed770da 197 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
oehlemar 0:1a972ed770da 198 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
oehlemar 0:1a972ed770da 199 pwmRight = dutyCycleRight;
oehlemar 0:1a972ed770da 200
oehlemar 0:1a972ed770da 201 // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
oehlemar 0:1a972ed770da 202 actualTranslationalVelocity = 0.5*(actualSpeedLeft+actualSpeedRight);
oehlemar 0:1a972ed770da 203 actualRotationalVelocity = 1/(WHEEL_DISTANCE)*(actualSpeedRight-actualSpeedLeft);
oehlemar 0:1a972ed770da 204 }
oehlemar 0:1a972ed770da 205