BA / Mbed OS BaBoRo1

Erste version der Software für der Prototyp

Controller.cpp@0:380207fcb5c1, 2018-03-29 (annotated)

Committer:
borlanic
Date:
Thu Mar 29 07:02:09 2018 +0000
Revision:
0:380207fcb5c1
Child:
1:d5c5bb30ac90
Encoder, IMU --> OK; Controller --> in bearbeitung

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 0:380207fcb5c1 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 0:380207fcb5c1 31
borlanic 0:380207fcb5c1 32 // initialize local values
borlanic 0:380207fcb5c1 33
borlanic 0:380207fcb5c1 34 pwm0.period(0.002f);
borlanic 0:380207fcb5c1 35 pwm0.write(0.5f);
borlanic 0:380207fcb5c1 36
borlanic 0:380207fcb5c1 37 pwm1.period(0.002f);
borlanic 0:380207fcb5c1 38 pwm1.write(0.5f);
borlanic 0:380207fcb5c1 39
borlanic 0:380207fcb5c1 40 pwm2.period(0.002f);
borlanic 0:380207fcb5c1 41 pwm2.write(0.5f);
borlanic 0:380207fcb5c1 42
borlanic 0:380207fcb5c1 43 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 44 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 45 gammaZ = imu.getGammaZ();
borlanic 0:380207fcb5c1 46
borlanic 0:380207fcb5c1 47 phiX = 0.0f;
borlanic 0:380207fcb5c1 48 phiY = 0.0f;
borlanic 0:380207fcb5c1 49
borlanic 0:380207fcb5c1 50 x = 0.0f;
borlanic 0:380207fcb5c1 51 y = 0.0f;
borlanic 0:380207fcb5c1 52
borlanic 0:380207fcb5c1 53 float previousValueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 54 float previousValueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 55 float previousValueCounter3 = counter3.read();
borlanic 0:380207fcb5c1 56
borlanic 0:380207fcb5c1 57 speedFilter1.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 58 speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:380207fcb5c1 59
borlanic 0:380207fcb5c1 60 speedFilter2.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 61 speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:380207fcb5c1 62
borlanic 0:380207fcb5c1 63 speedFilter3.setPeriod(PERIOD);
borlanic 0:380207fcb5c1 64 speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:380207fcb5c1 65
borlanic 0:380207fcb5c1 66 previousValueCounter1 = 0.0f;
borlanic 0:380207fcb5c1 67 previousValueCounter2 = 0.0f;
borlanic 0:380207fcb5c1 68 previousValueCounter3 = 0.0f;
borlanic 0:380207fcb5c1 69
borlanic 0:380207fcb5c1 70 // start thread and timer interrupt
borlanic 0:380207fcb5c1 71
borlanic 0:380207fcb5c1 72 //thread.start(callback(this, &Controller::run));
borlanic 0:380207fcb5c1 73 //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
borlanic 0:380207fcb5c1 74 }
borlanic 0:380207fcb5c1 75
borlanic 0:380207fcb5c1 76 /**
borlanic 0:380207fcb5c1 77 * Delete the robot controller object and release all allocated resources.
borlanic 0:380207fcb5c1 78 */
borlanic 0:380207fcb5c1 79 Controller::~Controller() {
borlanic 0:380207fcb5c1 80
borlanic 0:380207fcb5c1 81 //ticker.detach();
borlanic 0:380207fcb5c1 82 }
borlanic 0:380207fcb5c1 83
borlanic 0:380207fcb5c1 84 // set --------------------------------
borlanic 0:380207fcb5c1 85 void Controller::setGammaX(float gammaX) {
borlanic 0:380207fcb5c1 86
borlanic 0:380207fcb5c1 87 this->gammaX = gammaX;
borlanic 0:380207fcb5c1 88 }
borlanic 0:380207fcb5c1 89
borlanic 0:380207fcb5c1 90
borlanic 0:380207fcb5c1 91 void Controller::setGammaY(float gammaY) {
borlanic 0:380207fcb5c1 92
borlanic 0:380207fcb5c1 93 this->gammaY = gammaY;
borlanic 0:380207fcb5c1 94 }
borlanic 0:380207fcb5c1 95
borlanic 0:380207fcb5c1 96
borlanic 0:380207fcb5c1 97 void Controller::setGammaZ(float gammaZ) {
borlanic 0:380207fcb5c1 98
borlanic 0:380207fcb5c1 99 this->gammaZ = gammaZ;
borlanic 0:380207fcb5c1 100 }
borlanic 0:380207fcb5c1 101
borlanic 0:380207fcb5c1 102
borlanic 0:380207fcb5c1 103 void Controller::setPhiX(float phiX) {
borlanic 0:380207fcb5c1 104
borlanic 0:380207fcb5c1 105 this->phiX = phiX;
borlanic 0:380207fcb5c1 106 }
borlanic 0:380207fcb5c1 107
borlanic 0:380207fcb5c1 108 void Controller::setPhiY(float phiY) {
borlanic 0:380207fcb5c1 109
borlanic 0:380207fcb5c1 110 this->phiY = phiY;
borlanic 0:380207fcb5c1 111 }
borlanic 0:380207fcb5c1 112
borlanic 0:380207fcb5c1 113 void Controller::setX(float x) {
borlanic 0:380207fcb5c1 114
borlanic 0:380207fcb5c1 115 this->x = x;
borlanic 0:380207fcb5c1 116 }
borlanic 0:380207fcb5c1 117
borlanic 0:380207fcb5c1 118 /**
borlanic 0:380207fcb5c1 119 * Sets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 120 * @param y the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 121 */
borlanic 0:380207fcb5c1 122 void Controller::setY(float y) {
borlanic 0:380207fcb5c1 123
borlanic 0:380207fcb5c1 124 this->y = y;
borlanic 0:380207fcb5c1 125 }
borlanic 0:380207fcb5c1 126
borlanic 0:380207fcb5c1 127 // get --------------------------------
borlanic 0:380207fcb5c1 128
borlanic 0:380207fcb5c1 129 float Controller::getGammaX() {
borlanic 0:380207fcb5c1 130
borlanic 0:380207fcb5c1 131 return gammaX;
borlanic 0:380207fcb5c1 132 }
borlanic 0:380207fcb5c1 133
borlanic 0:380207fcb5c1 134
borlanic 0:380207fcb5c1 135 float Controller::getGammaY() {
borlanic 0:380207fcb5c1 136
borlanic 0:380207fcb5c1 137 return gammaY;
borlanic 0:380207fcb5c1 138 }
borlanic 0:380207fcb5c1 139
borlanic 0:380207fcb5c1 140
borlanic 0:380207fcb5c1 141 float Controller::getGammaZ() {
borlanic 0:380207fcb5c1 142
borlanic 0:380207fcb5c1 143 return gammaZ;
borlanic 0:380207fcb5c1 144 }
borlanic 0:380207fcb5c1 145
borlanic 0:380207fcb5c1 146
borlanic 0:380207fcb5c1 147 float Controller::getPhiX() {
borlanic 0:380207fcb5c1 148
borlanic 0:380207fcb5c1 149 return phiX;
borlanic 0:380207fcb5c1 150 }
borlanic 0:380207fcb5c1 151
borlanic 0:380207fcb5c1 152 float Controller::getPhiY() {
borlanic 0:380207fcb5c1 153
borlanic 0:380207fcb5c1 154 return phiY;
borlanic 0:380207fcb5c1 155 }
borlanic 0:380207fcb5c1 156
borlanic 0:380207fcb5c1 157 /**
borlanic 0:380207fcb5c1 158 * Gets the actual x coordinate of the robots position.
borlanic 0:380207fcb5c1 159 * @return the x coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 160 */
borlanic 0:380207fcb5c1 161 float Controller::getX() {
borlanic 0:380207fcb5c1 162
borlanic 0:380207fcb5c1 163 return x;
borlanic 0:380207fcb5c1 164 }
borlanic 0:380207fcb5c1 165
borlanic 0:380207fcb5c1 166 /**
borlanic 0:380207fcb5c1 167 * Gets the actual y coordinate of the robots position.
borlanic 0:380207fcb5c1 168 * @return the y coordinate of the position, given in [m].
borlanic 0:380207fcb5c1 169 */
borlanic 0:380207fcb5c1 170 float Controller::getY() {
borlanic 0:380207fcb5c1 171
borlanic 0:380207fcb5c1 172 return y;
borlanic 0:380207fcb5c1 173 }
borlanic 0:380207fcb5c1 174
borlanic 0:380207fcb5c1 175 /**
borlanic 0:380207fcb5c1 176 * This method is called by the ticker timer interrupt service routine.
borlanic 0:380207fcb5c1 177 * It sends a signal to the thread to make it run again.
borlanic 0:380207fcb5c1 178 */
borlanic 0:380207fcb5c1 179 //void Controller::sendSignal() {
borlanic 0:380207fcb5c1 180
borlanic 0:380207fcb5c1 181 // thread.signal_set(signal);
borlanic 0:380207fcb5c1 182 //}
borlanic 0:380207fcb5c1 183
borlanic 0:380207fcb5c1 184 /**
borlanic 0:380207fcb5c1 185 * This <code>run()</code> method contains an infinite loop with the run logic.
borlanic 0:380207fcb5c1 186 */
borlanic 0:380207fcb5c1 187 void Controller::run() {
borlanic 0:380207fcb5c1 188
borlanic 0:380207fcb5c1 189 float integratedGammaX = 0.0f;
borlanic 0:380207fcb5c1 190 float integratedGammaY = 0.0f;
borlanic 0:380207fcb5c1 191 float integratedGammaZ = 0.0f;
borlanic 0:380207fcb5c1 192 float integratedPhiX = 0.0f;
borlanic 0:380207fcb5c1 193 float integratedPhiY = 0.0f;
borlanic 0:380207fcb5c1 194
borlanic 0:380207fcb5c1 195 float previousGammaX = 0.0;
borlanic 0:380207fcb5c1 196 float previousGammaY = 0.0;
borlanic 0:380207fcb5c1 197 float previousGammaZ = 0.0;
borlanic 0:380207fcb5c1 198 float previousPhiX = 0.0;
borlanic 0:380207fcb5c1 199 float previousPhiY = 0.0;
borlanic 0:380207fcb5c1 200
borlanic 0:380207fcb5c1 201 while (true) {
borlanic 0:380207fcb5c1 202
borlanic 0:380207fcb5c1 203 gammaX = imu.getGammaX();
borlanic 0:380207fcb5c1 204 gammaY = imu.getGammaY();
borlanic 0:380207fcb5c1 205 gammaZ = imu.getGammaZ();
borlanic 0:380207fcb5c1 206 d_gammaX = imu.getDGammaX();
borlanic 0:380207fcb5c1 207 d_gammaY = imu.getDGammaY();
borlanic 0:380207fcb5c1 208 d_gammaZ = imu.getDGammaZ();
borlanic 0:380207fcb5c1 209
borlanic 0:380207fcb5c1 210 // wait for the periodic signal
borlanic 0:380207fcb5c1 211
borlanic 0:380207fcb5c1 212 // thread.signal_wait(signal);
borlanic 0:380207fcb5c1 213
borlanic 0:380207fcb5c1 214 // Read encoder data
borlanic 0:380207fcb5c1 215 float w1 = counter1.read()/1024.0f *1/(2*PI); // Angle wheel 1
borlanic 0:380207fcb5c1 216 float w2 = counter2.read()/1024.0f *1/(2*PI); // Angle wheel 2
borlanic 0:380207fcb5c1 217 float w3 = counter3.read()/1024.0f *1/(2*PI); // Angle wheel 3
borlanic 0:380207fcb5c1 218
borlanic 0:380207fcb5c1 219 // Calculate Ball angle in [rad] using the kinematic model
borlanic 0:380207fcb5c1 220 this->phiX = ((w1 - w2 - w3) - sin(ALPHA)*RB/RW*gammaZ)/(2*cos(ALPHA))+gammaX;
borlanic 0:380207fcb5c1 221 this->phiY = (w2 - w3)/(SQRT_3*cos(ALPHA))+gammaY;
borlanic 0:380207fcb5c1 222
borlanic 0:380207fcb5c1 223 // Integration states
borlanic 0:380207fcb5c1 224 integratedGammaX = integratedGammaX + (gammaX-previousGammaX)*PERIOD;
borlanic 0:380207fcb5c1 225 integratedGammaY = integratedGammaY + (gammaY-previousGammaY)*PERIOD;
borlanic 0:380207fcb5c1 226 integratedGammaZ = integratedGammaZ + (gammaZ-previousGammaZ)*PERIOD;
borlanic 0:380207fcb5c1 227 integratedPhiX = integratedPhiX + (phiX-previousPhiX)*PERIOD;
borlanic 0:380207fcb5c1 228 integratedPhiY = integratedPhiY + (phiY-previousPhiY)*PERIOD;
borlanic 0:380207fcb5c1 229
borlanic 0:380207fcb5c1 230 // Calculate Ball Velocities
borlanic 0:380207fcb5c1 231 short valueCounter1 = counter1.read();
borlanic 0:380207fcb5c1 232 short valueCounter2 = counter2.read();
borlanic 0:380207fcb5c1 233 short valueCounter3 = counter3.read();
borlanic 0:380207fcb5c1 234
borlanic 0:380207fcb5c1 235 short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
borlanic 0:380207fcb5c1 236 short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
borlanic 0:380207fcb5c1 237 short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
borlanic 0:380207fcb5c1 238
borlanic 0:380207fcb5c1 239 previousValueCounter1 = valueCounter1;
borlanic 0:380207fcb5c1 240 previousValueCounter2 = valueCounter2;
borlanic 0:380207fcb5c1 241 previousValueCounter3 = valueCounter3;
borlanic 0:380207fcb5c1 242
borlanic 0:380207fcb5c1 243 actualSpeed1 = speedFilter1.filter((float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 244 actualSpeed2 = speedFilter2.filter((float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 245 actualSpeed3 = speedFilter3.filter((float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
borlanic 0:380207fcb5c1 246
borlanic 0:380207fcb5c1 247 float d_phiX = ((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX;
borlanic 0:380207fcb5c1 248 float d_phiY = (actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY;
borlanic 0:380207fcb5c1 249
borlanic 0:380207fcb5c1 250 // Calculate Torque motors
borlanic 0:380207fcb5c1 251
borlanic 0:380207fcb5c1 252 // Calculate duty cycles from desired Torque
borlanic 0:380207fcb5c1 253
borlanic 0:380207fcb5c1 254 // actual robot pose
borlanic 0:380207fcb5c1 255
borlanic 0:380207fcb5c1 256 // set new gamma's, phi's
borlanic 0:380207fcb5c1 257 previousGammaX = gammaX;
borlanic 0:380207fcb5c1 258 previousGammaY = gammaY;
borlanic 0:380207fcb5c1 259 previousGammaZ = gammaZ;
borlanic 0:380207fcb5c1 260 previousPhiX = phiX;
borlanic 0:380207fcb5c1 261 previousPhiY = phiY;
borlanic 0:380207fcb5c1 262 }
borlanic 0:380207fcb5c1 263 };