![](/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:
- 8:2d4887624f94
- Parent:
- 7:6d58adc26b78
- Child:
- 9:4053b5217339
--- a/main.cpp Tue Jun 05 13:55:04 2018 +0000 +++ b/main.cpp Thu Oct 24 13:21:27 2019 +0000 @@ -9,9 +9,7 @@ #define PI 3.1415926535898 -#define NSpeed 100 - -Serial pc (PA_2, PA_3, 921600); +Serial pc (PA_2, PA_3, 115200); PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); @@ -20,157 +18,28 @@ 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_15, PA_6, PB_14, PC_7, PB_13, PB_2); +PIXY pixy (PA_0, PA_1, 115200); -PIXY Pixy (PA_0, PA_1, 230400); - -DigitalIn bp (PC_13); +InterruptIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); -DigitalOut disquette (PA_12); +DigitalOut unused1 (PB_10); +DigitalOut unused2 (PA_15); +DigitalOut unused3 (PA_12); + +DigitalIn unused4 (PA_4, PullUp); +DigitalIn unused5 (PB_0, PullUp); +DigitalIn unused6 (PC_1, PullUp); +DigitalIn unused7 (PC_0, PullUp); + +Timer temps; 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, vG, vD; - - pc.printf ("\n\rHelloWorld\n"); - led1 = 1; - led2 = 0; - disquette = 0; - - //motor.resetPosition(); - - wait (5); while (1) { - pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); - - if (exterior.whatAmIOn()) pc.printf("\rwhite"); - else pc.printf("\rblue "); - if (ligneG.whatAmIOn()) pc.printf("\r\twhite"); - else pc.printf("\r\tblue "); - if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n"); - else pc.printf("\r\t\tblue\n"); - - 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()); - if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD()); - pc.printf ("\n"); - - if (Pixy.checkNewImage()) { - Pixy.detectedObject (&nbNM, &nbCC); - pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM); - while (nbCC > 0) { - CCBuf = Pixy.getCCBloc (); - nbCC--; - pc.printf ("\rCC %5d : x=%5d, y=%5d - w=%5d, h=%5d, a=%5d\n", CCBuf.signature, CCBuf.x, CCBuf.y, CCBuf.width, CCBuf.height, (short)CCBuf.angle); - } - while (nbNM > 0) { - NMBuf = Pixy.getNMBloc (); - nbNM--; - pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height); - } - } - - // Square danse !!! - 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 (NSpeed,NSpeed); - if (x > 300) { - etatmvt = 1; - } - break; - case 1 : - // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/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 (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 (NSpeed,-NSpeed); - if (theta > 0) { - etatmvt = 4; - } - break; - case 4 : - // On avance de 50cm (coordonnés X = 0, Y = -500) - 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 (NSpeed,-NSpeed); - if (theta < (PI/2.0)) { - etatmvt = 6; - } - break; - case 6 : - // On avance de 50cm (coordonnés X = 0, Y = 0) - 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 (NSpeed,-NSpeed); - if (theta < 0) { - etatmvt = 0; - } - break; - } - - pc.printf ("\n\n"); - led1 = !led1; - led2 = !led2; - wait (0.2); } } \ No newline at end of file