programme test hackathon 2019
Dependencies: mbed lib_FRC_2019
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
- Child:
- 8:7b0fa8a914c0
Unstable first attempt of all system in one test
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 | 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 | } |