Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

Controller.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Brignall 0:1a0321f1ffbc 1 /*
Brignall 0:1a0321f1ffbc 2 * Controller.cpp
Brignall 0:1a0321f1ffbc 3 * Copyright (c) 2018, ZHAW
Brignall 0:1a0321f1ffbc 4 * All rights reserved.
Brignall 0:1a0321f1ffbc 5 */
Brignall 0:1a0321f1ffbc 6
Brignall 0:1a0321f1ffbc 7 #include <cmath>
Brignall 0:1a0321f1ffbc 8 #include "Controller.h"
Brignall 0:1a0321f1ffbc 9
Brignall 0:1a0321f1ffbc 10 using namespace std;
Brignall 0:1a0321f1ffbc 11
Brignall 0:1a0321f1ffbc 12 const float Controller::PERIOD = 0.001f; // period of control task, given in [s]
Brignall 0:1a0321f1ffbc 13 const float Controller::PI = 3.14159265f; // the constant PI
Brignall 0:1a0321f1ffbc 14 const float Controller::WHEEL_DISTANCE = 0.170f; // distance between wheels, given in [m]
Brignall 0:1a0321f1ffbc 15 const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m]
Brignall 0:1a0321f1ffbc 16 const float Controller::COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
Brignall 0:1a0321f1ffbc 17 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
Brignall 0:1a0321f1ffbc 18 const float Controller::KN = 40.0f; // speed constant of motor, given in [rpm/V]
Brignall 0:1a0321f1ffbc 19 const float Controller::KP = 0.2f; // speed controller gain, given in [V/rpm]
Brignall 0:1a0321f1ffbc 20 const float Controller::MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
Brignall 0:1a0321f1ffbc 21 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
Brignall 0:1a0321f1ffbc 22 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
Brignall 0:1a0321f1ffbc 23 const float Controller::SIGMA_TRANSLATION = 0.0001; // standard deviation of estimated translation per period, given in [m]
Brignall 0:1a0321f1ffbc 24 const float Controller::SIGMA_ORIENTATION = 0.0002; // standard deviation of estimated orientation per period, given in [rad]
Brignall 0:1a0321f1ffbc 25 const float Controller::SIGMA_DISTANCE = 0.01; // standard deviation of distance measurement, given in [m]
Brignall 0:1a0321f1ffbc 26 const float Controller::SIGMA_GAMMA = 0.03; // standard deviation of angle measurement, given in [rad]
Brignall 0:1a0321f1ffbc 27
Brignall 0:1a0321f1ffbc 28 /**
Brignall 0:1a0321f1ffbc 29 * Creates and initializes a Controller object.
Brignall 0:1a0321f1ffbc 30 * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
Brignall 0:1a0321f1ffbc 31 * @param pwmRight a pwm output object to set the duty cycle for the right motor.
Brignall 0:1a0321f1ffbc 32 * @param counterLeft an encoder counter object to read the position of the left motor.
Brignall 0:1a0321f1ffbc 33 * @param counterRight an encoder counter object to read the position of the right motor.
Brignall 0:1a0321f1ffbc 34 */
Brignall 0:1a0321f1ffbc 35 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
Brignall 0:1a0321f1ffbc 36
Brignall 0:1a0321f1ffbc 37 // initialize periphery drivers
Brignall 0:1a0321f1ffbc 38
Brignall 0:1a0321f1ffbc 39 pwmLeft.period(0.00005f);
Brignall 0:1a0321f1ffbc 40 pwmLeft.write(0.5f);
Brignall 0:1a0321f1ffbc 41
Brignall 0:1a0321f1ffbc 42 pwmRight.period(0.00005f);
Brignall 0:1a0321f1ffbc 43 pwmRight.write(0.5f);
Brignall 0:1a0321f1ffbc 44
Brignall 0:1a0321f1ffbc 45 // initialize local variables
Brignall 0:1a0321f1ffbc 46
Brignall 0:1a0321f1ffbc 47 translationalMotion.setProfileVelocity(1.5f);
Brignall 0:1a0321f1ffbc 48 translationalMotion.setProfileAcceleration(1.5f);
Brignall 0:1a0321f1ffbc 49 translationalMotion.setProfileDeceleration(3.0f);
Brignall 0:1a0321f1ffbc 50
Brignall 0:1a0321f1ffbc 51 rotationalMotion.setProfileVelocity(3.0f);
Brignall 0:1a0321f1ffbc 52 rotationalMotion.setProfileAcceleration(15.0f);
Brignall 0:1a0321f1ffbc 53 rotationalMotion.setProfileDeceleration(15.0f);
Brignall 0:1a0321f1ffbc 54
Brignall 0:1a0321f1ffbc 55 translationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 56 rotationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 57 actualTranslationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 58 actualRotationalVelocity = 0.0f;
Brignall 0:1a0321f1ffbc 59
Brignall 0:1a0321f1ffbc 60 previousValueCounterLeft = counterLeft.read();
Brignall 0:1a0321f1ffbc 61 previousValueCounterRight = counterRight.read();
Brignall 0:1a0321f1ffbc 62
Brignall 0:1a0321f1ffbc 63 speedLeftFilter.setPeriod(PERIOD);
Brignall 0:1a0321f1ffbc 64 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Brignall 0:1a0321f1ffbc 65
Brignall 0:1a0321f1ffbc 66 speedRightFilter.setPeriod(PERIOD);
Brignall 0:1a0321f1ffbc 67 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Brignall 0:1a0321f1ffbc 68
Brignall 0:1a0321f1ffbc 69 desiredSpeedLeft = 0.0f;
Brignall 0:1a0321f1ffbc 70 desiredSpeedRight = 0.0f;
Brignall 0:1a0321f1ffbc 71
Brignall 0:1a0321f1ffbc 72 actualSpeedLeft = 0.0f;
Brignall 0:1a0321f1ffbc 73 actualSpeedRight = 0.0f;
Brignall 0:1a0321f1ffbc 74
Brignall 0:1a0321f1ffbc 75 x = 0.0f;
Brignall 0:1a0321f1ffbc 76 y = 0.0f;
Brignall 0:1a0321f1ffbc 77 alpha = 0.0f;
Brignall 0:1a0321f1ffbc 78
Brignall 0:1a0321f1ffbc 79 p[0][0] = 0.001f;
Brignall 0:1a0321f1ffbc 80 p[0][1] = 0.0f;
Brignall 0:1a0321f1ffbc 81 p[0][2] = 0.0f;
Brignall 0:1a0321f1ffbc 82 p[1][0] = 0.0f;
Brignall 0:1a0321f1ffbc 83 p[1][1] = 0.001f;
Brignall 0:1a0321f1ffbc 84 p[1][2] = 0.0f;
Brignall 0:1a0321f1ffbc 85 p[2][0] = 0.0f;
Brignall 0:1a0321f1ffbc 86 p[2][1] = 0.0f;
Brignall 0:1a0321f1ffbc 87 p[2][2] = 0.001f;
Brignall 0:1a0321f1ffbc 88
Brignall 0:1a0321f1ffbc 89 // start periodic task
Brignall 0:1a0321f1ffbc 90
Brignall 0:1a0321f1ffbc 91 ticker.attach(callback(this, &Controller::run), PERIOD);
Brignall 0:1a0321f1ffbc 92 }
Brignall 0:1a0321f1ffbc 93
Brignall 0:1a0321f1ffbc 94 /**
Brignall 0:1a0321f1ffbc 95 * Deletes the Controller object and releases all allocated resources.
Brignall 0:1a0321f1ffbc 96 */
Brignall 0:1a0321f1ffbc 97 Controller::~Controller() {
Brignall 0:1a0321f1ffbc 98
Brignall 0:1a0321f1ffbc 99 ticker.detach();
Brignall 0:1a0321f1ffbc 100 }
Brignall 0:1a0321f1ffbc 101
Brignall 0:1a0321f1ffbc 102 /**
Brignall 0:1a0321f1ffbc 103 * Sets the desired translational velocity of the robot.
Brignall 0:1a0321f1ffbc 104 * @param velocity the desired translational velocity, given in [m/s].
Brignall 0:1a0321f1ffbc 105 */
Brignall 0:1a0321f1ffbc 106 void Controller::setTranslationalVelocity(float velocity) {
Brignall 0:1a0321f1ffbc 107
Brignall 0:1a0321f1ffbc 108 this->translationalVelocity = velocity;
Brignall 0:1a0321f1ffbc 109 }
Brignall 0:1a0321f1ffbc 110
Brignall 0:1a0321f1ffbc 111 /**
Brignall 0:1a0321f1ffbc 112 * Sets the desired rotational velocity of the robot.
Brignall 0:1a0321f1ffbc 113 * @param velocity the desired rotational velocity, given in [rad/s].
Brignall 0:1a0321f1ffbc 114 */
Brignall 0:1a0321f1ffbc 115 void Controller::setRotationalVelocity(float velocity) {
Brignall 0:1a0321f1ffbc 116
Brignall 0:1a0321f1ffbc 117 this->rotationalVelocity = velocity;
Brignall 0:1a0321f1ffbc 118 }
Brignall 0:1a0321f1ffbc 119
Brignall 0:1a0321f1ffbc 120 /**
Brignall 0:1a0321f1ffbc 121 * Gets the actual translational velocity of the robot.
Brignall 0:1a0321f1ffbc 122 * @return the actual translational velocity, given in [m/s].
Brignall 0:1a0321f1ffbc 123 */
Brignall 0:1a0321f1ffbc 124 float Controller::getActualTranslationalVelocity() {
Brignall 0:1a0321f1ffbc 125
Brignall 0:1a0321f1ffbc 126 return actualTranslationalVelocity;
Brignall 0:1a0321f1ffbc 127 }
Brignall 0:1a0321f1ffbc 128
Brignall 0:1a0321f1ffbc 129 /**
Brignall 0:1a0321f1ffbc 130 * Gets the actual rotational velocity of the robot.
Brignall 0:1a0321f1ffbc 131 * @return the actual rotational velocity, given in [rad/s].
Brignall 0:1a0321f1ffbc 132 */
Brignall 0:1a0321f1ffbc 133 float Controller::getActualRotationalVelocity() {
Brignall 0:1a0321f1ffbc 134
Brignall 0:1a0321f1ffbc 135 return actualRotationalVelocity;
Brignall 0:1a0321f1ffbc 136 }
Brignall 0:1a0321f1ffbc 137
Brignall 0:1a0321f1ffbc 138 /**
Brignall 0:1a0321f1ffbc 139 * Sets the actual x coordinate of the robots position.
Brignall 0:1a0321f1ffbc 140 * @param x the x coordinate of the position, given in [m].
Brignall 0:1a0321f1ffbc 141 */
Brignall 0:1a0321f1ffbc 142 void Controller::setX(float x) {
Brignall 0:1a0321f1ffbc 143
Brignall 0:1a0321f1ffbc 144 this->x = x;
Brignall 0:1a0321f1ffbc 145 }
Brignall 0:1a0321f1ffbc 146
Brignall 0:1a0321f1ffbc 147 /**
Brignall 0:1a0321f1ffbc 148 * Gets the actual x coordinate of the robots position.
Brignall 0:1a0321f1ffbc 149 * @return the x coordinate of the position, given in [m].
Brignall 0:1a0321f1ffbc 150 */
Brignall 0:1a0321f1ffbc 151 float Controller::getX() {
Brignall 0:1a0321f1ffbc 152
Brignall 0:1a0321f1ffbc 153 return x;
Brignall 0:1a0321f1ffbc 154 }
Brignall 0:1a0321f1ffbc 155
Brignall 0:1a0321f1ffbc 156 /**
Brignall 0:1a0321f1ffbc 157 * Sets the actual y coordinate of the robots position.
Brignall 0:1a0321f1ffbc 158 * @param y the y coordinate of the position, given in [m].
Brignall 0:1a0321f1ffbc 159 */
Brignall 0:1a0321f1ffbc 160 void Controller::setY(float y) {
Brignall 0:1a0321f1ffbc 161
Brignall 0:1a0321f1ffbc 162 this->y = y;
Brignall 0:1a0321f1ffbc 163 }
Brignall 0:1a0321f1ffbc 164
Brignall 0:1a0321f1ffbc 165 /**
Brignall 0:1a0321f1ffbc 166 * Gets the actual y coordinate of the robots position.
Brignall 0:1a0321f1ffbc 167 * @return the y coordinate of the position, given in [m].
Brignall 0:1a0321f1ffbc 168 */
Brignall 0:1a0321f1ffbc 169 float Controller::getY() {
Brignall 0:1a0321f1ffbc 170
Brignall 0:1a0321f1ffbc 171 return y;
Brignall 0:1a0321f1ffbc 172 }
Brignall 0:1a0321f1ffbc 173
Brignall 0:1a0321f1ffbc 174 /**
Brignall 0:1a0321f1ffbc 175 * Sets the actual orientation of the robot.
Brignall 0:1a0321f1ffbc 176 * @param alpha the orientation, given in [rad].
Brignall 0:1a0321f1ffbc 177 */
Brignall 0:1a0321f1ffbc 178 void Controller::setAlpha(float alpha) {
Brignall 0:1a0321f1ffbc 179
Brignall 0:1a0321f1ffbc 180 this->alpha = alpha;
Brignall 0:1a0321f1ffbc 181 }
Brignall 0:1a0321f1ffbc 182
Brignall 0:1a0321f1ffbc 183 /**
Brignall 0:1a0321f1ffbc 184 * Gets the actual orientation of the robot.
Brignall 0:1a0321f1ffbc 185 * @return the orientation, given in [rad].
Brignall 0:1a0321f1ffbc 186 */
Brignall 0:1a0321f1ffbc 187 float Controller::getAlpha() {
Brignall 0:1a0321f1ffbc 188
Brignall 0:1a0321f1ffbc 189 return alpha;
Brignall 0:1a0321f1ffbc 190 }
Brignall 0:1a0321f1ffbc 191
Brignall 0:1a0321f1ffbc 192 /**
Brignall 0:1a0321f1ffbc 193 * Correct the pose with given actual and measured coordinates of a beacon.
Brignall 0:1a0321f1ffbc 194 * @param xActual the actual x coordinate of the beacon, given in [m].
Brignall 0:1a0321f1ffbc 195 * @param yActual the actual y coordinate of the beacon, given in [m].
Brignall 0:1a0321f1ffbc 196 * @param xMeasured the x coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
Brignall 0:1a0321f1ffbc 197 * @param yMeasured the y coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
Brignall 0:1a0321f1ffbc 198 */
Brignall 0:1a0321f1ffbc 199 void Controller::correctPoseWithBeacon(float xActual, float yActual, float xMeasured, float yMeasured) {
Brignall 0:1a0321f1ffbc 200
Brignall 0:1a0321f1ffbc 201 // create copies of current state and covariance matrix for Kalman filter P
Brignall 0:1a0321f1ffbc 202
Brignall 0:1a0321f1ffbc 203 float x = this->x;
Brignall 0:1a0321f1ffbc 204 float y = this->y;
Brignall 0:1a0321f1ffbc 205 float alpha = this->alpha;
Brignall 0:1a0321f1ffbc 206
Brignall 0:1a0321f1ffbc 207 float p[3][3];
Brignall 0:1a0321f1ffbc 208
Brignall 0:1a0321f1ffbc 209 for (int i = 0; i < 3; i++) {
Brignall 0:1a0321f1ffbc 210 for (int j = 0; j < 3; j++) {
Brignall 0:1a0321f1ffbc 211 p[i][j] = this->p[i][j];
Brignall 0:1a0321f1ffbc 212 }
Brignall 0:1a0321f1ffbc 213 }
Brignall 0:1a0321f1ffbc 214
Brignall 0:1a0321f1ffbc 215 // calculate covariance matrix of innovation S
Brignall 0:1a0321f1ffbc 216
Brignall 0:1a0321f1ffbc 217 float s[2][2];
Brignall 0:1a0321f1ffbc 218 float r = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
Brignall 0:1a0321f1ffbc 219
Brignall 0:1a0321f1ffbc 220 s[0][0] = 1.0f/r/r*(p[1][0]*xActual*yActual+p[1][1]*yActual*yActual+r*r*SIGMA_DISTANCE*SIGMA_DISTANCE+p[0][0]*(xActual-x)*(xActual-x)-p[1][0]*yActual*x+p[0][1]*(xActual-x)*(yActual-y)-p[1][0]*xActual*y-2.0f*p[1][1]*yActual*y+p[1][0]*x*y+p[1][1]*y*y);
Brignall 0:1a0321f1ffbc 221 s[0][1] = -(1.0f/r/r/r*(-p[1][1]*xActual*yActual+p[1][0]*yActual*yActual-p[0][2]*xActual*r*r-p[1][2]*yActual*r*r-p[0][1]*(xActual-x)*(xActual-x)+p[1][1]*yActual*x+p[0][2]*r*r*x+p[0][0]*(xActual-x)*(yActual-y)+p[1][1]*xActual*y-2.0f*p[1][0]*yActual*y+p[1][2]*r*r*y-p[1][1]*x*y+p[1][0]*y*y));
Brignall 0:1a0321f1ffbc 222 s[1][0] = ((xActual-x)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))+(yActual-y)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y)))/r/r/r;
Brignall 0:1a0321f1ffbc 223 s[1][1] = p[2][2]+SIGMA_GAMMA*SIGMA_GAMMA+p[1][2]*(xActual-x)/r/r+p[0][2]*(-yActual+y)/r/r-(yActual-y)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))/r/r/r/r+(xActual-x)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y))/r/r/r/r;
Brignall 0:1a0321f1ffbc 224
Brignall 0:1a0321f1ffbc 225 // calculate Kalman matrix K
Brignall 0:1a0321f1ffbc 226
Brignall 0:1a0321f1ffbc 227 float k[3][2];
Brignall 0:1a0321f1ffbc 228
Brignall 0:1a0321f1ffbc 229 k[0][0] = -((s[1][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 230 k[0][1] = (s[0][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 231 k[1][0] = -((s[1][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 232 k[1][1] = (s[0][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 233 k[2][0] = -((s[1][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 234 k[2][1] = (s[0][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
Brignall 0:1a0321f1ffbc 235
Brignall 0:1a0321f1ffbc 236 // calculate pose correction
Brignall 0:1a0321f1ffbc 237
Brignall 0:1a0321f1ffbc 238 float distanceMeasured = sqrt((xMeasured-x)*(xMeasured-x)+(yMeasured-y)*(yMeasured-y));
Brignall 0:1a0321f1ffbc 239 float gammaMeasured = atan2(yMeasured-y, xMeasured-x)-alpha;
Brignall 0:1a0321f1ffbc 240
Brignall 0:1a0321f1ffbc 241 if (gammaMeasured > PI) gammaMeasured -= 2.0f*PI;
Brignall 0:1a0321f1ffbc 242 else if (gammaMeasured < -PI) gammaMeasured += 2.0f*PI;
Brignall 0:1a0321f1ffbc 243
Brignall 0:1a0321f1ffbc 244 float distanceEstimated = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
Brignall 0:1a0321f1ffbc 245 float gammaEstimated = atan2(yActual-y, xActual-x)-alpha;
Brignall 0:1a0321f1ffbc 246
Brignall 0:1a0321f1ffbc 247 if (gammaEstimated > PI) gammaEstimated -= 2.0f*PI;
Brignall 0:1a0321f1ffbc 248 else if (gammaEstimated < -PI) gammaEstimated += 2.0f*PI;
Brignall 0:1a0321f1ffbc 249
Brignall 0:1a0321f1ffbc 250 x += k[0][0]*(distanceMeasured-distanceEstimated)+k[0][1]*(gammaMeasured-gammaEstimated);
Brignall 0:1a0321f1ffbc 251 y += k[1][0]*(distanceMeasured-distanceEstimated)+k[1][1]*(gammaMeasured-gammaEstimated);
Brignall 0:1a0321f1ffbc 252 alpha += k[2][0]*(distanceMeasured-distanceEstimated)+k[2][1]*(gammaMeasured-gammaEstimated);
Brignall 0:1a0321f1ffbc 253
Brignall 0:1a0321f1ffbc 254 this->x = x;
Brignall 0:1a0321f1ffbc 255 this->y = y;
Brignall 0:1a0321f1ffbc 256 this->alpha = alpha;
Brignall 0:1a0321f1ffbc 257
Brignall 0:1a0321f1ffbc 258 // calculate correction of covariance matrix for Kalman filter P
Brignall 0:1a0321f1ffbc 259
Brignall 0:1a0321f1ffbc 260 p[0][0] = k[0][1]*p[2][0]+p[0][0]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][0]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 261 p[0][1] = k[0][1]*p[2][1]+p[0][1]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][1]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 262 p[0][2] = k[0][1]*p[2][2]+p[0][2]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][2]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 263
Brignall 0:1a0321f1ffbc 264 p[1][0] = k[1][1]*p[2][0]+p[0][0]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][0]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 265 p[1][1] = k[1][1]*p[2][1]+p[0][1]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][1]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 266 p[1][2] = k[1][1]*p[2][2]+p[0][2]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][2]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 267
Brignall 0:1a0321f1ffbc 268 p[2][0] = (1+k[2][1])*p[2][0]+p[0][0]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][0]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 269 p[2][1] = (1+k[2][1])*p[2][1]+p[0][1]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][1]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 270 p[2][2] = (1+k[2][1])*p[2][2]+p[0][2]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][2]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
Brignall 0:1a0321f1ffbc 271
Brignall 0:1a0321f1ffbc 272 for (int i = 0; i < 3; i++) {
Brignall 0:1a0321f1ffbc 273 for (int j = 0; j < 3; j++) {
Brignall 0:1a0321f1ffbc 274 this->p[i][j] = p[i][j];
Brignall 0:1a0321f1ffbc 275 }
Brignall 0:1a0321f1ffbc 276 }
Brignall 0:1a0321f1ffbc 277 }
Brignall 0:1a0321f1ffbc 278
Brignall 0:1a0321f1ffbc 279 /**
Brignall 0:1a0321f1ffbc 280 * This method is called periodically by the ticker object and contains the
Brignall 0:1a0321f1ffbc 281 * algorithm of the speed controller.
Brignall 0:1a0321f1ffbc 282 */
Brignall 0:1a0321f1ffbc 283 void Controller::run() {
Brignall 0:1a0321f1ffbc 284
Brignall 0:1a0321f1ffbc 285 // calculate the planned velocities using the motion planner
Brignall 0:1a0321f1ffbc 286
Brignall 0:1a0321f1ffbc 287 translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
Brignall 0:1a0321f1ffbc 288 rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
Brignall 0:1a0321f1ffbc 289
Brignall 0:1a0321f1ffbc 290 // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
Brignall 0:1a0321f1ffbc 291
Brignall 0:1a0321f1ffbc 292 desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
Brignall 0:1a0321f1ffbc 293 desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
Brignall 0:1a0321f1ffbc 294
Brignall 0:1a0321f1ffbc 295 // calculate actual speed of motors in [rpm]
Brignall 0:1a0321f1ffbc 296
Brignall 0:1a0321f1ffbc 297 short valueCounterLeft = counterLeft.read();
Brignall 0:1a0321f1ffbc 298 short valueCounterRight = counterRight.read();
Brignall 0:1a0321f1ffbc 299
Brignall 0:1a0321f1ffbc 300 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
Brignall 0:1a0321f1ffbc 301 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
Brignall 0:1a0321f1ffbc 302
Brignall 0:1a0321f1ffbc 303 previousValueCounterLeft = valueCounterLeft;
Brignall 0:1a0321f1ffbc 304 previousValueCounterRight = valueCounterRight;
Brignall 0:1a0321f1ffbc 305
Brignall 0:1a0321f1ffbc 306 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
Brignall 0:1a0321f1ffbc 307 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
Brignall 0:1a0321f1ffbc 308
Brignall 0:1a0321f1ffbc 309 // calculate motor phase voltages
Brignall 0:1a0321f1ffbc 310
Brignall 0:1a0321f1ffbc 311 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
Brignall 0:1a0321f1ffbc 312 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
Brignall 0:1a0321f1ffbc 313
Brignall 0:1a0321f1ffbc 314 // calculate, limit and set duty cycles
Brignall 0:1a0321f1ffbc 315
Brignall 0:1a0321f1ffbc 316 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
Brignall 0:1a0321f1ffbc 317 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
Brignall 0:1a0321f1ffbc 318 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
Brignall 0:1a0321f1ffbc 319 pwmLeft.write(dutyCycleLeft);
Brignall 0:1a0321f1ffbc 320
Brignall 0:1a0321f1ffbc 321 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
Brignall 0:1a0321f1ffbc 322 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
Brignall 0:1a0321f1ffbc 323 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
Brignall 0:1a0321f1ffbc 324 pwmRight.write(dutyCycleRight);
Brignall 0:1a0321f1ffbc 325
Brignall 0:1a0321f1ffbc 326 // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
Brignall 0:1a0321f1ffbc 327
Brignall 0:1a0321f1ffbc 328 actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
Brignall 0:1a0321f1ffbc 329 actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
Brignall 0:1a0321f1ffbc 330
Brignall 0:1a0321f1ffbc 331 // calculate the actual robot pose
Brignall 0:1a0321f1ffbc 332
Brignall 0:1a0321f1ffbc 333 float deltaTranslation = actualTranslationalVelocity*PERIOD;
Brignall 0:1a0321f1ffbc 334 float deltaOrientation = actualRotationalVelocity*PERIOD;
Brignall 0:1a0321f1ffbc 335
Brignall 0:1a0321f1ffbc 336 float sinAlpha = sin(alpha+deltaOrientation);
Brignall 0:1a0321f1ffbc 337 float cosAlpha = cos(alpha+deltaOrientation);
Brignall 0:1a0321f1ffbc 338
Brignall 0:1a0321f1ffbc 339 x += cosAlpha*deltaTranslation;
Brignall 0:1a0321f1ffbc 340 y += sinAlpha*deltaTranslation;
Brignall 0:1a0321f1ffbc 341 float alpha = this->alpha+deltaOrientation;
Brignall 0:1a0321f1ffbc 342
Brignall 0:1a0321f1ffbc 343 while (alpha > PI) alpha -= 2.0f*PI;
Brignall 0:1a0321f1ffbc 344 while (alpha < -PI) alpha += 2.0f*PI;
Brignall 0:1a0321f1ffbc 345
Brignall 0:1a0321f1ffbc 346 this->alpha = alpha;
Brignall 0:1a0321f1ffbc 347
Brignall 0:1a0321f1ffbc 348 // calculate covariance matrix for Kalman filter P
Brignall 0:1a0321f1ffbc 349
Brignall 0:1a0321f1ffbc 350 p[0][0] = p[0][0]+SIGMA_TRANSLATION*SIGMA_TRANSLATION*cosAlpha*cosAlpha+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha*sinAlpha-deltaTranslation*(p[0][2]+p[2][0])*sinAlpha;
Brignall 0:1a0321f1ffbc 351 p[0][1] = p[0][1]-deltaTranslation*p[2][1]*sinAlpha+cosAlpha*(deltaTranslation*p[0][2]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
Brignall 0:1a0321f1ffbc 352 p[0][2] = p[0][2]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
Brignall 0:1a0321f1ffbc 353
Brignall 0:1a0321f1ffbc 354 p[1][0] = p[1][0]-deltaTranslation*p[1][2]*sinAlpha+cosAlpha*(deltaTranslation*p[2][0]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
Brignall 0:1a0321f1ffbc 355 p[1][1] = p[1][1]+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha*cosAlpha+deltaTranslation*(p[1][2]+p[2][1])*cosAlpha+SIGMA_TRANSLATION*SIGMA_TRANSLATION*sinAlpha*sinAlpha;
Brignall 0:1a0321f1ffbc 356 p[1][2] = p[1][2]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
Brignall 0:1a0321f1ffbc 357
Brignall 0:1a0321f1ffbc 358 p[2][0] = p[2][0]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
Brignall 0:1a0321f1ffbc 359 p[2][1] = p[2][1]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
Brignall 0:1a0321f1ffbc 360 p[2][2] = p[2][2]+SIGMA_ORIENTATION*SIGMA_ORIENTATION;
Brignall 0:1a0321f1ffbc 361 }
Brignall 0:1a0321f1ffbc 362