Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@17:10c18ca3368b, 2017-11-06 (annotated)

Committer:
Arnoud113
Date:
Mon Nov 06 23:36:22 2017 +0000
Revision:
17:10c18ca3368b
Parent:
16:000f2ebbd16c
Child:
18:622e717da184
neatified version for report, however do need to look at the biquad mess. Are still some non used values there which could be removed.

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