Nicolas Borla / Mbed OS BBR_1Ebene

BBR 1 Ebene

Controller.cpp@0:fbdae7e6d805, 2018-05-14 (annotated)

Committer:
borlanic
Date:
Mon May 14 11:29:06 2018 +0000
Revision:
0:fbdae7e6d805
BBR

Who changed what in which revision?

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