Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 3:0221daeeaa86
- Parent:
- 2:e1372e7b671f
- Child:
- 4:5038b4cd1088
diff -r e1372e7b671f -r 0221daeeaa86 main.cpp --- a/main.cpp Wed May 23 15:11:02 2018 +0000 +++ b/main.cpp Thu May 31 17:47:43 2018 +0000 @@ -2,49 +2,98 @@ #include "CMPS03.h" #include "CNY70.h" #include "GP2A.h" -#include "PID.h" -#include "Pixy.h" #include "RC_Servo.h" #include "VMA306.h" +#include "Pixy.h" -main () -{ -Serial pc (USBTX, USBRX, 921600); - -CMPS03 boussole (PC_4, PB_9, PB_8); +Serial pc (PA_2, PA_3, 921600); -CNY70 ligneG (PC_3); -CNY70 ligneD (PC_2); -CNY70 mediane (PA_7); +CMPS03 boussole (PC_4); -GP2A ld1 (PC_0, 20, 150, 60); -GP2A sd1 (PB_0, 7, 80, 20.88); -GP2A ld2 (PC_1, 20, 150, 60); -GP2A sd2 (PA_4, 7, 80, 20.88); +CNY70 ligneD (PC_3); +CNY70 ligneG (PC_2); +CNY70 exterior (PA_7); -PID motG (TIM4, PA_9, PC_9, PC_8); -PID motD (TIM3, PA_8, PC_6, PC_5); - -PIXY pixy (PA_1, PA_0); +GP2A ld1 (PA_4, 20, 150, 55); +GP2A sd1 (PB_0, 5, 80, 10); -VMA306 us_G (PB_15, PA_6); -VMA306 us_B (PB_14, PC_7); -VMA306 us_D (PB_13, PB_2); +RC_Servo ballon (PB_10, 0); +RC_Servo verrou (PA_15, 0); -AnalogIn vbat (PB_1); +VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); -DigitalIn jack (PC_13); - -InterruptIn Vg (PA_11); -InterruptIn Vd (PA_10); +PIXY Pixy (PA_0, PA_1, 230400); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); DigitalOut disquette (PA_12); -RC_Servo ballon (PB_10, 0); -RC_Servo verrou (PA_15, 0); +main () +{ + int sens = 1, nbiter = 0, nbCC, nbNM; + double pos = 0.5; + T_pixyCCBloc CCBuf; + T_pixyNMBloc NMBuf; + + pc.printf ("\n\rHelloWorld\n"); + led1 = 1; + led2 = 0; + disquette = 0; + + 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 ("HelloWorld"); + 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); + } + } + + pc.printf ("\n"); + led1 = !led1; + led2 = !led2; + wait (0.2); + } } \ No newline at end of file