For Kenya Workshop
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- haarkon
- Date:
- 2018-06-05
- Revision:
- 4:5038b4cd1088
- Parent:
- 3:0221daeeaa86
- Child:
- 5:6afed0e241cb
File content as of revision 4:5038b4cd1088:
#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 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); CNY70 ligneD (PC_3); 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 (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); 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); 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; wait (0.2); } }