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@4:5038b4cd1088, 2018-06-05 (annotated)

Committer:
haarkon
Date:
Tue Jun 05 07:29:54 2018 +0000
Revision:
4:5038b4cd1088
Parent:
3:0221daeeaa86
Child:
5:6afed0e241cb
Unworking test program

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 4:5038b4cd1088 8 #include "PID.h"
haarkon 4:5038b4cd1088 9
haarkon 4:5038b4cd1088 10 #define PI 3.1415926535898
haarkon 0:4902b3f17137 11
haarkon 3:0221daeeaa86 12 Serial pc (PA_2, PA_3, 921600);
haarkon 4:5038b4cd1088 13 PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
haarkon 0:4902b3f17137 14
haarkon 3:0221daeeaa86 15 CMPS03 boussole (PC_4);
haarkon 0:4902b3f17137 16
haarkon 3:0221daeeaa86 17 CNY70 ligneD (PC_3);
haarkon 3:0221daeeaa86 18 CNY70 ligneG (PC_2);
haarkon 3:0221daeeaa86 19 CNY70 exterior (PA_7);
haarkon 0:4902b3f17137 20
haarkon 3:0221daeeaa86 21 GP2A ld1 (PA_4, 20, 150, 55);
haarkon 3:0221daeeaa86 22 GP2A sd1 (PB_0, 5, 80, 10);
haarkon 0:4902b3f17137 23
haarkon 4:5038b4cd1088 24 RC_Servo ballon (PA_10, 0);
haarkon 4:5038b4cd1088 25 RC_Servo verrou (PA_11, 0);
haarkon 0:4902b3f17137 26
haarkon 3:0221daeeaa86 27 VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
haarkon 0:4902b3f17137 28
haarkon 3:0221daeeaa86 29 PIXY Pixy (PA_0, PA_1, 230400);
haarkon 0:4902b3f17137 30
haarkon 4:5038b4cd1088 31
haarkon 0:4902b3f17137 32 DigitalOut led1 (PA_5);
haarkon 0:4902b3f17137 33 DigitalOut led2 (PD_2);
haarkon 0:4902b3f17137 34 DigitalOut disquette (PA_12);
haarkon 0:4902b3f17137 35
haarkon 3:0221daeeaa86 36 main ()
haarkon 3:0221daeeaa86 37 {
haarkon 4:5038b4cd1088 38 int etatmvt = 0;
haarkon 3:0221daeeaa86 39 int sens = 1, nbiter = 0, nbCC, nbNM;
haarkon 3:0221daeeaa86 40 double pos = 0.5;
haarkon 3:0221daeeaa86 41 T_pixyCCBloc CCBuf;
haarkon 3:0221daeeaa86 42 T_pixyNMBloc NMBuf;
haarkon 4:5038b4cd1088 43 double x, y, theta;
haarkon 3:0221daeeaa86 44
haarkon 3:0221daeeaa86 45 pc.printf ("\n\rHelloWorld\n");
haarkon 3:0221daeeaa86 46 led1 = 1;
haarkon 3:0221daeeaa86 47 led2 = 0;
haarkon 3:0221daeeaa86 48 disquette = 0;
haarkon 4:5038b4cd1088 49
haarkon 4:5038b4cd1088 50 motor.resetPosition();
haarkon 3:0221daeeaa86 51
haarkon 3:0221daeeaa86 52 wait (5);
haarkon 3:0221daeeaa86 53
haarkon 3:0221daeeaa86 54 while (1) {
haarkon 3:0221daeeaa86 55 pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());
haarkon 3:0221daeeaa86 56
haarkon 3:0221daeeaa86 57 if (exterior.whatAmIOn()) pc.printf("\rwhite");
haarkon 3:0221daeeaa86 58 else pc.printf("\rblue ");
haarkon 3:0221daeeaa86 59 if (ligneG.whatAmIOn()) pc.printf("\r\twhite");
haarkon 3:0221daeeaa86 60 else pc.printf("\r\tblue ");
haarkon 3:0221daeeaa86 61 if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n");
haarkon 3:0221daeeaa86 62 else pc.printf("\r\t\tblue\n");
haarkon 3:0221daeeaa86 63
haarkon 3:0221daeeaa86 64 pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage());
haarkon 3:0221daeeaa86 65
haarkon 3:0221daeeaa86 66 pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ());
haarkon 3:0221daeeaa86 67 pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ());
haarkon 2:e1372e7b671f 68
haarkon 3:0221daeeaa86 69 pc.printf("\rpos = %2.1lf\n",pos);
haarkon 4:5038b4cd1088 70 /*ballon.write (pos);
haarkon 3:0221daeeaa86 71 verrou.write (pos);
haarkon 3:0221daeeaa86 72 nbiter++;
haarkon 3:0221daeeaa86 73 if (nbiter%5==0) {
haarkon 3:0221daeeaa86 74 if (sens) {
haarkon 3:0221daeeaa86 75 pos += 0.1;
haarkon 3:0221daeeaa86 76 if (pos>0.9) sens = 0;
haarkon 3:0221daeeaa86 77 } else {
haarkon 3:0221daeeaa86 78 pos-=0.1;
haarkon 3:0221daeeaa86 79 if (pos<0.1) sens = 1;
haarkon 3:0221daeeaa86 80 }
haarkon 4:5038b4cd1088 81 }*/
haarkon 3:0221daeeaa86 82
haarkon 3:0221daeeaa86 83 if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
haarkon 3:0221daeeaa86 84 if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
haarkon 3:0221daeeaa86 85 if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD());
haarkon 3:0221daeeaa86 86 pc.printf ("\n");
haarkon 2:e1372e7b671f 87
haarkon 3:0221daeeaa86 88 if (Pixy.checkNewImage()) {
haarkon 3:0221daeeaa86 89 Pixy.detectedObject (&nbNM, &nbCC);
haarkon 3:0221daeeaa86 90 pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM);
haarkon 3:0221daeeaa86 91 while (nbCC > 0) {
haarkon 3:0221daeeaa86 92 CCBuf = Pixy.getCCBloc ();
haarkon 3:0221daeeaa86 93 nbCC--;
haarkon 3:0221daeeaa86 94 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 95 }
haarkon 3:0221daeeaa86 96 while (nbNM > 0) {
haarkon 3:0221daeeaa86 97 NMBuf = Pixy.getNMBloc ();
haarkon 3:0221daeeaa86 98 nbNM--;
haarkon 3:0221daeeaa86 99 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 100 }
haarkon 3:0221daeeaa86 101 }
haarkon 3:0221daeeaa86 102
haarkon 4:5038b4cd1088 103 // Square danse !!!
haarkon 4:5038b4cd1088 104 motor.getPosition (&x,&y, &theta);
haarkon 4:5038b4cd1088 105 switch (etatmvt) {
haarkon 4:5038b4cd1088 106 case 0 :
haarkon 4:5038b4cd1088 107 // On avance de 50cm (coordonnés X = 500, Y = 0)
haarkon 4:5038b4cd1088 108 motor.setSpeed (400,400);
haarkon 4:5038b4cd1088 109 if (x >= 500) etatmvt = 1;
haarkon 4:5038b4cd1088 110 break;
haarkon 4:5038b4cd1088 111 case 1 :
haarkon 4:5038b4cd1088 112 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
haarkon 4:5038b4cd1088 113 motor.setSpeed (400,-400);
haarkon 4:5038b4cd1088 114 if (theta <= -PI/2.0) etatmvt = 2;
haarkon 4:5038b4cd1088 115 break;
haarkon 4:5038b4cd1088 116 case 2 :
haarkon 4:5038b4cd1088 117 // On avance de 50cm (coordonnés X = 500, Y = -500)
haarkon 4:5038b4cd1088 118 motor.setSpeed (400,400);
haarkon 4:5038b4cd1088 119 if (y <= -500) etatmvt = 3;
haarkon 4:5038b4cd1088 120 break;
haarkon 4:5038b4cd1088 121 case 3 :
haarkon 4:5038b4cd1088 122 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
haarkon 4:5038b4cd1088 123 motor.setSpeed (400,-400);
haarkon 4:5038b4cd1088 124 if (theta >= 0) etatmvt = 4;
haarkon 4:5038b4cd1088 125 break;
haarkon 4:5038b4cd1088 126 case 4 :
haarkon 4:5038b4cd1088 127 // On avance de 50cm (coordonnés X = 0, Y = -500)
haarkon 4:5038b4cd1088 128 motor.setSpeed (400,400);
haarkon 4:5038b4cd1088 129 if (x <= 0) etatmvt = 5;
haarkon 4:5038b4cd1088 130 break;
haarkon 4:5038b4cd1088 131 case 5 :
haarkon 4:5038b4cd1088 132 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
haarkon 4:5038b4cd1088 133 motor.setSpeed (400,-400);
haarkon 4:5038b4cd1088 134 if (theta <= PI/2.0) etatmvt = 6;
haarkon 4:5038b4cd1088 135 break;
haarkon 4:5038b4cd1088 136 case 6 :
haarkon 4:5038b4cd1088 137 // On avance de 50cm (coordonnés X = 0, Y = 0)
haarkon 4:5038b4cd1088 138 motor.setSpeed (400,400);
haarkon 4:5038b4cd1088 139 if (y <= 0) etatmvt = 7;
haarkon 4:5038b4cd1088 140 break;
haarkon 4:5038b4cd1088 141 case 7 :
haarkon 4:5038b4cd1088 142 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
haarkon 4:5038b4cd1088 143 motor.setSpeed (400,-400);
haarkon 4:5038b4cd1088 144 if (theta <= 0) etatmvt = 0;
haarkon 4:5038b4cd1088 145 break;
haarkon 4:5038b4cd1088 146 }
haarkon 4:5038b4cd1088 147
haarkon 3:0221daeeaa86 148 pc.printf ("\n");
haarkon 3:0221daeeaa86 149 led1 = !led1;
haarkon 3:0221daeeaa86 150 led2 = !led2;
haarkon 3:0221daeeaa86 151 wait (0.2);
haarkon 3:0221daeeaa86 152 }
haarkon 0:4902b3f17137 153 }