Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@18:622e717da184, 2017-11-07 (annotated)

Committer:
Arnoud113
Date:
Tue Nov 07 10:39:27 2017 +0000
Revision:
18:622e717da184
Parent:
17:10c18ca3368b 
Final neat version for project report

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:77ad62c61c78 1 #include "mbed.h"
Arnoud113 0:77ad62c61c78 2 #include "MODSERIAL.h"
Arnoud113 0:77ad62c61c78 3 #include "math.h"
Arnoud113 2:2563d1d8461f 4 #include "FastPWM.h"
Arnoud113 3:b353ee86230a 5 #include "encoder.h"
Arnoud113 13:b5868fd8ffe9 6 #include "BiQuad.h"
Arnoud113 0:77ad62c61c78 7
Arnoud113 0:77ad62c61c78 8
Arnoud113 16:000f2ebbd16c 9 //Reference signals
Arnoud113 0:77ad62c61c78 10 DigitalOut gpo(D0);
Arnoud113 0:77ad62c61c78 11 DigitalOut ledb(LED_BLUE);
Arnoud113 0:77ad62c61c78 12 DigitalOut ledr(LED_RED);
Arnoud113 0:77ad62c61c78 13 DigitalOut ledg(LED_GREEN);
Arnoud113 12:02eba9a294d2 14
Arnoud113 16:000f2ebbd16c 15 //Motors
Arnoud113 0:77ad62c61c78 16 DigitalOut motor1DC(D7);
Arnoud113 3:b353ee86230a 17 DigitalOut motor2DC(D4);
Arnoud113 8:b932f8b71d3a 18 FastPWM motor1PWM(D6);
Arnoud113 8:b932f8b71d3a 19 FastPWM motor2PWM(D5);
Arnoud113 0:77ad62c61c78 20
Arnoud113 16:000f2ebbd16c 21 //Position control
Arnoud113 12:02eba9a294d2 22 AnalogIn potMeter1(A0);
Arnoud113 12:02eba9a294d2 23 AnalogIn potMeter2(A1);
Arnoud113 12:02eba9a294d2 24 AnalogIn emg1(A2);
Arnoud113 12:02eba9a294d2 25 AnalogIn emg2(A3);
Arnoud113 12:02eba9a294d2 26 AnalogIn emg3(A4);
Arnoud113 12:02eba9a294d2 27 AnalogIn emg4(A5);
Arnoud113 16:000f2ebbd16c 28 Encoder Encoder1(D12,D13);
Arnoud113 16:000f2ebbd16c 29 Encoder Encoder2(D8,D9);
Arnoud113 12:02eba9a294d2 30
Arnoud113 17:10c18ca3368b 31 //callibrating
Arnoud113 17:10c18ca3368b 32 DigitalIn button1(SW2); //(x,y) = (..,..)
Arnoud113 17:10c18ca3368b 33 DigitalIn button2(SW3); //(x,y) = (..,..)
Arnoud113 0:77ad62c61c78 34
Arnoud113 0:77ad62c61c78 35 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:77ad62c61c78 36 Ticker controller;
Arnoud113 0:77ad62c61c78 37
Arnoud113 17:10c18ca3368b 38 // --- EMG ---
Arnoud113 17:10c18ca3368b 39 double in1;
Arnoud113 17:10c18ca3368b 40 double in2;
Arnoud113 17:10c18ca3368b 41 double RA;
Arnoud113 17:10c18ca3368b 42
Arnoud113 17:10c18ca3368b 43 double in3;
Arnoud113 17:10c18ca3368b 44 double in4;
Arnoud113 17:10c18ca3368b 45 double LA;
Arnoud113 17:10c18ca3368b 46
Arnoud113 17:10c18ca3368b 47 // Constants Position ----------------------- Start
Arnoud113 12:02eba9a294d2 48 double X;
Arnoud113 15:65f295a49a4b 49 double X_maxTreshold = 450;
Arnoud113 12:02eba9a294d2 50 double X_minTreshold = 20;
Arnoud113 12:02eba9a294d2 51 const double X_Callibrate = 300;
Arnoud113 12:02eba9a294d2 52
Arnoud113 12:02eba9a294d2 53 double Y;
Arnoud113 15:65f295a49a4b 54 double Y_maxTreshold = 450;
Arnoud113 15:65f295a49a4b 55 double Y_minTreshold = 0;
Arnoud113 12:02eba9a294d2 56 const double Y_Callibrate = 300;
Arnoud113 12:02eba9a294d2 57
Arnoud113 12:02eba9a294d2 58
Arnoud113 3:b353ee86230a 59 // ---- Motor Constants-------
Arnoud113 17:10c18ca3368b 60 double motor1;
Arnoud113 17:10c18ca3368b 61 double motor2;
Arnoud113 17:10c18ca3368b 62 int SetPosM1;
Arnoud113 17:10c18ca3368b 63 int SetPosM2;
Arnoud113 17:10c18ca3368b 64
Arnoud113 12:02eba9a294d2 65 const double pi = 3.1415926535897;
Arnoud113 17:10c18ca3368b 66 const double Pwmperiod = 0.001f;
Arnoud113 16:000f2ebbd16c 67 const double gainM1 = 1/29.17; // 1 / encoder pulses per degree theta
Arnoud113 16:000f2ebbd16c 68 const double gainM2 = 1/105.0; // 1 / pulses per mm
Arnoud113 0:77ad62c61c78 69
Arnoud113 14:54cbd8f0efe4 70 double reference_motor1 = 0; // reference for Theta
Arnoud113 14:54cbd8f0efe4 71 double reference_motor2 = 0;
Arnoud113 16:000f2ebbd16c 72 double pos_M1 = 0; // start angle theta
Arnoud113 16:000f2ebbd16c 73 double pos_M2 = 0; // start radius
Arnoud113 14:54cbd8f0efe4 74
Arnoud113 3:b353ee86230a 75
Arnoud113 3:b353ee86230a 76 //Start constants PID -------------------------------
Arnoud113 12:02eba9a294d2 77 const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
Arnoud113 0:77ad62c61c78 78
Arnoud113 11:66d0be7efd3f 79 const float M1_KP = 10;
Arnoud113 7:88d1ccba9200 80 const float M1_KI = 0.5;
Arnoud113 8:b932f8b71d3a 81 const float M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 7:88d1ccba9200 82 double m1_err_int = 0;
Arnoud113 7:88d1ccba9200 83 double m1_prev_err = 0;
Arnoud113 7:88d1ccba9200 84
Arnoud113 13:b5868fd8ffe9 85 const float M2_KP = 30;
Arnoud113 11:66d0be7efd3f 86 const float M2_KI = 0.5;
Arnoud113 18:622e717da184 87 const float M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5, KP increased for faster response
Arnoud113 7:88d1ccba9200 88 double m2_err_int = 0;
Arnoud113 7:88d1ccba9200 89 double m2_prev_err = 0;
Arnoud113 7:88d1ccba9200 90
Arnoud113 8:b932f8b71d3a 91 // Constants Biquad
Arnoud113 13:b5868fd8ffe9 92 BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
Arnoud113 13:b5868fd8ffe9 93 BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
Arnoud113 13:b5868fd8ffe9 94 BiQuadChain bqc;
Arnoud113 5:a1a5b5bebd5c 95 //---------------------------------End of constants PID
Arnoud113 0:77ad62c61c78 96
Arnoud113 3:b353ee86230a 97 //-----------------Start PID part----------------------------START
Arnoud113 18:622e717da184 98 double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1){
Arnoud113 3:b353ee86230a 99
Arnoud113 8:b932f8b71d3a 100 double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep // Derivative
Arnoud113 13:b5868fd8ffe9 101
Arnoud113 18:622e717da184 102 e_der1 = bqc.step(e_der1);
Arnoud113 7:88d1ccba9200 103 e_prev1 = e1;
Arnoud113 12:02eba9a294d2 104 e_int1 += Ts*e1;
Arnoud113 12:02eba9a294d2 105
Arnoud113 17:10c18ca3368b 106 if(button1 == 0 || button2 ==0){
Arnoud113 12:02eba9a294d2 107 e1 = 0;
Arnoud113 12:02eba9a294d2 108 e_prev1 = 0;
Arnoud113 12:02eba9a294d2 109 e_int1 = 0;
Arnoud113 12:02eba9a294d2 110 e_der1 = 0;
Arnoud113 12:02eba9a294d2 111 }
Arnoud113 12:02eba9a294d2 112 // Integral
Arnoud113 8:b932f8b71d3a 113 return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID
Arnoud113 7:88d1ccba9200 114 }
Arnoud113 3:b353ee86230a 115
Arnoud113 18:622e717da184 116 double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2){
Arnoud113 7:88d1ccba9200 117
Arnoud113 8:b932f8b71d3a 118 double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep // Derivative
Arnoud113 7:88d1ccba9200 119 // biquad part, see slide
Arnoud113 7:88d1ccba9200 120 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 8:b932f8b71d3a 121
Arnoud113 13:b5868fd8ffe9 122 e_der2 = bqc.step(e_der2);
Arnoud113 13:b5868fd8ffe9 123
Arnoud113 7:88d1ccba9200 124 e_prev2 = e2;
Arnoud113 12:02eba9a294d2 125 e_int2 += Ts*e2;
Arnoud113 12:02eba9a294d2 126
Arnoud113 17:10c18ca3368b 127 if(button1 == 0 || button2 ==0){
Arnoud113 12:02eba9a294d2 128 e2 = 0;
Arnoud113 12:02eba9a294d2 129 e_prev2 = 0;
Arnoud113 12:02eba9a294d2 130 e_int2 = 0;
Arnoud113 12:02eba9a294d2 131 e_der2 = 0;
Arnoud113 12:02eba9a294d2 132 }
Arnoud113 12:02eba9a294d2 133 // Integral
Arnoud113 8:b932f8b71d3a 134 return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID
Arnoud113 3:b353ee86230a 135 }
Arnoud113 3:b353ee86230a 136
Arnoud113 17:10c18ca3368b 137 // --- Get reference position ---
Arnoud113 0:77ad62c61c78 138 float Get_X_Position(){
Arnoud113 15:65f295a49a4b 139
Arnoud113 17:10c18ca3368b 140 // --- Altering X value with potmeter ---
Arnoud113 17:10c18ca3368b 141 if (potMeter1 < 0.3){
Arnoud113 12:02eba9a294d2 142 X = X-0.5;
Arnoud113 17:10c18ca3368b 143 }
Arnoud113 17:10c18ca3368b 144 else if (potMeter1> 0.7){
Arnoud113 12:02eba9a294d2 145 X = X+0.5;
Arnoud113 17:10c18ca3368b 146 }
Arnoud113 17:10c18ca3368b 147 else{
Arnoud113 12:02eba9a294d2 148 X = X;
Arnoud113 17:10c18ca3368b 149 }
Arnoud113 17:10c18ca3368b 150
Arnoud113 17:10c18ca3368b 151 // --- Getting reference values from emg input ---
Arnoud113 17:10c18ca3368b 152 in1 = emg1.read();
Arnoud113 17:10c18ca3368b 153 in2 = emg2.read();
Arnoud113 17:10c18ca3368b 154 RA = in1+in2;
Arnoud113 15:65f295a49a4b 155
Arnoud113 12:02eba9a294d2 156
Arnoud113 17:10c18ca3368b 157 if (RA < 0.8){
Arnoud113 12:02eba9a294d2 158 X = X;
Arnoud113 17:10c18ca3368b 159 }
Arnoud113 17:10c18ca3368b 160 else if (RA > 1.8){
Arnoud113 15:65f295a49a4b 161 X = X-0.4;
Arnoud113 17:10c18ca3368b 162 }
Arnoud113 17:10c18ca3368b 163 else{
Arnoud113 15:65f295a49a4b 164 X = X+0.4;
Arnoud113 17:10c18ca3368b 165 }
Arnoud113 17:10c18ca3368b 166
Arnoud113 17:10c18ca3368b 167 // --- Setting max and min boundaries for X ---
Arnoud113 13:b5868fd8ffe9 168 if (X >= X_maxTreshold){
Arnoud113 12:02eba9a294d2 169 X = X_maxTreshold;
Arnoud113 12:02eba9a294d2 170 }
Arnoud113 13:b5868fd8ffe9 171 else if (X <= X_minTreshold){
Arnoud113 12:02eba9a294d2 172 X = X_minTreshold;
Arnoud113 17:10c18ca3368b 173 }
Arnoud113 12:02eba9a294d2 174 else{
Arnoud113 12:02eba9a294d2 175 X = X;
Arnoud113 17:10c18ca3368b 176 }
Arnoud113 17:10c18ca3368b 177
Arnoud113 17:10c18ca3368b 178 // --- Setting callibration values for X ---
Arnoud113 17:10c18ca3368b 179 if(button1 == 0){
Arnoud113 12:02eba9a294d2 180 X = X_minTreshold;
Arnoud113 12:02eba9a294d2 181 }
Arnoud113 17:10c18ca3368b 182 else if (button2 == 0){
Arnoud113 12:02eba9a294d2 183 X = X_Callibrate;
Arnoud113 12:02eba9a294d2 184 }
Arnoud113 12:02eba9a294d2 185 else{
Arnoud113 12:02eba9a294d2 186 X = X;
Arnoud113 12:02eba9a294d2 187 }
Arnoud113 12:02eba9a294d2 188 //pc.baud(115200);
Arnoud113 12:02eba9a294d2 189 //pc.printf("\r (in1,in2):(%f,%f), RA = %f, X = %f \n",in1, in2, RA, X);
Arnoud113 1:13d8940f0fd4 190 return X;
Arnoud113 0:77ad62c61c78 191 }
Arnoud113 0:77ad62c61c78 192
Arnoud113 17:10c18ca3368b 193 double Get_Y_Position(){
Arnoud113 17:10c18ca3368b 194 // --- Altering Y value with potmeter ---
Arnoud113 17:10c18ca3368b 195 if (potMeter2 < 0.3){
Arnoud113 16:000f2ebbd16c 196 Y = Y-0.8;
Arnoud113 12:02eba9a294d2 197 }
Arnoud113 17:10c18ca3368b 198 else if (potMeter2 > 0.7){
Arnoud113 12:02eba9a294d2 199 Y = Y+0.5;
Arnoud113 17:10c18ca3368b 200 }
Arnoud113 17:10c18ca3368b 201 else{
Arnoud113 12:02eba9a294d2 202 Y = Y;
Arnoud113 17:10c18ca3368b 203 }
Arnoud113 17:10c18ca3368b 204 // --- Getting reference values from emg input ---
Arnoud113 17:10c18ca3368b 205 in3 = emg3.read();
Arnoud113 17:10c18ca3368b 206 in4 = emg4.read();
Arnoud113 17:10c18ca3368b 207 LA = in3+in4;
Arnoud113 12:02eba9a294d2 208
Arnoud113 17:10c18ca3368b 209 if (LA < 0.8){
Arnoud113 12:02eba9a294d2 210 Y = Y;
Arnoud113 16:000f2ebbd16c 211
Arnoud113 17:10c18ca3368b 212 ledb = 1; // Implement LED feedback to give feedback to the user whether Y is increasing, decreasing, or stationairy.
Arnoud113 17:10c18ca3368b 213 ledr = 1; // The same is done for X on the transmitting embed, this way there is feedback for both X and Y.
Arnoud113 16:000f2ebbd16c 214 ledg = 0; //Groen
Arnoud113 12:02eba9a294d2 215 }
Arnoud113 17:10c18ca3368b 216 else if (LA > 1.8){
Arnoud113 15:65f295a49a4b 217 Y = Y-0.4;
Arnoud113 16:000f2ebbd16c 218
Arnoud113 16:000f2ebbd16c 219 ledr = 1;
Arnoud113 16:000f2ebbd16c 220 ledg = 1; //Blau
Arnoud113 16:000f2ebbd16c 221 ledb = 0;
Arnoud113 17:10c18ca3368b 222 }
Arnoud113 17:10c18ca3368b 223 else{
Arnoud113 15:65f295a49a4b 224 Y = Y+0.4;
Arnoud113 16:000f2ebbd16c 225
Arnoud113 16:000f2ebbd16c 226 ledb = 1; //Rood
Arnoud113 16:000f2ebbd16c 227 ledr = 0;
Arnoud113 16:000f2ebbd16c 228 ledg = 1;
Arnoud113 17:10c18ca3368b 229 }
Arnoud113 17:10c18ca3368b 230 // --- Setting max and min boundaries for Y ---
Arnoud113 13:b5868fd8ffe9 231 if (Y >= Y_maxTreshold){
Arnoud113 12:02eba9a294d2 232 Y = Y_maxTreshold;
Arnoud113 12:02eba9a294d2 233 }
Arnoud113 13:b5868fd8ffe9 234 else if (Y <= Y_minTreshold){
Arnoud113 12:02eba9a294d2 235 Y = Y_minTreshold;
Arnoud113 17:10c18ca3368b 236 }
Arnoud113 12:02eba9a294d2 237 else{
Arnoud113 12:02eba9a294d2 238 Y = Y;
Arnoud113 17:10c18ca3368b 239 }
Arnoud113 17:10c18ca3368b 240 // --- Setting callibration values for Y ---
Arnoud113 17:10c18ca3368b 241 if(button1 == 0){
Arnoud113 12:02eba9a294d2 242 Y = Y_minTreshold;
Arnoud113 12:02eba9a294d2 243 }
Arnoud113 17:10c18ca3368b 244 else if (button2 == 0){
Arnoud113 12:02eba9a294d2 245 Y = Y_Callibrate;
Arnoud113 17:10c18ca3368b 246 }
Arnoud113 12:02eba9a294d2 247 else{
Arnoud113 12:02eba9a294d2 248 Y = Y;
Arnoud113 17:10c18ca3368b 249 }
Arnoud113 17:10c18ca3368b 250 //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, Y);
Arnoud113 1:13d8940f0fd4 251 return Y;
Arnoud113 0:77ad62c61c78 252 }
Arnoud113 3:b353ee86230a 253 //----------------------------------------------------END
Arnoud113 0:77ad62c61c78 254
Arnoud113 3:b353ee86230a 255 //-------------Get current Position-------------------START
Arnoud113 3:b353ee86230a 256 double motor1_Position(){ // has as output Theta
Arnoud113 17:10c18ca3368b 257 // --- Setting callibration values for the encoder ---
Arnoud113 17:10c18ca3368b 258 if (button1 == 0){
Arnoud113 17:10c18ca3368b 259 SetPosM1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1;
Arnoud113 17:10c18ca3368b 260 Encoder1.setPosition(SetPosM1);
Arnoud113 12:02eba9a294d2 261 }
Arnoud113 17:10c18ca3368b 262 else if (button2 ==0){
Arnoud113 17:10c18ca3368b 263 SetPosM1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1;
Arnoud113 17:10c18ca3368b 264 Encoder1.setPosition(SetPosM1);
Arnoud113 12:02eba9a294d2 265 }
Arnoud113 12:02eba9a294d2 266 else{
Arnoud113 12:02eba9a294d2 267 }
Arnoud113 17:10c18ca3368b 268 // --- getting current position ----
Arnoud113 12:02eba9a294d2 269 double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta
Arnoud113 14:54cbd8f0efe4 270 double countM1 = Encoder1.getPosition();
Arnoud113 14:54cbd8f0efe4 271
Arnoud113 14:54cbd8f0efe4 272 pc.baud(115200);
Arnoud113 15:65f295a49a4b 273 //pc.printf("\r counts encoder: %f\n",countM1);
Arnoud113 12:02eba9a294d2 274 return pos_M1;
Arnoud113 0:77ad62c61c78 275 }
Arnoud113 3:b353ee86230a 276 double motor2_Position(){ //output R
Arnoud113 17:10c18ca3368b 277 // --- Setting callibration values for the encoder ---
Arnoud113 17:10c18ca3368b 278 if (button1 == 0){
Arnoud113 17:10c18ca3368b 279 SetPosM2 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2;
Arnoud113 17:10c18ca3368b 280 Encoder2.setPosition(SetPosM2);
Arnoud113 12:02eba9a294d2 281 }
Arnoud113 17:10c18ca3368b 282 else if (button2 ==0){
Arnoud113 17:10c18ca3368b 283 SetPosM2 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2;
Arnoud113 17:10c18ca3368b 284 Encoder2.setPosition(SetPosM2);
Arnoud113 12:02eba9a294d2 285 }
Arnoud113 12:02eba9a294d2 286 else{
Arnoud113 12:02eba9a294d2 287 }
Arnoud113 17:10c18ca3368b 288 // --- getting current position ----
Arnoud113 17:10c18ca3368b 289 double pos_M2 = gainM2*Encoder2.getPosition(); // current position for the radius;
Arnoud113 15:65f295a49a4b 290 double countM2 = Encoder2.getPosition();
Arnoud113 12:02eba9a294d2 291 pc.baud(115200);
Arnoud113 15:65f295a49a4b 292 //pc.printf("\r counts encoder: %f\n",countM2);
Arnoud113 12:02eba9a294d2 293 return pos_M2;
Arnoud113 3:b353ee86230a 294 }
Arnoud113 3:b353ee86230a 295 //-----------------------------------------------------END
Arnoud113 0:77ad62c61c78 296
Arnoud113 0:77ad62c61c78 297
Arnoud113 3:b353ee86230a 298 //------------Controller-------------------------------START
Arnoud113 0:77ad62c61c78 299 void Controller(){
Arnoud113 3:b353ee86230a 300
Arnoud113 1:13d8940f0fd4 301 double x = Get_X_Position();
Arnoud113 1:13d8940f0fd4 302 double y = Get_Y_Position();
Arnoud113 3:b353ee86230a 303
Arnoud113 14:54cbd8f0efe4 304 reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta
Arnoud113 14:54cbd8f0efe4 305 reference_motor2 = sqrt((x*x+y*y)); // reference for radius
Arnoud113 0:77ad62c61c78 306
Arnoud113 14:54cbd8f0efe4 307 pos_M1 = motor1_Position(); // current position for theta
Arnoud113 14:54cbd8f0efe4 308 pos_M2 = motor2_Position(); // current position for the radius
Arnoud113 12:02eba9a294d2 309
Arnoud113 18:622e717da184 310 double deltaM1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err);
Arnoud113 18:622e717da184 311 double deltaM2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err);
Arnoud113 3:b353ee86230a 312
Arnoud113 3:b353ee86230a 313 double dTheta = reference_motor1 - pos_M1;
Arnoud113 3:b353ee86230a 314 double dRadius = reference_motor2 - pos_M2;
Arnoud113 0:77ad62c61c78 315
Arnoud113 0:77ad62c61c78 316 pc.baud(115200);
Arnoud113 17:10c18ca3368b 317 //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (deltaM1,deltaM2):(%f,%f)\n",x,y, dTheta ,dRadius,deltaM1, deltaM2);
Arnoud113 15:65f295a49a4b 318 pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y);
Arnoud113 14:54cbd8f0efe4 319 //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
Arnoud113 0:77ad62c61c78 320
Arnoud113 17:10c18ca3368b 321 // --- Direction control motors --- START
Arnoud113 17:10c18ca3368b 322 if(deltaM1 > 0.5){
Arnoud113 16:000f2ebbd16c 323 motor1DC = 0;
Arnoud113 0:77ad62c61c78 324 }
Arnoud113 17:10c18ca3368b 325 else if (deltaM1< -0.5) {
Arnoud113 3:b353ee86230a 326 motor1DC = 1;
Arnoud113 0:77ad62c61c78 327 }
Arnoud113 0:77ad62c61c78 328 else{
Arnoud113 0:77ad62c61c78 329 motor1PWM = 0;
Arnoud113 0:77ad62c61c78 330 }
Arnoud113 17:10c18ca3368b 331
Arnoud113 17:10c18ca3368b 332 if(deltaM2 > 2.0){
Arnoud113 17:10c18ca3368b 333 motor2DC = 0;
Arnoud113 17:10c18ca3368b 334 }
Arnoud113 17:10c18ca3368b 335 else if (deltaM2<-2.0) {
Arnoud113 17:10c18ca3368b 336 motor2DC = 1;
Arnoud113 17:10c18ca3368b 337 }
Arnoud113 17:10c18ca3368b 338 else{
Arnoud113 17:10c18ca3368b 339 motor2PWM = 0;
Arnoud113 17:10c18ca3368b 340 }
Arnoud113 17:10c18ca3368b 341 // --- Direction control motors --- END
Arnoud113 17:10c18ca3368b 342
Arnoud113 17:10c18ca3368b 343 // --- PWM values ---
Arnoud113 17:10c18ca3368b 344 motor1 = abs(deltaM1)/1000.0;
Arnoud113 15:65f295a49a4b 345 if(motor1 >= 0.10) {
Arnoud113 15:65f295a49a4b 346 motor1 = 0.10;
Arnoud113 9:edf01d06935e 347 //pc.baud(115200);
Arnoud113 9:edf01d06935e 348 //pc.printf("\r val motor1: %f\n", motor1);
Arnoud113 11:66d0be7efd3f 349 }
Arnoud113 0:77ad62c61c78 350
Arnoud113 17:10c18ca3368b 351 motor2 = abs(deltaM2)/1000.0;
Arnoud113 11:66d0be7efd3f 352 if(motor2 >= 0.50) {
Arnoud113 11:66d0be7efd3f 353 motor2 = 0.50;
Arnoud113 11:66d0be7efd3f 354 }
Arnoud113 3:b353ee86230a 355
Arnoud113 15:65f295a49a4b 356 motor1PWM = motor1 + 0.90;
Arnoud113 12:02eba9a294d2 357 motor2PWM = motor2 + 0.50;
Arnoud113 3:b353ee86230a 358
Arnoud113 17:10c18ca3368b 359 //pc.printf("\r delta(1,2):(%f,%f)\n", deltaM1,deltaM2);
Arnoud113 12:02eba9a294d2 360 //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1 + 0.65, motor2 + 0.20);
Arnoud113 3:b353ee86230a 361 //pc.printf("\r
Arnoud113 0:77ad62c61c78 362 }
Arnoud113 0:77ad62c61c78 363
Arnoud113 0:77ad62c61c78 364 int main()
Arnoud113 0:77ad62c61c78 365 {
Arnoud113 0:77ad62c61c78 366 controller.attach(&Controller, M1_TS);
Arnoud113 10:4b0b4f2abacf 367 motor1PWM.period(Pwmperiod);
Arnoud113 10:4b0b4f2abacf 368 motor2PWM.period(Pwmperiod);
Arnoud113 0:77ad62c61c78 369
Arnoud113 16:000f2ebbd16c 370 ledr = 1;
Arnoud113 16:000f2ebbd16c 371 ledb = 1;
Arnoud113 16:000f2ebbd16c 372 ledg = 1;
Arnoud113 16:000f2ebbd16c 373
Arnoud113 13:b5868fd8ffe9 374 bqc.add(&bq1).add(&bq2);
Arnoud113 13:b5868fd8ffe9 375
Arnoud113 17:10c18ca3368b 376 while(1){}
Arnoud113 0:77ad62c61c78 377
Arnoud113 0:77ad62c61c78 378 }