ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

Controller.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

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