BA / Mbed OS BaBoRo_test2

Backup 1

Controller.cpp@0:02dd72d1d465, 2018-04-24 (annotated)

Committer:
borlanic
Date:
Tue Apr 24 11:45:18 2018 +0000
Revision:
0:02dd72d1d465
BaBoRo_test2 - backup 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:02dd72d1d465 1 /*
borlanic 0:02dd72d1d465 2 * Controller.cpp
borlanic 0:02dd72d1d465 3 * Copyright (c) 2018, ZHAW
borlanic 0:02dd72d1d465 4 * All rights reserved.
borlanic 0:02dd72d1d465 5 *
borlanic 0:02dd72d1d465 6 * Created on: 27.03.2018
borlanic 0:02dd72d1d465 7 * Author: BaBoRo Development Team
borlanic 0:02dd72d1d465 8 */
borlanic 0:02dd72d1d465 9
borlanic 0:02dd72d1d465 10 #include <cmath>
borlanic 0:02dd72d1d465 11 #include "Controller.h"
borlanic 0:02dd72d1d465 12
borlanic 0:02dd72d1d465 13 using namespace std;
borlanic 0:02dd72d1d465 14
borlanic 0:02dd72d1d465 15 const float Controller::PERIOD = 0.001f; // the period of the timer interrupt, given in [s]
borlanic 0:02dd72d1d465 16 const float Controller::ALPHA = 0.785398163397448f; // alpha = 45° in [rad]
borlanic 0:02dd72d1d465 17 const float Controller::RB = 0.0925f; // Ball Radius in [m]
borlanic 0:02dd72d1d465 18 const float Controller::RW = 0.024f; // Wheel Radius in [m]
borlanic 0:02dd72d1d465 19 const float Controller::PI = 3.141592653589793f; // PI
borlanic 0:02dd72d1d465 20 const float Controller::SQRT_3 = 1.732050807568877f; // Square root of 3
borlanic 0:02dd72d1d465 21 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
borlanic 0:02dd72d1d465 22 const float Controller::COUNTS_PER_TURN = 4096.0f;
borlanic 0:02dd72d1d465 23 const float Controller::KI = 58.5f; // torque constant [mNm/A]
borlanic 0:02dd72d1d465 24 const float Controller::MIN_DUTY_CYCLE = 0.1f; // minimum allowed value for duty cycle (10%)
borlanic 0:02dd72d1d465 25 const float Controller::MAX_DUTY_CYCLE = 0.9f; // maximum allowed value for duty cycle (90%)
borlanic 0:02dd72d1d465 26 const float Controller::MAX_ACC_M = 2.0f;
borlanic 0:02dd72d1d465 27 /**
borlanic 0:02dd72d1d465 28 * Create and initialize a robot controller object.
borlanic 0:02dd72d1d465 29 * @param pwm0 a pwm output object to set the duty cycle for the first motor.
borlanic 0:02dd72d1d465 30 * @param pwm1 a pwm output object to set the duty cycle for the second motor.
borlanic 0:02dd72d1d465 31 * @param pwm2 a pwm output object to set the duty cycle for the third motor.
borlanic 0:02dd72d1d465 32 * @param counter1
borlanic 0:02dd72d1d465 33 */
borlanic 0:02dd72d1d465 34 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:02dd72d1d465 35 {
borlanic 0:02dd72d1d465 36
borlanic 0:02dd72d1d465 37 // initialize local values
borlanic 0:02dd72d1d465 38
borlanic 0:02dd72d1d465 39 pwm0.period(0.0005f);// 0.5ms 2KHz
borlanic 0:02dd72d1d465 40 pwm0.write(0.5f);
borlanic 0:02dd72d1d465 41
borlanic 0:02dd72d1d465 42 pwm1.period(0.0005f);
borlanic 0:02dd72d1d465 43 pwm1.write(0.5f);
borlanic 0:02dd72d1d465 44
borlanic 0:02dd72d1d465 45 pwm2.period(0.0005f);
borlanic 0:02dd72d1d465 46 pwm2.write(0.5f);
borlanic 0:02dd72d1d465 47
borlanic 0:02dd72d1d465 48 gammaX = imu.getGammaX();
borlanic 0:02dd72d1d465 49 gammaY = imu.getGammaY();
borlanic 0:02dd72d1d465 50 gammaZ = imu.getGammaZ();
borlanic 0:02dd72d1d465 51
borlanic 0:02dd72d1d465 52 phiX = 0.0f;
borlanic 0:02dd72d1d465 53 phiY = 0.0f;
borlanic 0:02dd72d1d465 54
borlanic 0:02dd72d1d465 55 d_phiX = 0.0f;
borlanic 0:02dd72d1d465 56 d_phiY = 0.0f;
borlanic 0:02dd72d1d465 57
borlanic 0:02dd72d1d465 58 x = 0.0f;
borlanic 0:02dd72d1d465 59 y = 0.0f;
borlanic 0:02dd72d1d465 60
borlanic 0:02dd72d1d465 61 float previousValueCounter1 = counter1.read();
borlanic 0:02dd72d1d465 62 float previousValueCounter2 = counter2.read();
borlanic 0:02dd72d1d465 63 float previousValueCounter3 = -counter3.read();
borlanic 0:02dd72d1d465 64
borlanic 0:02dd72d1d465 65 speedFilter1.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 66 speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:02dd72d1d465 67
borlanic 0:02dd72d1d465 68 speedFilter2.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 69 speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:02dd72d1d465 70
borlanic 0:02dd72d1d465 71 speedFilter3.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 72 speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
borlanic 0:02dd72d1d465 73
borlanic 0:02dd72d1d465 74 d_phiXFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 75 d_phiXFilter.setFrequency(10.0f);
borlanic 0:02dd72d1d465 76
borlanic 0:02dd72d1d465 77 d_phiYFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 78 d_phiYFilter.setFrequency(10.0f);
borlanic 0:02dd72d1d465 79
borlanic 0:02dd72d1d465 80 gammaXFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 81 gammaXFilter.setFrequency(100.0f);
borlanic 0:02dd72d1d465 82 gammaYFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 83 gammaYFilter.setFrequency(100.0f);
borlanic 0:02dd72d1d465 84 d_gammaXFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 85 d_gammaXFilter.setFrequency(100.0f);
borlanic 0:02dd72d1d465 86 d_gammaYFilter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 87 d_gammaYFilter.setFrequency(100.0f);
borlanic 0:02dd72d1d465 88
borlanic 0:02dd72d1d465 89 M1Filter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 90 M1Filter.setFrequency(50.0f);
borlanic 0:02dd72d1d465 91
borlanic 0:02dd72d1d465 92 M2Filter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 93 M2Filter.setFrequency(50.0f);
borlanic 0:02dd72d1d465 94
borlanic 0:02dd72d1d465 95 M3Filter.setPeriod(PERIOD);
borlanic 0:02dd72d1d465 96 M3Filter.setFrequency(50.0f);
borlanic 0:02dd72d1d465 97
borlanic 0:02dd72d1d465 98 previousValueCounter1 = 0.0f;
borlanic 0:02dd72d1d465 99 previousValueCounter2 = 0.0f;
borlanic 0:02dd72d1d465 100 previousValueCounter3 = 0.0f;
borlanic 0:02dd72d1d465 101
borlanic 0:02dd72d1d465 102 actualSpeed1 = 0.0f;
borlanic 0:02dd72d1d465 103 actualSpeed2 = 0.0f;
borlanic 0:02dd72d1d465 104 actualSpeed3 = 0.0f;
borlanic 0:02dd72d1d465 105
borlanic 0:02dd72d1d465 106 gammaXref = 0.0f;
borlanic 0:02dd72d1d465 107 gammaYref = 0.0f;
borlanic 0:02dd72d1d465 108 gammaZref = 0.0f;
borlanic 0:02dd72d1d465 109 phiXref = 0.0f;
borlanic 0:02dd72d1d465 110 phiYref = 0.0f;
borlanic 0:02dd72d1d465 111
borlanic 0:02dd72d1d465 112 M1motion.setProfileVelocity(0.4f);
borlanic 0:02dd72d1d465 113 M1motion.setProfileAcceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 114 M1motion.setProfileDeceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 115
borlanic 0:02dd72d1d465 116 M2motion.setProfileVelocity(0.4f);
borlanic 0:02dd72d1d465 117 M2motion.setProfileAcceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 118 M2motion.setProfileDeceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 119
borlanic 0:02dd72d1d465 120 M3motion.setProfileVelocity(0.4f);
borlanic 0:02dd72d1d465 121 M3motion.setProfileAcceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 122 M3motion.setProfileDeceleration(MAX_ACC_M);
borlanic 0:02dd72d1d465 123
borlanic 0:02dd72d1d465 124 d_phiXMotion.setProfileVelocity(0.5f);
borlanic 0:02dd72d1d465 125 d_phiXMotion.setProfileAcceleration(1.0f);
borlanic 0:02dd72d1d465 126 d_phiXMotion.setProfileDeceleration(2.0f);
borlanic 0:02dd72d1d465 127
borlanic 0:02dd72d1d465 128 d_phiYMotion.setProfileVelocity(0.5f);
borlanic 0:02dd72d1d465 129 d_phiYMotion.setProfileAcceleration(1.0f);
borlanic 0:02dd72d1d465 130 d_phiYMotion.setProfileDeceleration(2.0f);
borlanic 0:02dd72d1d465 131
borlanic 0:02dd72d1d465 132 // start thread and timer interrupt
borlanic 0:02dd72d1d465 133
borlanic 0:02dd72d1d465 134 thread.start(callback(this, &Controller::run));
borlanic 0:02dd72d1d465 135 //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
borlanic 0:02dd72d1d465 136 }
borlanic 0:02dd72d1d465 137
borlanic 0:02dd72d1d465 138 /**
borlanic 0:02dd72d1d465 139 * Delete the robot controller object and release all allocated resources.
borlanic 0:02dd72d1d465 140 */
borlanic 0:02dd72d1d465 141 Controller::~Controller()
borlanic 0:02dd72d1d465 142 {
borlanic 0:02dd72d1d465 143
borlanic 0:02dd72d1d465 144 //ticker.detach();
borlanic 0:02dd72d1d465 145 }
borlanic 0:02dd72d1d465 146
borlanic 0:02dd72d1d465 147 // set --------------------------------
borlanic 0:02dd72d1d465 148 void Controller::setGammaX(float gammaX)
borlanic 0:02dd72d1d465 149 {
borlanic 0:02dd72d1d465 150
borlanic 0:02dd72d1d465 151 this->gammaX = gammaX;
borlanic 0:02dd72d1d465 152 }
borlanic 0:02dd72d1d465 153
borlanic 0:02dd72d1d465 154
borlanic 0:02dd72d1d465 155 void Controller::setGammaY(float gammaY)
borlanic 0:02dd72d1d465 156 {
borlanic 0:02dd72d1d465 157
borlanic 0:02dd72d1d465 158 this->gammaY = gammaY;
borlanic 0:02dd72d1d465 159 }
borlanic 0:02dd72d1d465 160
borlanic 0:02dd72d1d465 161
borlanic 0:02dd72d1d465 162 void Controller::setGammaZ(float gammaZ)
borlanic 0:02dd72d1d465 163 {
borlanic 0:02dd72d1d465 164
borlanic 0:02dd72d1d465 165 this->gammaZ = gammaZ;
borlanic 0:02dd72d1d465 166 }
borlanic 0:02dd72d1d465 167
borlanic 0:02dd72d1d465 168
borlanic 0:02dd72d1d465 169 void Controller::setPhiX(float phiX)
borlanic 0:02dd72d1d465 170 {
borlanic 0:02dd72d1d465 171
borlanic 0:02dd72d1d465 172 this->phiX = phiX;
borlanic 0:02dd72d1d465 173 }
borlanic 0:02dd72d1d465 174
borlanic 0:02dd72d1d465 175 void Controller::setPhiY(float phiY)
borlanic 0:02dd72d1d465 176 {
borlanic 0:02dd72d1d465 177
borlanic 0:02dd72d1d465 178 this->phiY = phiY;
borlanic 0:02dd72d1d465 179 }
borlanic 0:02dd72d1d465 180
borlanic 0:02dd72d1d465 181 void Controller::setX(float x)
borlanic 0:02dd72d1d465 182 {
borlanic 0:02dd72d1d465 183
borlanic 0:02dd72d1d465 184 this->x = x;
borlanic 0:02dd72d1d465 185 }
borlanic 0:02dd72d1d465 186
borlanic 0:02dd72d1d465 187 /**
borlanic 0:02dd72d1d465 188 * Sets the actual y coordinate of the robots position.
borlanic 0:02dd72d1d465 189 * @param y the y coordinate of the position, given in [m].
borlanic 0:02dd72d1d465 190 */
borlanic 0:02dd72d1d465 191 void Controller::setY(float y)
borlanic 0:02dd72d1d465 192 {
borlanic 0:02dd72d1d465 193
borlanic 0:02dd72d1d465 194 this->y = y;
borlanic 0:02dd72d1d465 195 }
borlanic 0:02dd72d1d465 196
borlanic 0:02dd72d1d465 197 // get --------------------------------
borlanic 0:02dd72d1d465 198
borlanic 0:02dd72d1d465 199 float Controller::getPhiX()
borlanic 0:02dd72d1d465 200 {
borlanic 0:02dd72d1d465 201
borlanic 0:02dd72d1d465 202 return phiX;
borlanic 0:02dd72d1d465 203 }
borlanic 0:02dd72d1d465 204
borlanic 0:02dd72d1d465 205 float Controller::getPhiY()
borlanic 0:02dd72d1d465 206 {
borlanic 0:02dd72d1d465 207
borlanic 0:02dd72d1d465 208 return phiY;
borlanic 0:02dd72d1d465 209 }
borlanic 0:02dd72d1d465 210
borlanic 0:02dd72d1d465 211 /**
borlanic 0:02dd72d1d465 212 * Gets the actual x coordinate of the robots position.
borlanic 0:02dd72d1d465 213 * @return the x coordinate of the position, given in [m].
borlanic 0:02dd72d1d465 214 */
borlanic 0:02dd72d1d465 215 float Controller::getX()
borlanic 0:02dd72d1d465 216 {
borlanic 0:02dd72d1d465 217
borlanic 0:02dd72d1d465 218 return x;
borlanic 0:02dd72d1d465 219 }
borlanic 0:02dd72d1d465 220
borlanic 0:02dd72d1d465 221 /**
borlanic 0:02dd72d1d465 222 * Gets the actual y coordinate of the robots position.
borlanic 0:02dd72d1d465 223 * @return the y coordinate of the position, given in [m].
borlanic 0:02dd72d1d465 224 */
borlanic 0:02dd72d1d465 225 float Controller::getY()
borlanic 0:02dd72d1d465 226 {
borlanic 0:02dd72d1d465 227
borlanic 0:02dd72d1d465 228 return y;
borlanic 0:02dd72d1d465 229 }
borlanic 0:02dd72d1d465 230
borlanic 0:02dd72d1d465 231 /**
borlanic 0:02dd72d1d465 232 * This method is called by the ticker timer interrupt service routine.
borlanic 0:02dd72d1d465 233 * It sends a signal to the thread to make it run again.
borlanic 0:02dd72d1d465 234 */
borlanic 0:02dd72d1d465 235 //void Controller::sendSignal() {
borlanic 0:02dd72d1d465 236
borlanic 0:02dd72d1d465 237 // thread.signal_set(signal);
borlanic 0:02dd72d1d465 238 //}
borlanic 0:02dd72d1d465 239
borlanic 0:02dd72d1d465 240 /**
borlanic 0:02dd72d1d465 241 * This <code>run()</code> method contains an infinite loop with the run logic.
borlanic 0:02dd72d1d465 242 */
borlanic 0:02dd72d1d465 243 void Controller::run()
borlanic 0:02dd72d1d465 244 {
borlanic 0:02dd72d1d465 245 Serial pc1(USBTX, USBRX); // tx, rx
borlanic 0:02dd72d1d465 246 pc1.baud(100000);
borlanic 0:02dd72d1d465 247
borlanic 0:02dd72d1d465 248 float integratedGammaX = 0.0f;
borlanic 0:02dd72d1d465 249 float integratedGammaY = 0.0f;
borlanic 0:02dd72d1d465 250 float integratedGammaZ = 0.0f;
borlanic 0:02dd72d1d465 251 float integratedPhiX = 0.0f;
borlanic 0:02dd72d1d465 252 float integratedPhiY = 0.0f;
borlanic 0:02dd72d1d465 253
borlanic 0:02dd72d1d465 254 float previousGammaX = 0.0;
borlanic 0:02dd72d1d465 255 float previousGammaY = 0.0;
borlanic 0:02dd72d1d465 256 float previousGammaZ = 0.0;
borlanic 0:02dd72d1d465 257 float previousPhiX = 0.0;
borlanic 0:02dd72d1d465 258 float previousPhiY = 0.0;
borlanic 0:02dd72d1d465 259
borlanic 0:02dd72d1d465 260 // K matrix
borlanic 0:02dd72d1d465 261 /*
borlanic 0:02dd72d1d465 262 float K[3][10] = {
borlanic 0:02dd72d1d465 263 {0.002327,0.000000,2.166704,0.000000,0.055133,0.032581,0.000000,0.517927,0.000000,0.031336},
borlanic 0:02dd72d1d465 264 {-0.001164,0.002015,-1.083352,1.876421,0.055133,-0.016291,0.028216,-0.258963,0.448538,0.031336},
borlanic 0:02dd72d1d465 265 {-0.001164,-0.002015,-1.083352,-1.876421,0.055133,-0.016291,-0.028216,-0.258963,-0.448538,0.031336}
borlanic 0:02dd72d1d465 266 };
borlanic 0:02dd72d1d465 267 */
borlanic 0:02dd72d1d465 268
borlanic 0:02dd72d1d465 269 float K[3][10] = {
borlanic 0:02dd72d1d465 270 {0.002327,0.000000,2.362528,-0.000000,0.055133,0.009269,0.000000,0.499429,0.000000,0.031336},
borlanic 0:02dd72d1d465 271 {-0.001164,0.002015,-1.181264,2.046009,0.055133,-0.004635,0.008027,-0.249714,0.432518,0.031336},
borlanic 0:02dd72d1d465 272 {-0.001164,-0.002015,-1.181264,-2.046009,0.055133,-0.004635,-0.008027,-0.249714,-0.432518,0.031336}
borlanic 0:02dd72d1d465 273 };
borlanic 0:02dd72d1d465 274
borlanic 0:02dd72d1d465 275
borlanic 0:02dd72d1d465 276 float rad1 = 0.0f;
borlanic 0:02dd72d1d465 277 float rad2 = 0.0f;
borlanic 0:02dd72d1d465 278 float rad3 = 0.0f;
borlanic 0:02dd72d1d465 279
borlanic 0:02dd72d1d465 280 float rad1contr = 0.0f;
borlanic 0:02dd72d1d465 281
borlanic 0:02dd72d1d465 282 long int t = 0;
borlanic 0:02dd72d1d465 283
borlanic 0:02dd72d1d465 284 // messung
borlanic 0:02dd72d1d465 285 int * T = new int[2000];
borlanic 0:02dd72d1d465 286 //float * PX = new float[5000];
borlanic 0:02dd72d1d465 287 //float * PY = new float[5000];
borlanic 0:02dd72d1d465 288 float * GX = new float[2000];
borlanic 0:02dd72d1d465 289 float * GY = new float[2000];
borlanic 0:02dd72d1d465 290 float * GZ = new float[2000];
borlanic 0:02dd72d1d465 291 //float * dPX = new float[5000];
borlanic 0:02dd72d1d465 292 //float * dPY = new float[5000];
borlanic 0:02dd72d1d465 293 float * dGX = new float[2000];
borlanic 0:02dd72d1d465 294 float * dGY = new float[2000];
borlanic 0:02dd72d1d465 295 float * dGZ = new float[2000];
borlanic 0:02dd72d1d465 296 float * W1 = new float[2000];
borlanic 0:02dd72d1d465 297 float * W2 = new float[2000];
borlanic 0:02dd72d1d465 298 float * W3 = new float[2000];
borlanic 0:02dd72d1d465 299
borlanic 0:02dd72d1d465 300 int a = 0;
borlanic 0:02dd72d1d465 301
borlanic 0:02dd72d1d465 302 AnalogIn M3_AOUT1(PA_7);
borlanic 0:02dd72d1d465 303
borlanic 0:02dd72d1d465 304 pwm0.write(0.6);
borlanic 0:02dd72d1d465 305 pwm1.write(0.6);
borlanic 0:02dd72d1d465 306 pwm2.write(0.6);
borlanic 0:02dd72d1d465 307
borlanic 0:02dd72d1d465 308 while (true) {
borlanic 0:02dd72d1d465 309
borlanic 0:02dd72d1d465 310 /*
borlanic 0:02dd72d1d465 311 if(t==21000) {
borlanic 0:02dd72d1d465 312 pc1.printf("invio dati:\r\n\n");
borlanic 0:02dd72d1d465 313 for(int j=0; j<2000; j++) {
borlanic 0:02dd72d1d465 314 //pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(PX+j),*(PY+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(dPX+j),*(dPY+j),*(W1+j),*(W2+j),*(W3+j));
borlanic 0:02dd72d1d465 315 pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(W1+j),*(W2+j),*(W3+j));
borlanic 0:02dd72d1d465 316 }
borlanic 0:02dd72d1d465 317 pc1.printf("fine dati:\r\n\n");
borlanic 0:02dd72d1d465 318 delete T;
borlanic 0:02dd72d1d465 319 //delete PX;
borlanic 0:02dd72d1d465 320 //delete PY;
borlanic 0:02dd72d1d465 321 delete GX;
borlanic 0:02dd72d1d465 322 delete GY;
borlanic 0:02dd72d1d465 323 delete GZ;
borlanic 0:02dd72d1d465 324 //delete dPX;
borlanic 0:02dd72d1d465 325 //delete dPY;
borlanic 0:02dd72d1d465 326 delete dGX;
borlanic 0:02dd72d1d465 327 delete dGY;
borlanic 0:02dd72d1d465 328 delete dGZ;
borlanic 0:02dd72d1d465 329 delete W1;
borlanic 0:02dd72d1d465 330 delete W2;
borlanic 0:02dd72d1d465 331 delete W3;
borlanic 0:02dd72d1d465 332
borlanic 0:02dd72d1d465 333 }
borlanic 0:02dd72d1d465 334 */
borlanic 0:02dd72d1d465 335
borlanic 0:02dd72d1d465 336
borlanic 0:02dd72d1d465 337
borlanic 0:02dd72d1d465 338
borlanic 0:02dd72d1d465 339 mutex.lock();
borlanic 0:02dd72d1d465 340 //printf("Controller start\r\n");
borlanic 0:02dd72d1d465 341
borlanic 0:02dd72d1d465 342 gammaX = imu.getGammaX();
borlanic 0:02dd72d1d465 343 gammaY = imu.getGammaY();
borlanic 0:02dd72d1d465 344 gammaZ = imu.getGammaZ();
borlanic 0:02dd72d1d465 345 d_gammaX = imu.getDGammaX();
borlanic 0:02dd72d1d465 346 d_gammaY = imu.getDGammaY();
borlanic 0:02dd72d1d465 347 d_gammaZ = imu.getDGammaZ();
borlanic 0:02dd72d1d465 348
borlanic 0:02dd72d1d465 349
borlanic 0:02dd72d1d465 350
borlanic 0:02dd72d1d465 351 // wait for the periodic signal
borlanic 0:02dd72d1d465 352
borlanic 0:02dd72d1d465 353 // thread.signal_wait(signal);
borlanic 0:02dd72d1d465 354
borlanic 0:02dd72d1d465 355 //printf("%d %d %d\r\n",counter1.read(),counter2.read(),counter3.read());
borlanic 0:02dd72d1d465 356 //pwm0.write(0.6f);
borlanic 0:02dd72d1d465 357 //pwm1.write(0.6f);
borlanic 0:02dd72d1d465 358 //pwm2.write(0.6f);
borlanic 0:02dd72d1d465 359
borlanic 0:02dd72d1d465 360
borlanic 0:02dd72d1d465 361
borlanic 0:02dd72d1d465 362
borlanic 0:02dd72d1d465 363 // Calculate Ball Velocities
borlanic 0:02dd72d1d465 364 short valueCounter1 = counter1.read();
borlanic 0:02dd72d1d465 365 short valueCounter2 = counter2.read();
borlanic 0:02dd72d1d465 366 short valueCounter3 = -counter3.read();
borlanic 0:02dd72d1d465 367
borlanic 0:02dd72d1d465 368 short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
borlanic 0:02dd72d1d465 369 short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
borlanic 0:02dd72d1d465 370 short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
borlanic 0:02dd72d1d465 371
borlanic 0:02dd72d1d465 372 previousValueCounter1 = valueCounter1;
borlanic 0:02dd72d1d465 373 previousValueCounter2 = valueCounter2;
borlanic 0:02dd72d1d465 374 previousValueCounter3 = valueCounter3;
borlanic 0:02dd72d1d465 375
borlanic 0:02dd72d1d465 376 actualSpeed1 = speedFilter1.filter((2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
borlanic 0:02dd72d1d465 377 actualSpeed2 = speedFilter2.filter((2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
borlanic 0:02dd72d1d465 378 actualSpeed3 = speedFilter3.filter((2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
borlanic 0:02dd72d1d465 379
borlanic 0:02dd72d1d465 380 rad1 += actualSpeed1*PERIOD;
borlanic 0:02dd72d1d465 381 rad2 += actualSpeed2*PERIOD;
borlanic 0:02dd72d1d465 382 rad3 += actualSpeed3*PERIOD;
borlanic 0:02dd72d1d465 383
borlanic 0:02dd72d1d465 384 //printf("rad1 = %.7f rad2 = %.7f rad3 = %.7f\r\n",rad1,rad2,rad3);
borlanic 0:02dd72d1d465 385
borlanic 0:02dd72d1d465 386 //printf("%.7f %.7f %.7f %.7f %.7f %.7f\r\n",(2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD,actualSpeed1,(2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD,actualSpeed2,(2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD,actualSpeed3);
borlanic 0:02dd72d1d465 387
borlanic 0:02dd72d1d465 388
borlanic 0:02dd72d1d465 389 //printf("%.7f %.7f %.7f\r\n",actualSpeed1/2*PI,actualSpeed2/2*PI,actualSpeed3/2*PI);
borlanic 0:02dd72d1d465 390
borlanic 0:02dd72d1d465 391 float actualDPhiX = d_phiXFilter.filter(RW/RB*((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*0.0f)/(2*cos(ALPHA))+d_gammaX);
borlanic 0:02dd72d1d465 392 float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3))/(SQRT_3*cos(ALPHA))+d_gammaY); //float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3 - actualSpeed1)+cos(ALPHA)*(actualDPhiX-gammaX)+sin(ALPHA)*0.0f)/(SQRT_3*cos(ALPHA))+d_gammaY);
borlanic 0:02dd72d1d465 393
borlanic 0:02dd72d1d465 394 //printf("%.7f %.7f %.7f\r\n",d_gammaX,d_gammaY,d_gammaZ);
borlanic 0:02dd72d1d465 395
borlanic 0:02dd72d1d465 396 //this->d_phiX = RB/RW*(((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX);
borlanic 0:02dd72d1d465 397 //this->d_phiY = RB/RW*((actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY);
borlanic 0:02dd72d1d465 398
borlanic 0:02dd72d1d465 399 this->d_phiX = actualDPhiX;
borlanic 0:02dd72d1d465 400 this->d_phiY = actualDPhiY;
borlanic 0:02dd72d1d465 401
borlanic 0:02dd72d1d465 402 this->phiX = phiX + actualDPhiX*PERIOD;
borlanic 0:02dd72d1d465 403 this->phiY = phiY + actualDPhiY*PERIOD;
borlanic 0:02dd72d1d465 404
borlanic 0:02dd72d1d465 405
borlanic 0:02dd72d1d465 406
borlanic 0:02dd72d1d465 407
borlanic 0:02dd72d1d465 408 //printf("phiX = %.5f phiY = %.5f\r\n",phiX/PI*180.0f,phiY/PI*180.0f);
borlanic 0:02dd72d1d465 409 //printf("%.5f %.5f\r\n",phiX,phiY);
borlanic 0:02dd72d1d465 410 //printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,actualSpeed1,actualSpeed2,actualSpeed3);
borlanic 0:02dd72d1d465 411 //printf("%.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ);
borlanic 0:02dd72d1d465 412
borlanic 0:02dd72d1d465 413 // calculate actual error
borlanic 0:02dd72d1d465 414 float errorGammaX = -gammaXref + gammaX;
borlanic 0:02dd72d1d465 415 float errorGammaY = -gammaYref + gammaY;
borlanic 0:02dd72d1d465 416 float errorGammaZ = -gammaZref + gammaZ;
borlanic 0:02dd72d1d465 417 float errorPhiX = -phiXref + phiX;
borlanic 0:02dd72d1d465 418 float errorPhiY = -phiYref + phiY;
borlanic 0:02dd72d1d465 419
borlanic 0:02dd72d1d465 420 // Integration states
borlanic 0:02dd72d1d465 421 integratedGammaX = integratedGammaX + (errorGammaX-previousGammaX)*PERIOD;
borlanic 0:02dd72d1d465 422 integratedGammaY = integratedGammaY + (errorGammaY-previousGammaY)*PERIOD;
borlanic 0:02dd72d1d465 423 integratedGammaZ = integratedGammaZ + (errorGammaZ-previousGammaZ)*PERIOD;
borlanic 0:02dd72d1d465 424 integratedPhiX = integratedPhiX + (errorPhiX-previousPhiX)*PERIOD;
borlanic 0:02dd72d1d465 425 integratedPhiY = integratedPhiY + (errorPhiY-previousPhiY)*PERIOD;
borlanic 0:02dd72d1d465 426
borlanic 0:02dd72d1d465 427 // set new gamma's, phi's
borlanic 0:02dd72d1d465 428 previousGammaX = errorGammaX;
borlanic 0:02dd72d1d465 429 previousGammaY = errorGammaY;
borlanic 0:02dd72d1d465 430 previousGammaZ = errorGammaZ;
borlanic 0:02dd72d1d465 431 previousPhiX = errorPhiX;
borlanic 0:02dd72d1d465 432 previousPhiY = errorPhiY;
borlanic 0:02dd72d1d465 433
borlanic 0:02dd72d1d465 434 // Calculate Torque motors
borlanic 0:02dd72d1d465 435
borlanic 0:02dd72d1d465 436 //[M1,M2,M3]=K*x
borlanic 0:02dd72d1d465 437
borlanic 0:02dd72d1d465 438 float M1 = 0.0f;
borlanic 0:02dd72d1d465 439 float M2 = 0.0f;
borlanic 0:02dd72d1d465 440 float M3 = 0.0f;
borlanic 0:02dd72d1d465 441
borlanic 0:02dd72d1d465 442 d_phiXMotion.incrementToVelocity(d_phiX,PERIOD);
borlanic 0:02dd72d1d465 443 d_phiYMotion.incrementToVelocity(d_phiY,PERIOD);
borlanic 0:02dd72d1d465 444
borlanic 0:02dd72d1d465 445 float x[10][1] = {
borlanic 0:02dd72d1d465 446 {0.0f},{0.0f},{(gammaX)},{(gammaY)},{0.0f},{0.0f},{0.0f},{d_gammaX},{d_gammaY},{0.0f}
borlanic 0:02dd72d1d465 447 };
borlanic 0:02dd72d1d465 448
borlanic 0:02dd72d1d465 449 /*
borlanic 0:02dd72d1d465 450 if(gammaX<0.02f && gammaX>-0.02f) {
borlanic 0:02dd72d1d465 451 x[2][0]=0.0f;
borlanic 0:02dd72d1d465 452 }
borlanic 0:02dd72d1d465 453 if(gammaY<0.02f && gammaY>-0.02f) {
borlanic 0:02dd72d1d465 454 x[3][0]=0.0f;
borlanic 0:02dd72d1d465 455 }
borlanic 0:02dd72d1d465 456 if(d_gammaX<0.02f && gammaX>-0.02f) {
borlanic 0:02dd72d1d465 457 x[7][0]=0.0f;
borlanic 0:02dd72d1d465 458 }
borlanic 0:02dd72d1d465 459 if(d_gammaY<0.02f && gammaY>-0.02f) {
borlanic 0:02dd72d1d465 460 x[8][0]=0.0f;
borlanic 0:02dd72d1d465 461 }
borlanic 0:02dd72d1d465 462 */
borlanic 0:02dd72d1d465 463
borlanic 0:02dd72d1d465 464 for(int i=0; i<10; i++) {
borlanic 0:02dd72d1d465 465 M1 = M1 + K[0][i]*x[i][0];
borlanic 0:02dd72d1d465 466 M2 = M2 + K[1][i]*x[i][0];
borlanic 0:02dd72d1d465 467 M3 = M3 + K[2][i]*x[i][0];
borlanic 0:02dd72d1d465 468 };
borlanic 0:02dd72d1d465 469
borlanic 0:02dd72d1d465 470 printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",gammaX,gammaY,d_gammaX,d_gammaY);
borlanic 0:02dd72d1d465 471
borlanic 0:02dd72d1d465 472 M1motion.incrementToPosition(M1, PERIOD);
borlanic 0:02dd72d1d465 473 M2motion.incrementToPosition(M2, PERIOD);
borlanic 0:02dd72d1d465 474 M3motion.incrementToPosition(M3, PERIOD);
borlanic 0:02dd72d1d465 475
borlanic 0:02dd72d1d465 476 if(t<20001) {
borlanic 0:02dd72d1d465 477 if (t % 10 == 0) {
borlanic 0:02dd72d1d465 478 *(T+a) = t;
borlanic 0:02dd72d1d465 479 //*(PX+a) = phiX;
borlanic 0:02dd72d1d465 480 //*(PY+a) = phiY;
borlanic 0:02dd72d1d465 481 *(GX+a) = gammaXFilter.filter(gammaX);
borlanic 0:02dd72d1d465 482 *(GY+a) = gammaYFilter.filter(gammaY);
borlanic 0:02dd72d1d465 483 *(GZ+a) = gammaZ;
borlanic 0:02dd72d1d465 484 //*(dPX+a) = d_phiX;
borlanic 0:02dd72d1d465 485 //*(dPY+a) = d_phiY;
borlanic 0:02dd72d1d465 486 *(dGX+a) = d_gammaXFilter.filter(d_gammaX);
borlanic 0:02dd72d1d465 487 *(dGY+a) = d_gammaYFilter.filter(d_gammaY);
borlanic 0:02dd72d1d465 488 *(dGZ+a) = d_gammaZ;
borlanic 0:02dd72d1d465 489 *(W1+a) = M1;
borlanic 0:02dd72d1d465 490 *(W2+a) = M2;
borlanic 0:02dd72d1d465 491 *(W3+a) = M3;
borlanic 0:02dd72d1d465 492 a++;
borlanic 0:02dd72d1d465 493 }
borlanic 0:02dd72d1d465 494 }
borlanic 0:02dd72d1d465 495
borlanic 0:02dd72d1d465 496 //pc1.printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,M1,M2,M3);
borlanic 0:02dd72d1d465 497 //if(t<2000) {
borlanic 0:02dd72d1d465 498 //messung[0][t]=t/100.0f;
borlanic 0:02dd72d1d465 499 //messung[1][t]=gammaX;
borlanic 0:02dd72d1d465 500 //messung[2][t]=gammaY;
borlanic 0:02dd72d1d465 501 //pc1.printf("%.2f\r\n", t/100.0f);
borlanic 0:02dd72d1d465 502 //}
borlanic 0:02dd72d1d465 503
borlanic 0:02dd72d1d465 504 //pc1.printf("%.2f %.2f %.2f\r\n", t/100.0f,gammaX,gammaY);
borlanic 0:02dd72d1d465 505 //if(t>2100 && t<4000) {
borlanic 0:02dd72d1d465 506 //pc1.printf("%.2f %.7f %.7f\r\n",1.0,1.0,1.0);//messung[0][t-2100], messung[1][t-2100], messung[2][t-2100]
borlanic 0:02dd72d1d465 507 //}
borlanic 0:02dd72d1d465 508
borlanic 0:02dd72d1d465 509 //printf("Mi %.7f %.7f %.7f motion Mi %.7f %.7f %.7f\r\n", M1,M2,M3,M1motion.velocity,M2motion.velocity,M3motion.velocity);
borlanic 0:02dd72d1d465 510 //printf("%.7f %.7f\r\n", gammaX,gammaY);
borlanic 0:02dd72d1d465 511
borlanic 0:02dd72d1d465 512 //printf("x:\r\n %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n M:\r\n %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,integratedPhiX,integratedPhiY,integratedGammaX,integratedGammaY,integratedGammaZ,M1,M2,M3);
borlanic 0:02dd72d1d465 513
borlanic 0:02dd72d1d465 514 // Calculate duty cycles from desired Torque, limit and set duty cycles
borlanic 0:02dd72d1d465 515 //float I1 = -M1*1000.0f/KI;
borlanic 0:02dd72d1d465 516 //float I2 = -M2*1000.0f/KI;
borlanic 0:02dd72d1d465 517 //float I3 = -M3*1000.0f/KI;
borlanic 0:02dd72d1d465 518 float I1 = -M1motion.velocity*1000.0f/KI; //cos(0.1f*(float)t)
borlanic 0:02dd72d1d465 519 float I2 = -M2motion.velocity*1000.0f/KI;
borlanic 0:02dd72d1d465 520 float I3 = -M3motion.velocity*1000.0f/KI;
borlanic 0:02dd72d1d465 521
borlanic 0:02dd72d1d465 522 //printf("%.7f %.7f\r\n",M3_AOUT1.read()* 3.3f *4.0f - 6.0f,I3);
borlanic 0:02dd72d1d465 523 //printf("%.7f %.7f %.7f\r\n",I1,I2,I3);
borlanic 0:02dd72d1d465 524 //printf("%.7f %.7f %.7f\r\n",phiX/PI*180.0f,phiY/PI*180.0f,phiY/PI*180.0f);
borlanic 0:02dd72d1d465 525
borlanic 0:02dd72d1d465 526 //pc1.printf("%.7f %.7f\r\n", M1,M1motion.velocity);
borlanic 0:02dd72d1d465 527
borlanic 0:02dd72d1d465 528 float dutyCycle1 = 0.4f/18.0f*I1 + 0.5f;
borlanic 0:02dd72d1d465 529 if (dutyCycle1 < MIN_DUTY_CYCLE) {
borlanic 0:02dd72d1d465 530 dutyCycle1 = MIN_DUTY_CYCLE;
borlanic 0:02dd72d1d465 531 } else if (dutyCycle1 > MAX_DUTY_CYCLE) {
borlanic 0:02dd72d1d465 532 dutyCycle1 = MAX_DUTY_CYCLE;
borlanic 0:02dd72d1d465 533 }
borlanic 0:02dd72d1d465 534 //pwm0.write(dutyCycle1);
borlanic 0:02dd72d1d465 535
borlanic 0:02dd72d1d465 536 float dutyCycle2 = 0.4f/18.0f*I2 + 0.5f;
borlanic 0:02dd72d1d465 537 if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
borlanic 0:02dd72d1d465 538 else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
borlanic 0:02dd72d1d465 539 //pwm1.write(dutyCycle2);
borlanic 0:02dd72d1d465 540
borlanic 0:02dd72d1d465 541 float dutyCycle3 = 0.4f/18.0f*I3 + 0.5f;
borlanic 0:02dd72d1d465 542 if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
borlanic 0:02dd72d1d465 543 else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
borlanic 0:02dd72d1d465 544 //pwm2.write(dutyCycle3);
borlanic 0:02dd72d1d465 545
borlanic 0:02dd72d1d465 546 //printf("pwm1 = %.5f pwm2 = %.5f pwm3 = %.5f\r\n",dutyCycle1,dutyCycle2,dutyCycle3);
borlanic 0:02dd72d1d465 547
borlanic 0:02dd72d1d465 548 // actual robot pose
borlanic 0:02dd72d1d465 549 this->x = phiY*RB;
borlanic 0:02dd72d1d465 550 this->y = phiX*RB;
borlanic 0:02dd72d1d465 551
borlanic 0:02dd72d1d465 552
borlanic 0:02dd72d1d465 553 t++;
borlanic 0:02dd72d1d465 554
borlanic 0:02dd72d1d465 555
borlanic 0:02dd72d1d465 556 //printf("Controller end\r\n");
borlanic 0:02dd72d1d465 557
borlanic 0:02dd72d1d465 558 mutex.unlock();
borlanic 0:02dd72d1d465 559
borlanic 0:02dd72d1d465 560 thread.wait(1.0);
borlanic 0:02dd72d1d465 561 }
borlanic 0:02dd72d1d465 562 };