Tuk ZeroFour / Mbed 2 deprecated FRC_2019_test

borrowed codes

Dependencies:   mbed lib_FRC_2019

main.cpp@8:7b0fa8a914c0, 2019-06-06 (annotated)

Committer:
kyxstark
Date:
Thu Jun 06 12:34:37 2019 +0000
Revision:
8:7b0fa8a914c0
Parent:
7:6d58adc26b78
Child:
9:234439133426
hackathon 2019

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