Feike van Veen / Mbed 2 deprecated Minor_test_serial

Robot tryout

Dependencies:   mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@25:eb3204e45d33, 2019-10-29 (annotated)

Committer:
viviien
Date:
Tue Oct 29 08:48:48 2019 +0000
Revision:
25:eb3204e45d33
Parent:
24:61d03692f92e
Child:
26:432d3519ba86
Versie van maandag

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"
Feike 18:ab0fe311e7f3 6
Feike 17:6da57acb7bea 7 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 8 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 9 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 10 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 11 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 12 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 13 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 14 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 15
viviien 23:18b0be02187f 16 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 17 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 18 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 19
viviien 25:eb3204e45d33 20 float steps1 = 0;
viviien 25:eb3204e45d33 21 float steps2 = 0;
viviien 25:eb3204e45d33 22 float steps3 = 0;
Feike 19:9c8ab7922191 23 int check;
Feike 13:18dd7a15603f 24 int quit;
Feike 19:9c8ab7922191 25 int limit_pos = 8400;
Feike 19:9c8ab7922191 26 float steps;
Feike 19:9c8ab7922191 27 int g = 0;
Feike 19:9c8ab7922191 28
Feike 19:9c8ab7922191 29 bool check1;
Feike 19:9c8ab7922191 30 bool check2;
Feike 19:9c8ab7922191 31 bool check3;
Feike 18:ab0fe311e7f3 32 int counts1 = 0;
Feike 18:ab0fe311e7f3 33 int counts2 = 0;
Feike 18:ab0fe311e7f3 34 int counts3 = 0;
viviien 25:eb3204e45d33 35 int count1old = 0;
viviien 25:eb3204e45d33 36 int countdiff = 0;
viviien 25:eb3204e45d33 37 int counting1 = 0;
Feike 13:18dd7a15603f 38
Feike 18:ab0fe311e7f3 39 const float le = 15.0;
Feike 18:ab0fe311e7f3 40 const float f = 37.5;
Feike 18:ab0fe311e7f3 41 const float re = 174.0;
Feike 18:ab0fe311e7f3 42 const float rf = 50.0;
Feike 18:ab0fe311e7f3 43 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 44 const float cospi = -0.5;
viviien 22:9f911405e096 45 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 46 float y2;
Feike 18:ab0fe311e7f3 47 float y1;
Feike 18:ab0fe311e7f3 48 float z1;
Feike 18:ab0fe311e7f3 49 float z2;
Feike 18:ab0fe311e7f3 50 float rje2;
Feike 18:ab0fe311e7f3 51 float rje;
Feike 18:ab0fe311e7f3 52 float r2;
Feike 18:ab0fe311e7f3 53 float r;
Feike 19:9c8ab7922191 54
Feike 18:ab0fe311e7f3 55 float z0=-172;
Feike 18:ab0fe311e7f3 56 float y0=0;
Feike 18:ab0fe311e7f3 57 float x0=0;
viviien 21:c826abca79c3 58 float x00;
viviien 21:c826abca79c3 59 float y00;
viviien 21:c826abca79c3 60 float z00;
Feike 19:9c8ab7922191 61
Feike 18:ab0fe311e7f3 62 float theta1;
Feike 18:ab0fe311e7f3 63 float theta2;
Feike 18:ab0fe311e7f3 64 float theta3;
Feike 18:ab0fe311e7f3 65 float oldtheta1=0;
Feike 18:ab0fe311e7f3 66 float oldtheta2=0;
Feike 18:ab0fe311e7f3 67 float oldtheta3=0;
Feike 19:9c8ab7922191 68
viviien 25:eb3204e45d33 69 Ticker pulses1;
viviien 25:eb3204e45d33 70 void getpulses1() {
viviien 23:18b0be02187f 71 counts1 = Encoder1.getPulses();
viviien 25:eb3204e45d33 72 }
viviien 25:eb3204e45d33 73 Ticker pulses2;
viviien 25:eb3204e45d33 74 void getpulses2() {
viviien 23:18b0be02187f 75 counts2 = Encoder2.getPulses();
viviien 25:eb3204e45d33 76 }
viviien 25:eb3204e45d33 77 Ticker pulses3;
viviien 25:eb3204e45d33 78 void getpulses3() {
viviien 23:18b0be02187f 79 counts3 = Encoder3.getPulses();
viviien 23:18b0be02187f 80 }
viviien 25:eb3204e45d33 81
viviien 23:18b0be02187f 82
viviien 21:c826abca79c3 83 float delta_calcangleyz(float x00, float y00, float z00) {
viviien 21:c826abca79c3 84 float y2 = y00 + le;
Feike 18:ab0fe311e7f3 85 float y1 = f;
Feike 18:ab0fe311e7f3 86 float z1 = 0.0;
viviien 21:c826abca79c3 87 float z2 = z00;
viviien 21:c826abca79c3 88 float rje2 = re*re - x00*x00;
Feike 18:ab0fe311e7f3 89 float rje = sqrt(rje2);
viviien 22:9f911405e096 90 float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
Feike 18:ab0fe311e7f3 91 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 92
Feike 19:9c8ab7922191 93 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
Feike 19:9c8ab7922191 94 int check = 1;
Feike 19:9c8ab7922191 95 pc.printf("\n\rPunt bestaat niet");
Feike 19:9c8ab7922191 96 }
Feike 19:9c8ab7922191 97 else {
Feike 19:9c8ab7922191 98 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 19:9c8ab7922191 99 float beta = atan((z1-z2)/(y1-y2));
Feike 19:9c8ab7922191 100 if(beta<0) {
Feike 18:ab0fe311e7f3 101 beta = beta + pi;
Feike 18:ab0fe311e7f3 102 }
Feike 19:9c8ab7922191 103 float theta1 = (beta - alpha)*180.0/pi;
Feike 19:9c8ab7922191 104 return theta1;
viviien 22:9f911405e096 105 }
Feike 18:ab0fe311e7f3 106 }
Feike 18:ab0fe311e7f3 107
viviien 21:c826abca79c3 108 float delta_calcinverse(float x00, float y00, float z00) {
viviien 21:c826abca79c3 109 theta1 = theta2 = theta3 = 0;
viviien 22:9f911405e096 110 x00=x0;
viviien 22:9f911405e096 111 y00=y0;
viviien 22:9f911405e096 112 z00=z0;
viviien 21:c826abca79c3 113 theta1 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 114
viviien 21:c826abca79c3 115 if (check == 0) {
viviien 21:c826abca79c3 116 x00=x0*cospi+y0*sinpi;
viviien 21:c826abca79c3 117 y00=y0*cospi-x0*sinpi;
viviien 21:c826abca79c3 118 z00=z0;
viviien 22:9f911405e096 119 theta2 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 120 x00=x0*cospi-y0*sinpi;
viviien 21:c826abca79c3 121 y00=y0*cospi+y0*sinpi;
viviien 21:c826abca79c3 122 z00=z0;
viviien 22:9f911405e096 123 theta3 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 124 }
viviien 21:c826abca79c3 125
viviien 21:c826abca79c3 126 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 127 }
Feike 18:ab0fe311e7f3 128
viviien 23:18b0be02187f 129
Feike 12:2382468d36a4 130
Feike 19:9c8ab7922191 131 float anglestep(float angle) {
Feike 18:ab0fe311e7f3 132 float steps;
Feike 18:ab0fe311e7f3 133 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 134 return steps;
Feike 17:6da57acb7bea 135 }
Feike 17:6da57acb7bea 136
viviien 21:c826abca79c3 137
viviien 22:9f911405e096 138 float movefunction() {
viviien 21:c826abca79c3 139
viviien 22:9f911405e096 140 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 141 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 21:c826abca79c3 142 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 21:c826abca79c3 143
viviien 21:c826abca79c3 144 theta1 = theta1 - oldtheta1;
viviien 21:c826abca79c3 145 theta2 = theta2 - oldtheta2;
viviien 21:c826abca79c3 146 theta3 = theta3 - oldtheta3;
viviien 21:c826abca79c3 147
viviien 25:eb3204e45d33 148 steps1 = anglestep(theta1);
viviien 25:eb3204e45d33 149 pc.printf("\n\rsteps1.1 %f", steps1);
viviien 25:eb3204e45d33 150 steps2 = anglestep(theta2);
viviien 25:eb3204e45d33 151 steps3 = anglestep(theta3);
viviien 21:c826abca79c3 152
viviien 21:c826abca79c3 153 // Set the direction of the motors.
viviien 21:c826abca79c3 154 if (theta1 < 0) {
viviien 21:c826abca79c3 155 motor1_dir.write(1);
viviien 21:c826abca79c3 156 }
viviien 21:c826abca79c3 157 else {
viviien 21:c826abca79c3 158 motor1_dir.write(0);
viviien 21:c826abca79c3 159 }
viviien 21:c826abca79c3 160
viviien 21:c826abca79c3 161 if (theta2 < 0) {
viviien 21:c826abca79c3 162 motor2_dir.write(0);
viviien 21:c826abca79c3 163 }
viviien 21:c826abca79c3 164 else {
viviien 21:c826abca79c3 165 motor2_dir.write(1);
viviien 21:c826abca79c3 166 }
viviien 21:c826abca79c3 167
viviien 21:c826abca79c3 168 if (theta3 < 0) {
viviien 21:c826abca79c3 169 motor3_dir.write(0);
viviien 21:c826abca79c3 170 }
viviien 21:c826abca79c3 171 else {
viviien 21:c826abca79c3 172 motor3_dir.write(1);
viviien 21:c826abca79c3 173 }
viviien 21:c826abca79c3 174
viviien 21:c826abca79c3 175 int frequency_pwm = 10000; //10 kHz PWM
viviien 21:c826abca79c3 176 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 21:c826abca79c3 177 motor1_pwm.write(0.7); // write Duty Cycle
viviien 21:c826abca79c3 178
viviien 21:c826abca79c3 179 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 21:c826abca79c3 180 motor2_pwm.write(0.7); // write Duty Cycle
viviien 21:c826abca79c3 181
viviien 21:c826abca79c3 182 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 22:9f911405e096 183 motor3_pwm.write(0.7); // write Duty Cycle
viviien 22:9f911405e096 184
viviien 22:9f911405e096 185 check1 = true;
viviien 22:9f911405e096 186 check2 = true;
viviien 22:9f911405e096 187 check3 = true;
viviien 24:61d03692f92e 188
viviien 25:eb3204e45d33 189
viviien 25:eb3204e45d33 190 // for (int i=0; i<100; i++) {
viviien 25:eb3204e45d33 191 // pc.printf("\n\rcounts1 %i", counts1);
viviien 25:eb3204e45d33 192 // wait(0.001);
viviien 25:eb3204e45d33 193 // }
viviien 24:61d03692f92e 194
viviien 25:eb3204e45d33 195 pc.printf("\n\rsteps3.1 %f", steps3);
viviien 25:eb3204e45d33 196 pc.printf("\n\rcounts3.1 %i", counts3);
viviien 25:eb3204e45d33 197
viviien 25:eb3204e45d33 198
viviien 21:c826abca79c3 199 while (check1 || check2 || check3) {
viviien 25:eb3204e45d33 200 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f",steps1, steps2, steps3);
viviien 25:eb3204e45d33 201 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 25:eb3204e45d33 202
viviien 25:eb3204e45d33 203 if (steps1 <= 0) {
viviien 25:eb3204e45d33 204 if(counts1<=steps1) {
viviien 25:eb3204e45d33 205 pulses1.detach();
viviien 25:eb3204e45d33 206 pc.printf("\n 1 is false");
viviien 25:eb3204e45d33 207
viviien 21:c826abca79c3 208 motor1_pwm.write(0);
viviien 21:c826abca79c3 209 check1=false;
viviien 21:c826abca79c3 210 }
viviien 25:eb3204e45d33 211 if (counts2<=steps2) {
viviien 25:eb3204e45d33 212 pulses2.detach();
viviien 24:61d03692f92e 213 pc.printf("\n 2 is false");
viviien 21:c826abca79c3 214 motor2_pwm.write(0);
viviien 21:c826abca79c3 215 check2=false;
viviien 21:c826abca79c3 216 }
viviien 25:eb3204e45d33 217 if (counts3>=abs(steps3)) {
viviien 25:eb3204e45d33 218 pulses3.detach();
viviien 24:61d03692f92e 219 pc.printf("\n 3 is false");
viviien 21:c826abca79c3 220 motor3_pwm.write(0);
viviien 21:c826abca79c3 221 check3=false;
viviien 21:c826abca79c3 222 //pc.printf("\n\rsteps %f, counts %f", steps3, counts3);
viviien 25:eb3204e45d33 223 pc.printf("\n\rsteps3 %f", steps3);
viviien 25:eb3204e45d33 224 pc.printf("\n\rcounts3 %i", counts3);
viviien 25:eb3204e45d33 225 }
viviien 25:eb3204e45d33 226 }
viviien 25:eb3204e45d33 227 if (steps1 >= 0) {
viviien 25:eb3204e45d33 228 if(counts1>=steps1) {
viviien 25:eb3204e45d33 229 pc.printf("\n 1 is false");
viviien 25:eb3204e45d33 230 motor1_pwm.write(0);
viviien 25:eb3204e45d33 231 check1=false;
viviien 21:c826abca79c3 232 }
viviien 25:eb3204e45d33 233 if (counts2>=steps2) {
viviien 25:eb3204e45d33 234 pc.printf("\n 2 is false");
viviien 25:eb3204e45d33 235 motor2_pwm.write(0);
viviien 25:eb3204e45d33 236 check2=false;
viviien 25:eb3204e45d33 237 }
viviien 25:eb3204e45d33 238 if (counts3<=-steps3) {
viviien 25:eb3204e45d33 239 pc.printf("\n 3 is false");
viviien 25:eb3204e45d33 240 motor3_pwm.write(0);
viviien 25:eb3204e45d33 241 check3=false;
viviien 25:eb3204e45d33 242 }
viviien 25:eb3204e45d33 243 }
viviien 25:eb3204e45d33 244
viviien 21:c826abca79c3 245 }
viviien 25:eb3204e45d33 246 //Encoder1.reset();
viviien 25:eb3204e45d33 247 wait(1.0);
viviien 25:eb3204e45d33 248 pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
viviien 22:9f911405e096 249 }
viviien 22:9f911405e096 250
viviien 22:9f911405e096 251 int main(void) {
viviien 25:eb3204e45d33 252 pulses1.attach(&getpulses1, 1.0/1000);
viviien 25:eb3204e45d33 253 pulses2.attach(&getpulses2, 1.0/1000);
viviien 25:eb3204e45d33 254 pulses3.attach(&getpulses3, 1.0/1000);
viviien 25:eb3204e45d33 255 Encoder1.reset();
viviien 25:eb3204e45d33 256 Encoder2.reset();
viviien 25:eb3204e45d33 257 Encoder3.reset();
viviien 25:eb3204e45d33 258
Feike 18:ab0fe311e7f3 259 pc.baud(115200);
Feike 18:ab0fe311e7f3 260
Feike 17:6da57acb7bea 261 char cc = pc.getc();
Feike 19:9c8ab7922191 262
Feike 19:9c8ab7922191 263 while(true) {
Feike 18:ab0fe311e7f3 264
Feike 19:9c8ab7922191 265 delta_calcinverse(x0,y0,z0);
Feike 19:9c8ab7922191 266
Feike 19:9c8ab7922191 267 oldtheta1 = theta1;
Feike 19:9c8ab7922191 268 oldtheta2 = theta2;
viviien 21:c826abca79c3 269 oldtheta3 = theta3;
Feike 18:ab0fe311e7f3 270 char cc = pc.getc();
Feike 18:ab0fe311e7f3 271
viviien 21:c826abca79c3 272 if (cc=='d') {
viviien 24:61d03692f92e 273 x0=x0+5.0f;
viviien 22:9f911405e096 274 if (x0>=-75 && x0<=75) {
viviien 22:9f911405e096 275 movefunction ();
viviien 22:9f911405e096 276 }
viviien 22:9f911405e096 277 else {
viviien 24:61d03692f92e 278 x0=x0-5.0f;
viviien 22:9f911405e096 279 }
viviien 21:c826abca79c3 280 }
Feike 19:9c8ab7922191 281
viviien 21:c826abca79c3 282 if (cc=='a') {
viviien 24:61d03692f92e 283 x0=x0-5.0f;
viviien 21:c826abca79c3 284 if (x0>=-75 && x0<=75) {
viviien 21:c826abca79c3 285 movefunction ();
viviien 21:c826abca79c3 286 }
viviien 21:c826abca79c3 287 else {
viviien 24:61d03692f92e 288 x0=x0+5.0f;
viviien 21:c826abca79c3 289 }
viviien 21:c826abca79c3 290 }
viviien 21:c826abca79c3 291
viviien 21:c826abca79c3 292 if (cc=='w') {
viviien 24:61d03692f92e 293 y0=y0+5.0f;
viviien 21:c826abca79c3 294 if (y0>=-75 && y0<=75) {
viviien 21:c826abca79c3 295 movefunction ();
viviien 21:c826abca79c3 296 }
viviien 21:c826abca79c3 297 else {
viviien 24:61d03692f92e 298 y0=y0-5.0f;
viviien 21:c826abca79c3 299 }
Feike 19:9c8ab7922191 300 }
Feike 19:9c8ab7922191 301
viviien 21:c826abca79c3 302 if (cc=='s') {
viviien 24:61d03692f92e 303 y0=y0-5.0f;
viviien 21:c826abca79c3 304 if (y0>=-75 && y0<=75) {
viviien 21:c826abca79c3 305 movefunction ();
viviien 21:c826abca79c3 306 }
viviien 21:c826abca79c3 307 else {
viviien 24:61d03692f92e 308 y0=y0+5.0f;
Feike 19:9c8ab7922191 309 }
viviien 21:c826abca79c3 310 }
viviien 21:c826abca79c3 311
viviien 21:c826abca79c3 312 if (cc=='u') {
viviien 24:61d03692f92e 313 z0=z0+5.0f;
viviien 21:c826abca79c3 314 if (z0>=-210 && z0<=-130) {
viviien 21:c826abca79c3 315 movefunction ();
viviien 21:c826abca79c3 316 }
viviien 21:c826abca79c3 317 else {
viviien 24:61d03692f92e 318 z0=z0-5.0f;
viviien 21:c826abca79c3 319 }
viviien 21:c826abca79c3 320 }
Feike 18:ab0fe311e7f3 321
viviien 21:c826abca79c3 322 if (cc=='j') {
viviien 24:61d03692f92e 323 z0=z0-5.0f;
viviien 21:c826abca79c3 324 if (z0>=-210 && z0<=-130) {
viviien 21:c826abca79c3 325 movefunction ();
viviien 21:c826abca79c3 326 }
viviien 21:c826abca79c3 327 else {
viviien 24:61d03692f92e 328 z0=z0+5.0f;
Feike 18:ab0fe311e7f3 329 }
viviien 21:c826abca79c3 330 }
viviien 22:9f911405e096 331 }
viviien 22:9f911405e096 332 }