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@7:6d58adc26b78, 2018-06-05 (annotated)

Committer:
haarkon
Date:
Tue Jun 05 13:55:04 2018 +0000
Revision:
7:6d58adc26b78
Parent:
5:6afed0e241cb 
Unstable first attempt of all system in one test

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