![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 7:6d58adc26b78
- Parent:
- 5:6afed0e241cb
- Child:
- 8:2d4887624f94
--- a/main.cpp Tue Jun 05 08:06:57 2018 +0000 +++ b/main.cpp Tue Jun 05 13:55:04 2018 +0000 @@ -9,8 +9,10 @@ #define PI 3.1415926535898 +#define NSpeed 100 + Serial pc (PA_2, PA_3, 921600); -//PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); +PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); @@ -21,13 +23,14 @@ GP2A ld1 (PA_4, 20, 150, 55); GP2A sd1 (PB_0, 5, 80, 10); -RC_Servo ballon (PA_10, 0); -RC_Servo verrou (PA_11, 0); +RC_Servo ballon (PB_10, 0); +RC_Servo verrou (PA_15, 0); VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); PIXY Pixy (PA_0, PA_1, 230400); +DigitalIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); @@ -40,7 +43,7 @@ double pos = 0.5; T_pixyCCBloc CCBuf; T_pixyNMBloc NMBuf; - double x, y, theta; + double x, y, theta, vG, vD; pc.printf ("\n\rHelloWorld\n"); led1 = 1; @@ -101,51 +104,71 @@ } // Square danse !!! - /*motor.getPosition (&x,&y, &theta); + motor.getPosition (&x,&y, &theta); + motor.getSpeed (&vG, &vD); + + pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD); + switch (etatmvt) { case 0 : // On avance de 50cm (coordonnés X = 500, Y = 0) - motor.setSpeed (400,400); - if (x >= 500) etatmvt = 1; + motor.setSpeed (NSpeed,NSpeed); + if (x > 300) { + etatmvt = 1; + } break; case 1 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2 - motor.setSpeed (400,-400); - if (theta <= -PI/2.0) etatmvt = 2; + motor.setSpeed (NSpeed,-NSpeed); + if (theta < (-PI/2.0)) { + etatmvt = 2; + } break; case 2 : // On avance de 50cm (coordonnés X = 500, Y = -500) - motor.setSpeed (400,400); - if (y <= -500) etatmvt = 3; + motor.setSpeed (NSpeed,NSpeed); + if (y < -300) { + etatmvt = 3; + } break; case 3 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) - motor.setSpeed (400,-400); - if (theta >= 0) etatmvt = 4; + motor.setSpeed (NSpeed,-NSpeed); + if (theta > 0) { + etatmvt = 4; + } break; case 4 : // On avance de 50cm (coordonnés X = 0, Y = -500) - motor.setSpeed (400,400); - if (x <= 0) etatmvt = 5; + motor.setSpeed (NSpeed,NSpeed); + if (x < 0) { + etatmvt = 5; + } break; case 5 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 - motor.setSpeed (400,-400); - if (theta <= PI/2.0) etatmvt = 6; + motor.setSpeed (NSpeed,-NSpeed); + if (theta < (PI/2.0)) { + etatmvt = 6; + } break; case 6 : // On avance de 50cm (coordonnés X = 0, Y = 0) - motor.setSpeed (400,400); - if (y <= 0) etatmvt = 7; + motor.setSpeed (NSpeed,NSpeed); + if (y > 0) { + etatmvt = 7; + } break; case 7 : // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 - motor.setSpeed (400,-400); - if (theta <= 0) etatmvt = 0; + motor.setSpeed (NSpeed,-NSpeed); + if (theta < 0) { + etatmvt = 0; + } break; } -*/ - pc.printf ("\n"); + + pc.printf ("\n\n"); led1 = !led1; led2 = !led2; wait (0.2);