TeamSurface / Libs

Nim leo niiiim

Controller.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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