programme test hackathon 2019
Dependencies: mbed lib_FRC_2019
Diff: main.cpp
- Revision:
- 8:7b0fa8a914c0
- Parent:
- 7:6d58adc26b78
- Child:
- 9:234439133426
diff -r 6d58adc26b78 -r 7b0fa8a914c0 main.cpp --- a/main.cpp Tue Jun 05 13:55:04 2018 +0000 +++ b/main.cpp Thu Jun 06 12:34:37 2019 +0000 @@ -1,18 +1,17 @@ #include "mbed.h" #include "CMPS03.h" #include "CNY70.h" -#include "GP2A.h" -#include "RC_Servo.h" #include "VMA306.h" #include "Pixy.h" #include "PID.h" + #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 (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8); CMPS03 boussole (PC_4); @@ -20,13 +19,8 @@ CNY70 ligneG (PC_2); CNY70 exterior (PA_7); -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); - -VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); +VMA306 UltraSon (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6); PIXY Pixy (PA_0, PA_1, 230400); @@ -34,25 +28,37 @@ DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); -DigitalOut disquette (PA_12); main () { - int etatmvt = 0; + led1 = 0; + led2 = 0; + pc.printf ("\n\rHelloWorld\n"); + wait(0.5); + led1 = 1; + led2 = 0; + + int etatmvt = 00; int sens = 1, nbiter = 0, nbCC, nbNM; double pos = 0.5; T_pixyCCBloc CCBuf; T_pixyNMBloc NMBuf; double x, y, theta, vG, vD; - pc.printf ("\n\rHelloWorld\n"); + pc.printf ("\n\rinit\n"); led1 = 1; led2 = 0; - disquette = 0; - //motor.resetPosition(); + motor.resetPosition(); - wait (5); + pc.printf ("\n\r3\n"); + wait (1); + pc.printf ("\n\r2\n"); + wait (1); + pc.printf ("\n\r1\n"); + wait (1); + + motor.setSpeed (0,0); while (1) { pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); @@ -66,22 +72,6 @@ pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage()); - pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ()); - pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ()); - - pc.printf("\rpos = %2.1lf\n",pos); - ballon.write (pos); - verrou.write (pos); - nbiter++; - if (nbiter%5==0) { - if (sens) { - pos += 0.1; - if (pos>0.9) sens = 0; - } else { - 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()); @@ -108,7 +98,7 @@ 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) @@ -167,10 +157,10 @@ } break; } - +*/ pc.printf ("\n\n"); led1 = !led1; led2 = !led2; - wait (0.2); + wait (1); } } \ No newline at end of file