TeamSurface / Mbed 2 deprecated ROME_P3

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

Library/Controller.cpp@1:7bf9b6c007a1, 2018-03-23 (annotated)

Committer:
kueenste
Date:
Fri Mar 23 15:40:09 2018 +0000
Revision:
1:7bf9b6c007a1
Parent:
0:7cf5bf7e9486 
schissdreck funktioniert so halbe

Who changed what in which revision?

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