Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@16:000f2ebbd16c, 2017-11-06 (annotated)

Committer:
Arnoud113
Date:
Mon Nov 06 22:50:16 2017 +0000
Revision:
16:000f2ebbd16c
Parent:
15:65f295a49a4b
Child:
17:10c18ca3368b
Working version before making it neat and clean to hand in for the report

Who changed what in which revision?

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