BA / Mbed OS BaBoRo1

Erste version der Software für der Prototyp

Controller.cpp@1:d5c5bb30ac90, 2018-03-29 (annotated)

Committer:
borlanic
Date:
Thu Mar 29 09:25:16 2018 +0000
Revision:
1:d5c5bb30ac90
Parent:
0:380207fcb5c1
Child:
2:3f836b662104
Erste veriosn software

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:380207fcb5c1 1 /*
borlanic 0:380207fcb5c1 2 * Controller.cpp
borlanic 0:380207fcb5c1 3 * Copyright (c) 2018, ZHAW
borlanic 0:380207fcb5c1 4 * All rights reserved.
borlanic 0:380207fcb5c1 5 *
borlanic 0:380207fcb5c1 6 * Created on: 27.03.2018
borlanic 0:380207fcb5c1 7 * Author: BaBoRo Development Team
borlanic 0:380207fcb5c1 8 */
borlanic 0:380207fcb5c1 9
borlanic 0:380207fcb5c1 10 #include <cmath>
borlanic 0:380207fcb5c1 11 #include "Controller.h"
borlanic 0:380207fcb5c1 12
borlanic 0:380207fcb5c1 13 using namespace std;
borlanic 0:380207fcb5c1 14
borlanic 0:380207fcb5c1 15 const float Controller::PERIOD = 0.001f; // the period of the timer interrupt, given in [s]
borlanic 0:380207fcb5c1 16 const float Controller::ALPHA = 0.785398163397448f; // alpha = 45° in [rad]
borlanic 0:380207fcb5c1 17 const float Controller::RB = 0.095f; // Ball Radius in [m]
borlanic 0:380207fcb5c1 18 const float Controller::RW = 0.024f; // Wheel Radius in [m]
borlanic 0:380207fcb5c1 19 const float Controller::PI = 3.141592653589793f; // PI
borlanic 0:380207fcb5c1 20 const float Controller::SQRT_3 = 1.732050807568877f; // Square root of 3
borlanic 0:380207fcb5c1 21 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
borlanic 0:380207fcb5c1 22 const float Controller::COUNTS_PER_TURN = 1024.0f;
borlanic 0:380207fcb5c1 23
borlanic 0:380207fcb5c1 24 /**
borlanic 0:380207fcb5c1 25 * Create and initialize a robot controller object.
borlanic 0:380207fcb5c1 26 * @param pwm0 a pwm output object to set the duty cycle for the first motor.
borlanic 0:380207fcb5c1 27 * @param pwm1 a pwm output object to set the duty cycle for the second motor.
borlanic 0:380207fcb5c1 28 * @param pwm2 a pwm output object to set the duty cycle for the third motor.
borlanic 0:380207fcb5c1 29 */
borlanic 1:d5c5bb30ac90 30 Controller::Controller(PwmOut& pwm0, PwmOut& pwm1, PwmOut& pwm2, EncoderCounter& counter1, EncoderCounter& counter2, EncoderCounter& counter3, IMU& imu) : pwm0(pwm0), pwm1(pwm1), pwm2(pwm2), counter1(counter1), counter2(counter2), counter3(counter3), imu(imu), thread(osPriorityHigh, STACK_SIZE)
borlanic 1:d5c5bb30ac90 31 {
borlanic 1:d5c5bb30ac90 32
borlanic 0:380207fcb5c1 33 // initialize local values
borlanic 1:d5c5bb30ac90 34
borlanic 0:380207fcb5c1 35 pwm0.period(0.002f);
borlanic 0:380207fcb5c1 36 pwm0.write(0.5f);
borlanic 1:d5c5bb30ac90 37
borlanic 0:380207fcb5c1 38 pwm1.period(0.002f);
borlanic 0:380207fcb5c1 39 pwm1.write(0.5f);
borlanic 1:d5c5bb30ac90 40
borlanic 0:380207fcb5c1 41 pwm2.period(0.002f);
borlanic 0:380207fcb5c1 42 pwm2.write(0.5f);
borlanic 1:d5c5bb30ac90 43
borlanic 0:380207fcb5c1 44 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 45 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 46 gammaZ = imu.getGammaZ();
borlanic 1:d5c5bb30ac90 47
borlanic 0:380207fcb5c1 48 phiX = 0.0f;
borlanic 0:380207fcb5c1 49 phiY = 0.0f;
borlanic 1:d5c5bb30ac90 50
borlanic 0:380207fcb5c1 51 x = 0.0f;
borlanic 0:380207fcb5c1 52 y = 0.0f;
borlanic 1:d5c5bb30ac90 53
borlanic 0:380207fcb5c1 54 float previousValueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 55 float previousValueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 56 float previousValueCounter3 = counter3.read();
borlanic 1:d5c5bb30ac90 57
borlanic 0:380207fcb5c1 58 speedFilter1.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 59 speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 60
borlanic 0:380207fcb5c1 61 speedFilter2.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 62 speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 63
borlanic 0:380207fcb5c1 64 speedFilter3.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 65 speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 1:d5c5bb30ac90 66
borlanic 0:380207fcb5c1 67 previousValueCounter1 = 0.0f;
borlanic 0:380207fcb5c1 68 previousValueCounter2 = 0.0f;
borlanic 0:380207fcb5c1 69 previousValueCounter3 = 0.0f;
borlanic 1:d5c5bb30ac90 70
borlanic 0:380207fcb5c1 71 // start thread and timer interrupt
borlanic 1:d5c5bb30ac90 72
borlanic 0:380207fcb5c1 73 //thread.start(callback(this, &Controller::run));
borlanic 0:380207fcb5c1 74 //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
borlanic 0:380207fcb5c1 75 }
borlanic 0:380207fcb5c1 76
borlanic 0:380207fcb5c1 77 /**
borlanic 0:380207fcb5c1 78 * Delete the robot controller object and release all allocated resources.
borlanic 0:380207fcb5c1 79 */
borlanic 1:d5c5bb30ac90 80 Controller::~Controller()
borlanic 1:d5c5bb30ac90 81 {
borlanic 1:d5c5bb30ac90 82
borlanic 0:380207fcb5c1 83 //ticker.detach();
borlanic 0:380207fcb5c1 84 }
borlanic 0:380207fcb5c1 85
borlanic 0:380207fcb5c1 86 // set --------------------------------
borlanic 1:d5c5bb30ac90 87 void Controller::setGammaX(float gammaX)
borlanic 1:d5c5bb30ac90 88 {
borlanic 1:d5c5bb30ac90 89
borlanic 0:380207fcb5c1 90 this->gammaX = gammaX;
borlanic 0:380207fcb5c1 91 }
borlanic 0:380207fcb5c1 92
borlanic 0:380207fcb5c1 93
borlanic 1:d5c5bb30ac90 94 void Controller::setGammaY(float gammaY)
borlanic 1:d5c5bb30ac90 95 {
borlanic 1:d5c5bb30ac90 96
borlanic 0:380207fcb5c1 97 this->gammaY = gammaY;
borlanic 0:380207fcb5c1 98 }
borlanic 0:380207fcb5c1 99
borlanic 0:380207fcb5c1 100
borlanic 1:d5c5bb30ac90 101 void Controller::setGammaZ(float gammaZ)
borlanic 1:d5c5bb30ac90 102 {
borlanic 1:d5c5bb30ac90 103
borlanic 0:380207fcb5c1 104 this->gammaZ = gammaZ;
borlanic 0:380207fcb5c1 105 }
borlanic 0:380207fcb5c1 106
borlanic 0:380207fcb5c1 107
borlanic 1:d5c5bb30ac90 108 void Controller::setPhiX(float phiX)
borlanic 1:d5c5bb30ac90 109 {
borlanic 1:d5c5bb30ac90 110
borlanic 0:380207fcb5c1 111 this->phiX = phiX;
borlanic 0:380207fcb5c1 112 }
borlanic 0:380207fcb5c1 113
borlanic 1:d5c5bb30ac90 114 void Controller::setPhiY(float phiY)
borlanic 1:d5c5bb30ac90 115 {
borlanic 1:d5c5bb30ac90 116
borlanic 0:380207fcb5c1 117 this->phiY = phiY;
borlanic 0:380207fcb5c1 118 }
borlanic 0:380207fcb5c1 119
borlanic 1:d5c5bb30ac90 120 void Controller::setX(float x)
borlanic 1:d5c5bb30ac90 121 {
borlanic 1:d5c5bb30ac90 122
borlanic 0:380207fcb5c1 123 this->x = x;
borlanic 0:380207fcb5c1 124 }
borlanic 0:380207fcb5c1 125
borlanic 0:380207fcb5c1 126 /**
borlanic 0:380207fcb5c1 127 * Sets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 128 * @param y the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 129 */
borlanic 1:d5c5bb30ac90 130 void Controller::setY(float y)
borlanic 1:d5c5bb30ac90 131 {
borlanic 1:d5c5bb30ac90 132
borlanic 0:380207fcb5c1 133 this->y = y;
borlanic 0:380207fcb5c1 134 }
borlanic 0:380207fcb5c1 135
borlanic 0:380207fcb5c1 136 // get --------------------------------
borlanic 0:380207fcb5c1 137
borlanic 1:d5c5bb30ac90 138 float Controller::getPhiX()
borlanic 1:d5c5bb30ac90 139 {
borlanic 0:380207fcb5c1 140
borlanic 0:380207fcb5c1 141 return phiX;
borlanic 0:380207fcb5c1 142 }
borlanic 0:380207fcb5c1 143
borlanic 1:d5c5bb30ac90 144 float Controller::getPhiY()
borlanic 1:d5c5bb30ac90 145 {
borlanic 1:d5c5bb30ac90 146
borlanic 0:380207fcb5c1 147 return phiY;
borlanic 0:380207fcb5c1 148 }
borlanic 0:380207fcb5c1 149
borlanic 0:380207fcb5c1 150 /**
borlanic 0:380207fcb5c1 151 * Gets the actual x coordinate of the robots position.
borlanic 0:380207fcb5c1 152 * @return the x coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 153 */
borlanic 1:d5c5bb30ac90 154 float Controller::getX()
borlanic 1:d5c5bb30ac90 155 {
borlanic 1:d5c5bb30ac90 156
borlanic 0:380207fcb5c1 157 return x;
borlanic 0:380207fcb5c1 158 }
borlanic 0:380207fcb5c1 159
borlanic 0:380207fcb5c1 160 /**
borlanic 0:380207fcb5c1 161 * Gets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 162 * @return the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 163 */
borlanic 1:d5c5bb30ac90 164 float Controller::getY()
borlanic 1:d5c5bb30ac90 165 {
borlanic 1:d5c5bb30ac90 166
borlanic 0:380207fcb5c1 167 return y;
borlanic 0:380207fcb5c1 168 }
borlanic 0:380207fcb5c1 169
borlanic 0:380207fcb5c1 170 /**
borlanic 0:380207fcb5c1 171 * This method is called by the ticker timer interrupt service routine.
borlanic 0:380207fcb5c1 172 * It sends a signal to the thread to make it run again.
borlanic 0:380207fcb5c1 173 */
borlanic 0:380207fcb5c1 174 //void Controller::sendSignal() {
borlanic 1:d5c5bb30ac90 175
borlanic 0:380207fcb5c1 176 // thread.signal_set(signal);
borlanic 0:380207fcb5c1 177 //}
borlanic 0:380207fcb5c1 178
borlanic 0:380207fcb5c1 179 /**
borlanic 0:380207fcb5c1 180 * This <code>run()</code> method contains an infinite loop with the run logic.
borlanic 0:380207fcb5c1 181 */
borlanic 1:d5c5bb30ac90 182 void Controller::run()
borlanic 1:d5c5bb30ac90 183 {
borlanic 1:d5c5bb30ac90 184
borlanic 0:380207fcb5c1 185 float integratedGammaX = 0.0f;
borlanic 0:380207fcb5c1 186 float integratedGammaY = 0.0f;
borlanic 0:380207fcb5c1 187 float integratedGammaZ = 0.0f;
borlanic 0:380207fcb5c1 188 float integratedPhiX = 0.0f;
borlanic 0:380207fcb5c1 189 float integratedPhiY = 0.0f;
borlanic 1:d5c5bb30ac90 190
borlanic 0:380207fcb5c1 191 float previousGammaX = 0.0;
borlanic 0:380207fcb5c1 192 float previousGammaY = 0.0;
borlanic 0:380207fcb5c1 193 float previousGammaZ = 0.0;
borlanic 0:380207fcb5c1 194 float previousPhiX = 0.0;
borlanic 0:380207fcb5c1 195 float previousPhiY = 0.0;
borlanic 1:d5c5bb30ac90 196
borlanic 1:d5c5bb30ac90 197 // K matrix
borlanic 1:d5c5bb30ac90 198 float K[3][15] = {
borlanic 1:d5c5bb30ac90 199 {0.1010, 0.0000, 2.4661, 0.0000, 0.0639, 0.0621, 0.0000, 0.3066, 0.0000, 0.0237, 0.0816, 0.0000, 0.0000, 0.0000, 0.0577},
borlanic 1:d5c5bb30ac90 200 {-0.0505, 0.0875, -1.2330, 2.1357, 0.0639, -0.0311, 0.0538, -0.1533, 0.2655, 0.0237, -0.0408, 0.0707, 0.0000, 0.0000, 0.0577},
borlanic 1:d5c5bb30ac90 201 {-0.0505, -0.0875, -1.2330, -2.1357, 0.0639, -0.0311, -0.0538, -0.1533, -0.2655, 0.0237, -0.0408, -0.0707, 0.0000, 0.0000, 0.0577}
borlanic 1:d5c5bb30ac90 202 };
borlanic 1:d5c5bb30ac90 203
borlanic 1:d5c5bb30ac90 204
borlanic 0:380207fcb5c1 205 while (true) {
borlanic 1:d5c5bb30ac90 206
borlanic 0:380207fcb5c1 207 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 208 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 209 gammaZ = imu.getGammaZ();
borlanic 0:380207fcb5c1 210 d_gammaX = imu.getDGammaX();
borlanic 0:380207fcb5c1 211 d_gammaY = imu.getDGammaY();
borlanic 0:380207fcb5c1 212 d_gammaZ = imu.getDGammaZ();
borlanic 1:d5c5bb30ac90 213
borlanic 0:380207fcb5c1 214 // wait for the periodic signal
borlanic 1:d5c5bb30ac90 215
borlanic 0:380207fcb5c1 216 // thread.signal_wait(signal);
borlanic 1:d5c5bb30ac90 217
borlanic 0:380207fcb5c1 218 // Read encoder data
borlanic 0:380207fcb5c1 219 float w1 = counter1.read()/1024.0f *1/(2*PI); // Angle wheel 1
borlanic 0:380207fcb5c1 220 float w2 = counter2.read()/1024.0f *1/(2*PI); // Angle wheel 2
borlanic 0:380207fcb5c1 221 float w3 = counter3.read()/1024.0f *1/(2*PI); // Angle wheel 3
borlanic 1:d5c5bb30ac90 222
borlanic 0:380207fcb5c1 223 // Calculate Ball angle in [rad] using the kinematic model
borlanic 0:380207fcb5c1 224 this->phiX = ((w1 - w2 - w3) - sin(ALPHA)*RB/RW*gammaZ)/(2*cos(ALPHA))+gammaX;
borlanic 0:380207fcb5c1 225 this->phiY = (w2 - w3)/(SQRT_3*cos(ALPHA))+gammaY;
borlanic 1:d5c5bb30ac90 226
borlanic 0:380207fcb5c1 227 // Integration states
borlanic 0:380207fcb5c1 228 integratedGammaX = integratedGammaX + (gammaX-previousGammaX)*PERIOD;
borlanic 0:380207fcb5c1 229 integratedGammaY = integratedGammaY + (gammaY-previousGammaY)*PERIOD;
borlanic 0:380207fcb5c1 230 integratedGammaZ = integratedGammaZ + (gammaZ-previousGammaZ)*PERIOD;
borlanic 0:380207fcb5c1 231 integratedPhiX = integratedPhiX + (phiX-previousPhiX)*PERIOD;
borlanic 0:380207fcb5c1 232 integratedPhiY = integratedPhiY + (phiY-previousPhiY)*PERIOD;
borlanic 1:d5c5bb30ac90 233
borlanic 0:380207fcb5c1 234 // Calculate Ball Velocities
borlanic 0:380207fcb5c1 235 short valueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 236 short valueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 237 short valueCounter3 = counter3.read();
borlanic 1:d5c5bb30ac90 238
borlanic 0:380207fcb5c1 239 short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
borlanic 0:380207fcb5c1 240 short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
borlanic 0:380207fcb5c1 241 short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
borlanic 1:d5c5bb30ac90 242
borlanic 0:380207fcb5c1 243 previousValueCounter1 = valueCounter1;
borlanic 0:380207fcb5c1 244 previousValueCounter2 = valueCounter2;
borlanic 0:380207fcb5c1 245 previousValueCounter3 = valueCounter3;
borlanic 1:d5c5bb30ac90 246
borlanic 0:380207fcb5c1 247 actualSpeed1 = speedFilter1.filter((float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 248 actualSpeed2 = speedFilter2.filter((float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 249 actualSpeed3 = speedFilter3.filter((float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 1:d5c5bb30ac90 250
borlanic 0:380207fcb5c1 251 float d_phiX = ((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX;
borlanic 0:380207fcb5c1 252 float d_phiY = (actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY;
borlanic 1:d5c5bb30ac90 253
borlanic 0:380207fcb5c1 254 // Calculate Torque motors
borlanic 1:d5c5bb30ac90 255
borlanic 1:d5c5bb30ac90 256 //[M1,M2,M3]=K*x
borlanic 1:d5c5bb30ac90 257
borlanic 1:d5c5bb30ac90 258 float M1 = 0.0f;
borlanic 1:d5c5bb30ac90 259 float M2 = 0.0f;
borlanic 1:d5c5bb30ac90 260 float M3 = 0.0f;
borlanic 1:d5c5bb30ac90 261
borlanic 1:d5c5bb30ac90 262 float x[15][1] = {
borlanic 1:d5c5bb30ac90 263 {gammaX},{gammaY},{gammaZ},{phiX},{phiY},{d_gammaX},{d_gammaY},{d_gammaZ},{d_phiX},{d_phiY},{integratedGammaX},{integratedGammaY},{integratedGammaZ},{integratedPhiX},{integratedPhiY}
borlanic 1:d5c5bb30ac90 264 };
borlanic 1:d5c5bb30ac90 265
borlanic 1:d5c5bb30ac90 266 for(int i=0; i<14; i++) {
borlanic 1:d5c5bb30ac90 267 M1 = M1 + K[0][i]*x[i][1];
borlanic 1:d5c5bb30ac90 268 M2 = M2 + K[1][i]*x[i][1];
borlanic 1:d5c5bb30ac90 269 M3 = M3 + K[2][i]*x[i][1];
borlanic 1:d5c5bb30ac90 270 };
borlanic 1:d5c5bb30ac90 271
borlanic 0:380207fcb5c1 272 // Calculate duty cycles from desired Torque
borlanic 0:380207fcb5c1 273
borlanic 1:d5c5bb30ac90 274
borlanic 1:d5c5bb30ac90 275 //pwm1.write(dutyCycle1);
borlanic 1:d5c5bb30ac90 276 //pwm2.write(dutyCycle2);
borlanic 1:d5c5bb30ac90 277 //pwm3.write(dutyCycle3);
borlanic 1:d5c5bb30ac90 278
borlanic 0:380207fcb5c1 279 // actual robot pose
borlanic 1:d5c5bb30ac90 280 this->x = phiY*RB;
borlanic 1:d5c5bb30ac90 281 this->y = phiX*RB;
borlanic 0:380207fcb5c1 282
borlanic 0:380207fcb5c1 283 // set new gamma's, phi's
borlanic 0:380207fcb5c1 284 previousGammaX = gammaX;
borlanic 0:380207fcb5c1 285 previousGammaY = gammaY;
borlanic 0:380207fcb5c1 286 previousGammaZ = gammaZ;
borlanic 0:380207fcb5c1 287 previousPhiX = phiX;
borlanic 0:380207fcb5c1 288 previousPhiY = phiY;
borlanic 0:380207fcb5c1 289 }
borlanic 0:380207fcb5c1 290 };