![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Bibliothèque des éléments du robot
Dependencies: CMPS03 CNY70 GP2A PID Pixy RC_Servo VMA306 mbed
main.cpp@3:0221daeeaa86, 2018-05-31 (annotated)
- Committer:
- haarkon
- Date:
- Thu May 31 17:47:43 2018 +0000
- Revision:
- 3:0221daeeaa86
- Parent:
- 2:e1372e7b671f
- Child:
- 4:5038b4cd1088
Hello World (except PID)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:4902b3f17137 | 1 | #include "mbed.h" |
haarkon | 0:4902b3f17137 | 2 | #include "CMPS03.h" |
haarkon | 0:4902b3f17137 | 3 | #include "CNY70.h" |
haarkon | 0:4902b3f17137 | 4 | #include "GP2A.h" |
haarkon | 0:4902b3f17137 | 5 | #include "RC_Servo.h" |
haarkon | 0:4902b3f17137 | 6 | #include "VMA306.h" |
haarkon | 3:0221daeeaa86 | 7 | #include "Pixy.h" |
haarkon | 0:4902b3f17137 | 8 | |
haarkon | 3:0221daeeaa86 | 9 | Serial pc (PA_2, PA_3, 921600); |
haarkon | 0:4902b3f17137 | 10 | |
haarkon | 3:0221daeeaa86 | 11 | CMPS03 boussole (PC_4); |
haarkon | 0:4902b3f17137 | 12 | |
haarkon | 3:0221daeeaa86 | 13 | CNY70 ligneD (PC_3); |
haarkon | 3:0221daeeaa86 | 14 | CNY70 ligneG (PC_2); |
haarkon | 3:0221daeeaa86 | 15 | CNY70 exterior (PA_7); |
haarkon | 0:4902b3f17137 | 16 | |
haarkon | 3:0221daeeaa86 | 17 | GP2A ld1 (PA_4, 20, 150, 55); |
haarkon | 3:0221daeeaa86 | 18 | GP2A sd1 (PB_0, 5, 80, 10); |
haarkon | 0:4902b3f17137 | 19 | |
haarkon | 3:0221daeeaa86 | 20 | RC_Servo ballon (PB_10, 0); |
haarkon | 3:0221daeeaa86 | 21 | RC_Servo verrou (PA_15, 0); |
haarkon | 0:4902b3f17137 | 22 | |
haarkon | 3:0221daeeaa86 | 23 | VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); |
haarkon | 0:4902b3f17137 | 24 | |
haarkon | 3:0221daeeaa86 | 25 | PIXY Pixy (PA_0, PA_1, 230400); |
haarkon | 0:4902b3f17137 | 26 | |
haarkon | 0:4902b3f17137 | 27 | DigitalOut led1 (PA_5); |
haarkon | 0:4902b3f17137 | 28 | DigitalOut led2 (PD_2); |
haarkon | 0:4902b3f17137 | 29 | DigitalOut disquette (PA_12); |
haarkon | 0:4902b3f17137 | 30 | |
haarkon | 3:0221daeeaa86 | 31 | main () |
haarkon | 3:0221daeeaa86 | 32 | { |
haarkon | 3:0221daeeaa86 | 33 | int sens = 1, nbiter = 0, nbCC, nbNM; |
haarkon | 3:0221daeeaa86 | 34 | double pos = 0.5; |
haarkon | 3:0221daeeaa86 | 35 | T_pixyCCBloc CCBuf; |
haarkon | 3:0221daeeaa86 | 36 | T_pixyNMBloc NMBuf; |
haarkon | 3:0221daeeaa86 | 37 | |
haarkon | 3:0221daeeaa86 | 38 | pc.printf ("\n\rHelloWorld\n"); |
haarkon | 3:0221daeeaa86 | 39 | led1 = 1; |
haarkon | 3:0221daeeaa86 | 40 | led2 = 0; |
haarkon | 3:0221daeeaa86 | 41 | disquette = 0; |
haarkon | 3:0221daeeaa86 | 42 | |
haarkon | 3:0221daeeaa86 | 43 | wait (5); |
haarkon | 3:0221daeeaa86 | 44 | |
haarkon | 3:0221daeeaa86 | 45 | while (1) { |
haarkon | 3:0221daeeaa86 | 46 | pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); |
haarkon | 3:0221daeeaa86 | 47 | |
haarkon | 3:0221daeeaa86 | 48 | if (exterior.whatAmIOn()) pc.printf("\rwhite"); |
haarkon | 3:0221daeeaa86 | 49 | else pc.printf("\rblue "); |
haarkon | 3:0221daeeaa86 | 50 | if (ligneG.whatAmIOn()) pc.printf("\r\twhite"); |
haarkon | 3:0221daeeaa86 | 51 | else pc.printf("\r\tblue "); |
haarkon | 3:0221daeeaa86 | 52 | if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n"); |
haarkon | 3:0221daeeaa86 | 53 | else pc.printf("\r\t\tblue\n"); |
haarkon | 3:0221daeeaa86 | 54 | |
haarkon | 3:0221daeeaa86 | 55 | pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage()); |
haarkon | 3:0221daeeaa86 | 56 | |
haarkon | 3:0221daeeaa86 | 57 | pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ()); |
haarkon | 3:0221daeeaa86 | 58 | pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ()); |
haarkon | 2:e1372e7b671f | 59 | |
haarkon | 3:0221daeeaa86 | 60 | pc.printf("\rpos = %2.1lf\n",pos); |
haarkon | 3:0221daeeaa86 | 61 | ballon.write (pos); |
haarkon | 3:0221daeeaa86 | 62 | verrou.write (pos); |
haarkon | 3:0221daeeaa86 | 63 | nbiter++; |
haarkon | 3:0221daeeaa86 | 64 | if (nbiter%5==0) { |
haarkon | 3:0221daeeaa86 | 65 | if (sens) { |
haarkon | 3:0221daeeaa86 | 66 | pos += 0.1; |
haarkon | 3:0221daeeaa86 | 67 | if (pos>0.9) sens = 0; |
haarkon | 3:0221daeeaa86 | 68 | } else { |
haarkon | 3:0221daeeaa86 | 69 | pos-=0.1; |
haarkon | 3:0221daeeaa86 | 70 | if (pos<0.1) sens = 1; |
haarkon | 3:0221daeeaa86 | 71 | } |
haarkon | 3:0221daeeaa86 | 72 | } |
haarkon | 3:0221daeeaa86 | 73 | |
haarkon | 3:0221daeeaa86 | 74 | if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG()); |
haarkon | 3:0221daeeaa86 | 75 | if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB()); |
haarkon | 3:0221daeeaa86 | 76 | if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD()); |
haarkon | 3:0221daeeaa86 | 77 | pc.printf ("\n"); |
haarkon | 2:e1372e7b671f | 78 | |
haarkon | 3:0221daeeaa86 | 79 | if (Pixy.checkNewImage()) { |
haarkon | 3:0221daeeaa86 | 80 | Pixy.detectedObject (&nbNM, &nbCC); |
haarkon | 3:0221daeeaa86 | 81 | pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM); |
haarkon | 3:0221daeeaa86 | 82 | while (nbCC > 0) { |
haarkon | 3:0221daeeaa86 | 83 | CCBuf = Pixy.getCCBloc (); |
haarkon | 3:0221daeeaa86 | 84 | nbCC--; |
haarkon | 3:0221daeeaa86 | 85 | 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); |
haarkon | 3:0221daeeaa86 | 86 | } |
haarkon | 3:0221daeeaa86 | 87 | while (nbNM > 0) { |
haarkon | 3:0221daeeaa86 | 88 | NMBuf = Pixy.getNMBloc (); |
haarkon | 3:0221daeeaa86 | 89 | nbNM--; |
haarkon | 3:0221daeeaa86 | 90 | pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height); |
haarkon | 3:0221daeeaa86 | 91 | } |
haarkon | 3:0221daeeaa86 | 92 | } |
haarkon | 3:0221daeeaa86 | 93 | |
haarkon | 3:0221daeeaa86 | 94 | pc.printf ("\n"); |
haarkon | 3:0221daeeaa86 | 95 | led1 = !led1; |
haarkon | 3:0221daeeaa86 | 96 | led2 = !led2; |
haarkon | 3:0221daeeaa86 | 97 | wait (0.2); |
haarkon | 3:0221daeeaa86 | 98 | } |
haarkon | 0:4902b3f17137 | 99 | } |