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