Bibliothèque des éléments du robot
Dependencies: CMPS03 CNY70 GP2A PID Pixy RC_Servo VMA306 mbed
Diff: main.cpp
- Revision:
- 4:5038b4cd1088
- Parent:
- 3:0221daeeaa86
- Child:
- 5:6afed0e241cb
--- a/main.cpp Thu May 31 17:47:43 2018 +0000 +++ b/main.cpp Tue Jun 05 07:29:54 2018 +0000 @@ -5,8 +5,12 @@ #include "RC_Servo.h" #include "VMA306.h" #include "Pixy.h" +#include "PID.h" + +#define PI 3.1415926535898 Serial pc (PA_2, PA_3, 921600); +PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); @@ -17,28 +21,33 @@ GP2A ld1 (PA_4, 20, 150, 55); GP2A sd1 (PB_0, 5, 80, 10); -RC_Servo ballon (PB_10, 0); -RC_Servo verrou (PA_15, 0); +RC_Servo ballon (PA_10, 0); +RC_Servo verrou (PA_11, 0); VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); PIXY Pixy (PA_0, PA_1, 230400); + DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); DigitalOut disquette (PA_12); main () { + int etatmvt = 0; int sens = 1, nbiter = 0, nbCC, nbNM; double pos = 0.5; T_pixyCCBloc CCBuf; T_pixyNMBloc NMBuf; + double x, y, theta; pc.printf ("\n\rHelloWorld\n"); led1 = 1; led2 = 0; disquette = 0; + + motor.resetPosition(); wait (5); @@ -58,7 +67,7 @@ pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ()); pc.printf("\rpos = %2.1lf\n",pos); - ballon.write (pos); + /*ballon.write (pos); verrou.write (pos); nbiter++; if (nbiter%5==0) { @@ -69,7 +78,7 @@ pos-=0.1; if (pos<0.1) sens = 1; } - } + }*/ if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG()); if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB()); @@ -91,6 +100,51 @@ } } + // Square danse !!! + motor.getPosition (&x,&y, &theta); + switch (etatmvt) { + case 0 : + // On avance de 50cm (coordonnés X = 500, Y = 0) + motor.setSpeed (400,400); + if (x >= 500) 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; + break; + case 2 : + // On avance de 50cm (coordonnés X = 500, Y = -500) + motor.setSpeed (400,400); + if (y <= -500) 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; + break; + case 4 : + // On avance de 50cm (coordonnés X = 0, Y = -500) + motor.setSpeed (400,400); + 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; + break; + case 6 : + // On avance de 50cm (coordonnés X = 0, Y = 0) + motor.setSpeed (400,400); + 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; + break; + } + pc.printf ("\n"); led1 = !led1; led2 = !led2;