Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@12:02eba9a294d2, 2017-11-02 (annotated)

Committer:
Arnoud113
Date:
Thu Nov 02 11:59:11 2017 +0000
Revision:
12:02eba9a294d2
Parent:
11:66d0be7efd3f
Child:
13:b5868fd8ffe9
semi working version;

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