Vivien van Veldhuizen / Mbed 2 deprecated Signa-bot

Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@36:22d1bcb82061, 2019-11-01 (annotated)

Committer:
viviien
Date:
Fri Nov 01 09:57:21 2019 +0000
Revision:
36:22d1bcb82061
Parent:
35:4cb2ed6dd2d2 
werkt niet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Feike 4:bd21569250c7 1 #include "mbed.h"
Feike 8:017b813c72bb 2 #include "MODSERIAL.h"
Feike 12:2382468d36a4 3 #include "QEI.h"
Feike 18:ab0fe311e7f3 4 #include "Math.h"
Feike 18:ab0fe311e7f3 5 #include "ttmath.h"
viviien 33:88fbf14d8aaf 6 #include "FastPWM.h"
viviien 36:22d1bcb82061 7 #include "BiQuad.h"
Feike 18:ab0fe311e7f3 8
Feike 17:6da57acb7bea 9 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 10 //Serial term (USBTX, USBRX);
viviien 33:88fbf14d8aaf 11 FastPWM motor1_pwm(PTC2);
Feike 17:6da57acb7bea 12 DigitalOut motor1_dir(PTC3);
viviien 33:88fbf14d8aaf 13 FastPWM motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 14 DigitalOut motor2_dir(PTB23);
viviien 33:88fbf14d8aaf 15 FastPWM motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 16 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 17
viviien 30:390cab7cd6e6 18 AnalogIn potmeter1(A1);
viviien 30:390cab7cd6e6 19
viviien 36:22d1bcb82061 20 //Define objects
viviien 36:22d1bcb82061 21 AnalogIn emg0( A0 ); // 1e
viviien 36:22d1bcb82061 22 AnalogIn emg2( A2 ); // 2e
viviien 36:22d1bcb82061 23 AnalogIn emg4( A4 ); // 3e
viviien 36:22d1bcb82061 24
viviien 36:22d1bcb82061 25 float A;
viviien 36:22d1bcb82061 26 float B;
viviien 36:22d1bcb82061 27 Ticker sample_timer;
viviien 36:22d1bcb82061 28 DigitalOut led(LED1);
viviien 36:22d1bcb82061 29
viviien 36:22d1bcb82061 30 const int leng_filt = 60;
viviien 36:22d1bcb82061 31 const int box_length = 50;
viviien 36:22d1bcb82061 32 const int box_lengthC = 150;
viviien 36:22d1bcb82061 33 const int box_checkC = 50;
viviien 36:22d1bcb82061 34 const float grenswaardeA0 = 0.016;
viviien 36:22d1bcb82061 35 const float grenswaardeB0 = 0.012;
viviien 36:22d1bcb82061 36 const float grenswaardeC = 0.012;
viviien 36:22d1bcb82061 37 float Ay1;
viviien 36:22d1bcb82061 38 float Ay2;
viviien 36:22d1bcb82061 39 float A_array[leng_filt] = {0};
viviien 36:22d1bcb82061 40 float A_ar[leng_filt] = {0};
viviien 36:22d1bcb82061 41 float A_ar2[leng_filt] = {0};
viviien 36:22d1bcb82061 42 float A_ar3[box_length] = {0};
viviien 36:22d1bcb82061 43 int boxcheckC = 0;
viviien 36:22d1bcb82061 44 int boxcheckA = 0;
viviien 36:22d1bcb82061 45 int boxcheckB = 0;
viviien 36:22d1bcb82061 46
viviien 36:22d1bcb82061 47 int boxcheckCC = 0;
viviien 36:22d1bcb82061 48 int boxcheckAA = 0;
viviien 36:22d1bcb82061 49 int boxcheckBB = 0;
viviien 36:22d1bcb82061 50
viviien 36:22d1bcb82061 51 float By1;
viviien 36:22d1bcb82061 52 float By2;
viviien 36:22d1bcb82061 53 float B_array[leng_filt] = {0};
viviien 36:22d1bcb82061 54 float B_ar[leng_filt] = {0};
viviien 36:22d1bcb82061 55 float B_ar2[leng_filt] = {0};
viviien 36:22d1bcb82061 56 float B_ar3[box_length] = {0};
viviien 36:22d1bcb82061 57
viviien 36:22d1bcb82061 58 float Cy1;
viviien 36:22d1bcb82061 59 float Cy2;
viviien 36:22d1bcb82061 60 float C_array[leng_filt] = {0};
viviien 36:22d1bcb82061 61 float C_ar[leng_filt] = {0};
viviien 36:22d1bcb82061 62 float C_ar2[leng_filt] = {0};
viviien 36:22d1bcb82061 63 float C_ar3[box_lengthC] = {0};
viviien 36:22d1bcb82061 64
viviien 36:22d1bcb82061 65 float result = 0;
viviien 36:22d1bcb82061 66 float Asum = 0;
viviien 36:22d1bcb82061 67 const int Fs = 2000; //Sample Frequency
viviien 36:22d1bcb82061 68 const double b0 = 0.292893;
viviien 36:22d1bcb82061 69 const double b1 = 0.585786;
viviien 36:22d1bcb82061 70 const double b2 = 0.292893;
viviien 36:22d1bcb82061 71 const double a0 = 1.000000;
viviien 36:22d1bcb82061 72 const double a1 = -0.00000;
viviien 36:22d1bcb82061 73 const double a2 = 0.171573;
viviien 36:22d1bcb82061 74
viviien 36:22d1bcb82061 75 void sample()
viviien 36:22d1bcb82061 76 {
viviien 36:22d1bcb82061 77 BiQuad lowpassA(b0,b1, b2, a0, a1, a2);
viviien 36:22d1bcb82061 78 // Signaal 1 (A)
viviien 36:22d1bcb82061 79 float A = emg0.read();
viviien 36:22d1bcb82061 80 float Amean = 0;
viviien 36:22d1bcb82061 81 float Ay2 = 0;
viviien 36:22d1bcb82061 82
viviien 36:22d1bcb82061 83 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 84 { A_ar[j] = A_ar[j-1]; }
viviien 36:22d1bcb82061 85
viviien 36:22d1bcb82061 86 A_ar[0] = A;
viviien 36:22d1bcb82061 87
viviien 36:22d1bcb82061 88 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 89 { Amean += A_ar[i]*1/leng_filt; }
viviien 36:22d1bcb82061 90
viviien 36:22d1bcb82061 91 Ay1 = A - Amean;
viviien 36:22d1bcb82061 92 Ay1 = fabs(Ay1);
viviien 36:22d1bcb82061 93 Ay1 = lowpassA.step(Ay1); // First signal, after Butterworth
viviien 36:22d1bcb82061 94
viviien 36:22d1bcb82061 95 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 96 { A_ar2[j] = A_ar2[j-1]; }
viviien 36:22d1bcb82061 97
viviien 36:22d1bcb82061 98 A_ar2[0] = Ay1;
viviien 36:22d1bcb82061 99
viviien 36:22d1bcb82061 100 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 101 { Ay2 += A_ar2[i]*1/leng_filt; }
viviien 36:22d1bcb82061 102
viviien 36:22d1bcb82061 103 float Ay3;
viviien 36:22d1bcb82061 104 if(Ay2>grenswaardeA0)
viviien 36:22d1bcb82061 105 { Ay3 = 1; }
viviien 36:22d1bcb82061 106
viviien 36:22d1bcb82061 107 //if(Ay2<=grenswaardeA0)
viviien 36:22d1bcb82061 108 //{ if(Ay2>grenswaardeA1)
viviien 36:22d1bcb82061 109 // { Ay3 = 0.5; } }
viviien 36:22d1bcb82061 110
viviien 36:22d1bcb82061 111 if(Ay2<=grenswaardeA0)
viviien 36:22d1bcb82061 112 { Ay3 = 0; }
viviien 36:22d1bcb82061 113
viviien 36:22d1bcb82061 114 for (int j=box_length-1; j>=1; j--)
viviien 36:22d1bcb82061 115 { A_ar3[j] = A_ar3[j-1]; }
viviien 36:22d1bcb82061 116
viviien 36:22d1bcb82061 117 A_ar3[0] = Ay3;
viviien 36:22d1bcb82061 118 boxcheckA = 0;
viviien 36:22d1bcb82061 119
viviien 36:22d1bcb82061 120 for (int j=0; j<=box_length-1; j++)
viviien 36:22d1bcb82061 121 { if(A_ar3[j] == 1)
viviien 36:22d1bcb82061 122 { boxcheckA = 1;
viviien 36:22d1bcb82061 123 boxcheckAA = 1;} }
viviien 36:22d1bcb82061 124
viviien 36:22d1bcb82061 125 // Signaal 2 (B)
viviien 36:22d1bcb82061 126 BiQuad lowpassB(b0,b1, b2, a0, a1, a2);
viviien 36:22d1bcb82061 127 float B = emg2.read();
viviien 36:22d1bcb82061 128 float Bmean = 0;
viviien 36:22d1bcb82061 129 float By2 = 0;
viviien 36:22d1bcb82061 130
viviien 36:22d1bcb82061 131 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 132 { B_ar[j] = B_ar[j-1]; }
viviien 36:22d1bcb82061 133
viviien 36:22d1bcb82061 134 B_ar[0] = B;
viviien 36:22d1bcb82061 135
viviien 36:22d1bcb82061 136 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 137 { Bmean += B_ar[i]*1/leng_filt; }
viviien 36:22d1bcb82061 138
viviien 36:22d1bcb82061 139 By1 = B - Bmean;
viviien 36:22d1bcb82061 140 By1 = fabs(By1);
viviien 36:22d1bcb82061 141 By1 = lowpassB.step(By1); // First signal, after Butterworth
viviien 36:22d1bcb82061 142
viviien 36:22d1bcb82061 143 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 144 { B_ar2[j] = B_ar2[j-1]; }
viviien 36:22d1bcb82061 145
viviien 36:22d1bcb82061 146 B_ar2[0] = By1;
viviien 36:22d1bcb82061 147
viviien 36:22d1bcb82061 148 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 149 { By2 += B_ar2[i]*1/leng_filt; }
viviien 36:22d1bcb82061 150
viviien 36:22d1bcb82061 151 float By3;
viviien 36:22d1bcb82061 152 if(By2>grenswaardeB0)
viviien 36:22d1bcb82061 153 { By3 = 1; }
viviien 36:22d1bcb82061 154
viviien 36:22d1bcb82061 155 if(By2<=grenswaardeB0)
viviien 36:22d1bcb82061 156 { By3 = 0; }
viviien 36:22d1bcb82061 157
viviien 36:22d1bcb82061 158 for (int j=box_length-1; j>=1; j--)
viviien 36:22d1bcb82061 159 { B_ar3[j] = B_ar3[j-1]; }
viviien 36:22d1bcb82061 160
viviien 36:22d1bcb82061 161 B_ar3[0] = By3;
viviien 36:22d1bcb82061 162 boxcheckB = 0;
viviien 36:22d1bcb82061 163
viviien 36:22d1bcb82061 164 for (int j=0; j<=box_length-1; j++)
viviien 36:22d1bcb82061 165 { if(B_ar3[j] == 1)
viviien 36:22d1bcb82061 166 { boxcheckB = 1;
viviien 36:22d1bcb82061 167 boxcheckBB = 1;} }
viviien 36:22d1bcb82061 168
viviien 36:22d1bcb82061 169 // Signaal 3 (C)
viviien 36:22d1bcb82061 170 BiQuad lowpassC(b0,b1, b2, a0, a1, a2);
viviien 36:22d1bcb82061 171 float C = emg4.read();
viviien 36:22d1bcb82061 172 float Cmean = 0;
viviien 36:22d1bcb82061 173 float Cy2 = 0;
viviien 36:22d1bcb82061 174
viviien 36:22d1bcb82061 175 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 176 { C_ar[j] = C_ar[j-1]; }
viviien 36:22d1bcb82061 177
viviien 36:22d1bcb82061 178 C_ar[0] = C;
viviien 36:22d1bcb82061 179
viviien 36:22d1bcb82061 180 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 181 { Cmean += C_ar[i]*1/leng_filt; }
viviien 36:22d1bcb82061 182
viviien 36:22d1bcb82061 183 Cy1 = C - Cmean;
viviien 36:22d1bcb82061 184 Cy1 = fabs(Cy1);
viviien 36:22d1bcb82061 185 Cy1 = lowpassC.step(Cy1); // First signal, after Butterworth
viviien 36:22d1bcb82061 186
viviien 36:22d1bcb82061 187 for (int j=leng_filt-1; j>=1; j--)
viviien 36:22d1bcb82061 188 { C_ar2[j] = C_ar2[j-1]; }
viviien 36:22d1bcb82061 189
viviien 36:22d1bcb82061 190 C_ar2[0] = Cy1;
viviien 36:22d1bcb82061 191
viviien 36:22d1bcb82061 192 for(int i=0; i<=leng_filt-1; i++)
viviien 36:22d1bcb82061 193 { Cy2 += C_ar2[i]*1/leng_filt; }
viviien 36:22d1bcb82061 194
viviien 36:22d1bcb82061 195 float Cy3;
viviien 36:22d1bcb82061 196 if(Cy2>grenswaardeC)
viviien 36:22d1bcb82061 197 { Cy3 = 1; }
viviien 36:22d1bcb82061 198
viviien 36:22d1bcb82061 199 if(Cy2<=grenswaardeC)
viviien 36:22d1bcb82061 200 { Cy3 = 0; }
viviien 36:22d1bcb82061 201
viviien 36:22d1bcb82061 202 for (int j=box_lengthC-1; j>=1; j--)
viviien 36:22d1bcb82061 203 { C_ar3[j] = C_ar3[j-1]; }
viviien 36:22d1bcb82061 204
viviien 36:22d1bcb82061 205 C_ar3[0] = Cy3;
viviien 36:22d1bcb82061 206 boxcheckC = 0;
viviien 36:22d1bcb82061 207 int C_sum = 0;
viviien 36:22d1bcb82061 208 for (int j=0; j<=box_length-1; j++)
viviien 36:22d1bcb82061 209 { C_sum += C_ar3[j];
viviien 36:22d1bcb82061 210 if(C_sum >= box_checkC)
viviien 36:22d1bcb82061 211 { boxcheckC = 1;
viviien 36:22d1bcb82061 212 boxcheckCC=1;}
viviien 36:22d1bcb82061 213 }
viviien 36:22d1bcb82061 214
viviien 36:22d1bcb82061 215 led = !led;
viviien 36:22d1bcb82061 216 }
viviien 30:390cab7cd6e6 217
viviien 30:390cab7cd6e6 218
viviien 23:18b0be02187f 219 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 220 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 221 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 222
viviien 35:4cb2ed6dd2d2 223
Feike 13:18dd7a15603f 224 int quit;
Feike 19:9c8ab7922191 225 int limit_pos = 8400;
Feike 19:9c8ab7922191 226 float steps;
Feike 19:9c8ab7922191 227 int g = 0;
Feike 19:9c8ab7922191 228
Feike 18:ab0fe311e7f3 229 int counts1 = 0;
Feike 18:ab0fe311e7f3 230 int counts2 = 0;
Feike 18:ab0fe311e7f3 231 int counts3 = 0;
viviien 27:3eb181cbe183 232
viviien 35:4cb2ed6dd2d2 233 float savedX = 0;
viviien 35:4cb2ed6dd2d2 234 float savedY = 0;
viviien 35:4cb2ed6dd2d2 235 float savedZ = 0;
viviien 35:4cb2ed6dd2d2 236 int sign = 0;
viviien 35:4cb2ed6dd2d2 237
Feike 13:18dd7a15603f 238
Feike 18:ab0fe311e7f3 239 const float le = 15.0;
Feike 18:ab0fe311e7f3 240 const float f = 37.5;
Feike 18:ab0fe311e7f3 241 const float re = 174.0;
Feike 18:ab0fe311e7f3 242 const float rf = 50.0;
Feike 18:ab0fe311e7f3 243 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 244 const float cospi = -0.5;
viviien 22:9f911405e096 245 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 246 float y2;
Feike 18:ab0fe311e7f3 247 float y1;
Feike 18:ab0fe311e7f3 248 float z1;
Feike 18:ab0fe311e7f3 249 float z2;
Feike 18:ab0fe311e7f3 250 float rje2;
Feike 18:ab0fe311e7f3 251 float rje;
Feike 18:ab0fe311e7f3 252 float r2;
Feike 18:ab0fe311e7f3 253 float r;
Feike 19:9c8ab7922191 254
viviien 35:4cb2ed6dd2d2 255 float z0=-158;
Feike 18:ab0fe311e7f3 256 float y0=0;
Feike 18:ab0fe311e7f3 257 float x0=0;
Feike 19:9c8ab7922191 258
Feike 18:ab0fe311e7f3 259 float theta1;
Feike 18:ab0fe311e7f3 260 float theta2;
Feike 18:ab0fe311e7f3 261 float theta3;
viviien 35:4cb2ed6dd2d2 262
Feike 19:9c8ab7922191 263
viviien 28:43a1d67ff8ea 264 // Constant values for PI
viviien 30:390cab7cd6e6 265 float Kp;
viviien 30:390cab7cd6e6 266
viviien 30:390cab7cd6e6 267 float Ts = 0.0025;
viviien 28:43a1d67ff8ea 268 float error1 = 0;
viviien 30:390cab7cd6e6 269 float error2 = 0;
viviien 30:390cab7cd6e6 270 float error3 = 0;
viviien 28:43a1d67ff8ea 271
viviien 30:390cab7cd6e6 272 float newmotor1 = 1;
viviien 30:390cab7cd6e6 273 float newmotor2 = 1;
viviien 30:390cab7cd6e6 274 float newmotor3 = 1;
viviien 30:390cab7cd6e6 275
viviien 30:390cab7cd6e6 276 float u_k1 = 0;
viviien 30:390cab7cd6e6 277 float u_k2 = 0;
viviien 30:390cab7cd6e6 278 float u_k3 = 0;
viviien 30:390cab7cd6e6 279
viviien 32:60a71dcfdb7a 280 float angle1 = 0;
viviien 32:60a71dcfdb7a 281 float angle2 = 0;
viviien 32:60a71dcfdb7a 282 float angle3 = 0;
viviien 32:60a71dcfdb7a 283
viviien 32:60a71dcfdb7a 284 float time_sin = 0;
viviien 32:60a71dcfdb7a 285
viviien 32:60a71dcfdb7a 286
viviien 32:60a71dcfdb7a 287 void delta_calctheta1 () {
viviien 32:60a71dcfdb7a 288 float y2 = y0 + le;
viviien 32:60a71dcfdb7a 289 float y1 = f;
viviien 32:60a71dcfdb7a 290 float z1 = 0.0;
viviien 32:60a71dcfdb7a 291 float z2 = z0;
viviien 32:60a71dcfdb7a 292 float rje2 = re*re - x0*x0;
viviien 32:60a71dcfdb7a 293 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 294 float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
viviien 32:60a71dcfdb7a 295 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 296
viviien 32:60a71dcfdb7a 297 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 298 int check = 1;
viviien 32:60a71dcfdb7a 299 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 300 }
viviien 32:60a71dcfdb7a 301 else {
viviien 32:60a71dcfdb7a 302 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 303 float beta = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 304 if(beta<=0) {
viviien 32:60a71dcfdb7a 305 beta = beta + pi;
viviien 35:4cb2ed6dd2d2 306 if (beta>=alpha) {
viviien 35:4cb2ed6dd2d2 307 theta1 = beta-alpha;
viviien 35:4cb2ed6dd2d2 308 }
viviien 35:4cb2ed6dd2d2 309 else {
viviien 35:4cb2ed6dd2d2 310 theta1 = -alpha+beta;
viviien 35:4cb2ed6dd2d2 311 }
viviien 35:4cb2ed6dd2d2 312
viviien 32:60a71dcfdb7a 313 }
viviien 35:4cb2ed6dd2d2 314 if(beta>0) {
viviien 35:4cb2ed6dd2d2 315 if (beta<=alpha) {
viviien 35:4cb2ed6dd2d2 316 theta1 = -alpha+beta;
viviien 35:4cb2ed6dd2d2 317 }
viviien 35:4cb2ed6dd2d2 318 else {
viviien 35:4cb2ed6dd2d2 319 theta1 = beta-alpha;
viviien 35:4cb2ed6dd2d2 320 }
viviien 35:4cb2ed6dd2d2 321 }
viviien 32:60a71dcfdb7a 322 }
viviien 32:60a71dcfdb7a 323 }
viviien 32:60a71dcfdb7a 324
viviien 32:60a71dcfdb7a 325 void delta_calctheta2 () {
viviien 32:60a71dcfdb7a 326 float xref = x0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 327 float yref = y0*cospi-x0*sinpi;
viviien 32:60a71dcfdb7a 328 float zref = z0;
viviien 32:60a71dcfdb7a 329 float y2 = yref + le;
viviien 32:60a71dcfdb7a 330 float y1 = f;
viviien 32:60a71dcfdb7a 331 float z1 = 0.0;
viviien 32:60a71dcfdb7a 332 float z2 = zref;
viviien 32:60a71dcfdb7a 333 float rje2 = re*re - xref*xref;
viviien 32:60a71dcfdb7a 334 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 335 float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
viviien 32:60a71dcfdb7a 336 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 337
viviien 32:60a71dcfdb7a 338 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 339 int check = 1;
viviien 32:60a71dcfdb7a 340 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 341 }
viviien 35:4cb2ed6dd2d2 342 else {
viviien 35:4cb2ed6dd2d2 343 float alpha2 = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 344 float beta2 = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 345 if(beta2<=0) {
viviien 35:4cb2ed6dd2d2 346 beta2 = beta2 + pi;
viviien 35:4cb2ed6dd2d2 347 if (beta2>=alpha2) {
viviien 35:4cb2ed6dd2d2 348 theta2 = beta2-alpha2;
viviien 35:4cb2ed6dd2d2 349 }
viviien 35:4cb2ed6dd2d2 350 else {
viviien 35:4cb2ed6dd2d2 351 theta2 = -alpha2+beta2;
viviien 35:4cb2ed6dd2d2 352 }
viviien 35:4cb2ed6dd2d2 353
viviien 32:60a71dcfdb7a 354 }
viviien 35:4cb2ed6dd2d2 355 if(beta2>0) {
viviien 35:4cb2ed6dd2d2 356 if (beta2<=alpha2) {
viviien 35:4cb2ed6dd2d2 357 theta2 = -alpha2+beta2;
viviien 35:4cb2ed6dd2d2 358 }
viviien 35:4cb2ed6dd2d2 359 else {
viviien 35:4cb2ed6dd2d2 360 theta2 = beta2-alpha2;
viviien 35:4cb2ed6dd2d2 361 }
viviien 35:4cb2ed6dd2d2 362 }
viviien 32:60a71dcfdb7a 363 }
viviien 32:60a71dcfdb7a 364 }
viviien 32:60a71dcfdb7a 365
viviien 32:60a71dcfdb7a 366 void delta_calctheta3 () {
viviien 32:60a71dcfdb7a 367 float xreff = x0*cospi-y0*sinpi;
viviien 35:4cb2ed6dd2d2 368 float yreff = y0*cospi+x0*sinpi;
viviien 32:60a71dcfdb7a 369 float zreff = z0;
viviien 32:60a71dcfdb7a 370 float y2 = yreff + le;
viviien 32:60a71dcfdb7a 371 float y1 = f;
viviien 32:60a71dcfdb7a 372 float z1 = 0.0;
viviien 32:60a71dcfdb7a 373 float z2 = zreff;
viviien 32:60a71dcfdb7a 374 float rje2 = re*re - xreff*xreff;
viviien 32:60a71dcfdb7a 375 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 376 float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
viviien 32:60a71dcfdb7a 377 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 378
viviien 32:60a71dcfdb7a 379 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 380 int check = 1;
viviien 32:60a71dcfdb7a 381 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 382 }
viviien 35:4cb2ed6dd2d2 383 else {
viviien 35:4cb2ed6dd2d2 384 float alpha3 = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 35:4cb2ed6dd2d2 385 float beta3 = atan((z1+z2)/(y1-y2));
viviien 35:4cb2ed6dd2d2 386 if(beta3<=0) {
viviien 35:4cb2ed6dd2d2 387 beta3 = beta3 + pi;
viviien 35:4cb2ed6dd2d2 388 if (beta3>=alpha3) {
viviien 35:4cb2ed6dd2d2 389 theta3 = beta3-alpha3;
viviien 35:4cb2ed6dd2d2 390 }
viviien 35:4cb2ed6dd2d2 391 else {
viviien 35:4cb2ed6dd2d2 392 theta3 = -alpha3+beta3;
viviien 35:4cb2ed6dd2d2 393 }
viviien 35:4cb2ed6dd2d2 394
viviien 32:60a71dcfdb7a 395 }
viviien 35:4cb2ed6dd2d2 396 if(beta3>0) {
viviien 35:4cb2ed6dd2d2 397 if (beta3<=alpha3) {
viviien 35:4cb2ed6dd2d2 398 theta3 = -alpha3+beta3;
viviien 35:4cb2ed6dd2d2 399 }
viviien 35:4cb2ed6dd2d2 400 else {
viviien 35:4cb2ed6dd2d2 401 theta3 = beta3-alpha3;
viviien 35:4cb2ed6dd2d2 402 }
viviien 35:4cb2ed6dd2d2 403 }
viviien 32:60a71dcfdb7a 404 }
viviien 32:60a71dcfdb7a 405 }
viviien 32:60a71dcfdb7a 406
viviien 32:60a71dcfdb7a 407
viviien 32:60a71dcfdb7a 408
viviien 30:390cab7cd6e6 409
viviien 30:390cab7cd6e6 410 Ticker motor_timer;
viviien 32:60a71dcfdb7a 411 void motor(){
viviien 32:60a71dcfdb7a 412
viviien 30:390cab7cd6e6 413 counts1 = Encoder1.getPulses();
viviien 30:390cab7cd6e6 414 counts2 = Encoder2.getPulses();
viviien 30:390cab7cd6e6 415 counts3 = Encoder3.getPulses();
viviien 32:60a71dcfdb7a 416
viviien 35:4cb2ed6dd2d2 417 //z0 = 158 - 60* sin(3*time_sin);
viviien 33:88fbf14d8aaf 418
viviien 36:22d1bcb82061 419 // float r = 20*(1-1/(1+(time_sin)));
viviien 36:22d1bcb82061 420 // x0 = r*sin(3*time_sin);
viviien 36:22d1bcb82061 421 // y0 = r*cos(3*time_sin);
viviien 36:22d1bcb82061 422
viviien 33:88fbf14d8aaf 423
viviien 29:7eb028b359a1 424
viviien 32:60a71dcfdb7a 425 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 426 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 427 delta_calctheta3 ();
viviien 30:390cab7cd6e6 428
viviien 32:60a71dcfdb7a 429 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 430 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 431 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 432
viviien 32:60a71dcfdb7a 433 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 434 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 435 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 436
viviien 35:4cb2ed6dd2d2 437 Kp = potmeter1 * 10;
viviien 30:390cab7cd6e6 438
viviien 35:4cb2ed6dd2d2 439 u_k1 = 10 * error1;
viviien 35:4cb2ed6dd2d2 440 u_k2 = 10 * error2;
viviien 35:4cb2ed6dd2d2 441 u_k3 = 10 * error3;
viviien 30:390cab7cd6e6 442
viviien 32:60a71dcfdb7a 443 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 444 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 445 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 446
viviien 30:390cab7cd6e6 447 if (newmotor1>1.0f){
viviien 35:4cb2ed6dd2d2 448 newmotor1 = 1.0f;
viviien 23:18b0be02187f 449 }
viviien 30:390cab7cd6e6 450 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 451 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 452 }
viviien 30:390cab7cd6e6 453
viviien 30:390cab7cd6e6 454 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 455 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 456 }
viviien 30:390cab7cd6e6 457 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 458 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 459 }
viviien 30:390cab7cd6e6 460
viviien 30:390cab7cd6e6 461 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 462 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 463 }
viviien 30:390cab7cd6e6 464 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 465 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 466 }
viviien 30:390cab7cd6e6 467
viviien 35:4cb2ed6dd2d2 468
viviien 30:390cab7cd6e6 469 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 470 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 471
viviien 30:390cab7cd6e6 472 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 473 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 474
viviien 30:390cab7cd6e6 475 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 476 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 477
viviien 32:60a71dcfdb7a 478 time_sin += 0.002;
viviien 30:390cab7cd6e6 479 }
viviien 25:eb3204e45d33 480
viviien 32:60a71dcfdb7a 481
viviien 28:43a1d67ff8ea 482
viviien 28:43a1d67ff8ea 483 ///////////////////
viviien 28:43a1d67ff8ea 484 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 485 ///////////////////
viviien 28:43a1d67ff8ea 486
viviien 35:4cb2ed6dd2d2 487 int main(void) {
viviien 35:4cb2ed6dd2d2 488
viviien 35:4cb2ed6dd2d2 489 delta_calctheta1 ();
viviien 35:4cb2ed6dd2d2 490 delta_calctheta2 ();
viviien 35:4cb2ed6dd2d2 491 delta_calctheta3 ();
viviien 35:4cb2ed6dd2d2 492
viviien 36:22d1bcb82061 493 motor_timer.attach(&motor, 0.002);
viviien 36:22d1bcb82061 494 // sample_timer.attach(&sample, 0.005);
viviien 26:432d3519ba86 495
viviien 36:22d1bcb82061 496 char cc = pc.getc();
viviien 35:4cb2ed6dd2d2 497
viviien 35:4cb2ed6dd2d2 498
viviien 30:390cab7cd6e6 499 int frequency_pwm = 10000; //10 kHz PWM
viviien 33:88fbf14d8aaf 500 motor1_pwm.period_us(6);
viviien 33:88fbf14d8aaf 501 motor2_pwm.period_us(6);
viviien 33:88fbf14d8aaf 502 motor3_pwm.period_us(6);
viviien 35:4cb2ed6dd2d2 503
viviien 35:4cb2ed6dd2d2 504
viviien 35:4cb2ed6dd2d2 505 pc.baud(115200);
Feike 19:9c8ab7922191 506
viviien 28:43a1d67ff8ea 507 while(true){
viviien 36:22d1bcb82061 508 //
viviien 36:22d1bcb82061 509 //
viviien 36:22d1bcb82061 510 // while (boxcheckCC == 0) {
viviien 36:22d1bcb82061 511 // if (boxcheckAA == 1 && z0>=-179 && z0<=-158){
viviien 36:22d1bcb82061 512 // z0=z0+1.0f;
viviien 36:22d1bcb82061 513 // wait(1.0/2000);
viviien 36:22d1bcb82061 514 // boxcheckAA = 0;
viviien 36:22d1bcb82061 515 // }
viviien 36:22d1bcb82061 516 // if (boxcheckBB == 1 && z0>=-178 && z0<=-157){
viviien 36:22d1bcb82061 517 // z0=z0-1.0f;
viviien 36:22d1bcb82061 518 // wait(1.0/2000);
viviien 36:22d1bcb82061 519 // boxcheckBB = 0;
viviien 36:22d1bcb82061 520 // }
viviien 36:22d1bcb82061 521 // }
viviien 36:22d1bcb82061 522 //
viviien 36:22d1bcb82061 523 // wait(1.5);
viviien 36:22d1bcb82061 524 // boxcheckCC = 0;
viviien 36:22d1bcb82061 525 // savedZ = z0;
viviien 36:22d1bcb82061 526 //
viviien 36:22d1bcb82061 527 // sign = 1;
viviien 36:22d1bcb82061 528 //
viviien 36:22d1bcb82061 529 // wait(5.0);
viviien 36:22d1bcb82061 530 //
viviien 36:22d1bcb82061 531 // sign = 0;
viviien 36:22d1bcb82061 532 // boxcheckCC = 0;
viviien 36:22d1bcb82061 533 //
viviien 36:22d1bcb82061 534 //
viviien 36:22d1bcb82061 535 // float diffZ = -158 - z0;
viviien 36:22d1bcb82061 536 //
viviien 36:22d1bcb82061 537 //
viviien 36:22d1bcb82061 538 // for (int i=0; i<=diffZ; i++) {
viviien 36:22d1bcb82061 539 // if (diffZ>0) {
viviien 36:22d1bcb82061 540 // z0 = z0+1;
viviien 36:22d1bcb82061 541 // }
viviien 36:22d1bcb82061 542 // if (diffZ<0) {
viviien 36:22d1bcb82061 543 // z0 = z0-1;
viviien 36:22d1bcb82061 544 // }
viviien 36:22d1bcb82061 545 // wait(1.0/20);
viviien 36:22d1bcb82061 546 // }
viviien 36:22d1bcb82061 547 //
viviien 36:22d1bcb82061 548 //break;
viviien 35:4cb2ed6dd2d2 549
viviien 35:4cb2ed6dd2d2 550
viviien 35:4cb2ed6dd2d2 551
viviien 35:4cb2ed6dd2d2 552 // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 35:4cb2ed6dd2d2 553 // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 35:4cb2ed6dd2d2 554 // wait(0.1);
viviien 35:4cb2ed6dd2d2 555
viviien 35:4cb2ed6dd2d2 556
viviien 36:22d1bcb82061 557 char cc = pc.getc();
viviien 36:22d1bcb82061 558 if (cc=='d' && x0>=-76 && x0<=75) {
viviien 36:22d1bcb82061 559 x0=x0+1.0f ;
viviien 36:22d1bcb82061 560 savedX=x0;
viviien 36:22d1bcb82061 561 }
viviien 36:22d1bcb82061 562
viviien 36:22d1bcb82061 563 if (cc=='a' && x0>=-75 && x0<=76) {
viviien 36:22d1bcb82061 564
viviien 36:22d1bcb82061 565 x0=x0-1.0f;
viviien 36:22d1bcb82061 566 savedX=x0;
viviien 36:22d1bcb82061 567 }
viviien 36:22d1bcb82061 568
viviien 36:22d1bcb82061 569 if (cc=='w' && y0>=-76 && y0<=75){
viviien 36:22d1bcb82061 570 y0=y0+1.0f;
viviien 36:22d1bcb82061 571 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 36:22d1bcb82061 572 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 36:22d1bcb82061 573 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 36:22d1bcb82061 574 savedY=y0;
viviien 36:22d1bcb82061 575
viviien 36:22d1bcb82061 576 }
viviien 36:22d1bcb82061 577
viviien 36:22d1bcb82061 578 if (cc=='s' && y0>=-75 && y0<=76){
viviien 36:22d1bcb82061 579 y0=y0-1.0f;
viviien 36:22d1bcb82061 580 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 36:22d1bcb82061 581 pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 36:22d1bcb82061 582 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 36:22d1bcb82061 583 savedY=y0;
viviien 36:22d1bcb82061 584 }
viviien 36:22d1bcb82061 585
viviien 36:22d1bcb82061 586 if (cc=='u' && z0>=-211 && z0<=-130){
viviien 36:22d1bcb82061 587 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 36:22d1bcb82061 588 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 36:22d1bcb82061 589 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 36:22d1bcb82061 590 z0=z0+1.0f;
viviien 36:22d1bcb82061 591 savedZ=z0;
viviien 36:22d1bcb82061 592 }
viviien 36:22d1bcb82061 593
viviien 36:22d1bcb82061 594 if (cc=='j' && z0>=-210 && z0<=-129){
viviien 36:22d1bcb82061 595 pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
viviien 36:22d1bcb82061 596 pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
viviien 36:22d1bcb82061 597 pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
viviien 36:22d1bcb82061 598 z0=z0-1.0f;
viviien 36:22d1bcb82061 599 savedZ=z0;
viviien 36:22d1bcb82061 600 }
viviien 36:22d1bcb82061 601
viviien 22:9f911405e096 602 }
viviien 32:60a71dcfdb7a 603 //END MAIN
viviien 22:9f911405e096 604 }