FRC - Hackathon / Mbed 2 deprecated FRC_2018

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?

UserRevisionLine numberNew 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 }