borrowed codes
Dependencies: mbed lib_FRC_2019
main.cpp@9:234439133426, 2019-06-07 (annotated)
- Committer:
- kyxstark
- Date:
- Fri Jun 07 06:06:52 2019 +0000
- Revision:
- 9:234439133426
- Parent:
- 8:7b0fa8a914c0
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 "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 | 9:234439133426 | 61 | |
kyxstark | 9:234439133426 | 62 | //motor.setSpeed (0,0); |
haarkon | 3:0221daeeaa86 | 63 | |
haarkon | 3:0221daeeaa86 | 64 | while (1) { |
haarkon | 3:0221daeeaa86 | 65 | pc.printf ("\rCap = %5.2lf\n", boussole.getBearing()); |
haarkon | 3:0221daeeaa86 | 66 | |
haarkon | 3:0221daeeaa86 | 67 | if (exterior.whatAmIOn()) pc.printf("\rwhite"); |
haarkon | 3:0221daeeaa86 | 68 | else pc.printf("\rblue "); |
haarkon | 3:0221daeeaa86 | 69 | if (ligneG.whatAmIOn()) pc.printf("\r\twhite"); |
haarkon | 3:0221daeeaa86 | 70 | else pc.printf("\r\tblue "); |
haarkon | 3:0221daeeaa86 | 71 | if (ligneD.whatAmIOn()) pc.printf("\r\t\twhite\n"); |
haarkon | 3:0221daeeaa86 | 72 | else pc.printf("\r\t\tblue\n"); |
haarkon | 3:0221daeeaa86 | 73 | |
haarkon | 3:0221daeeaa86 | 74 | pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage()); |
haarkon | 3:0221daeeaa86 | 75 | |
haarkon | 3:0221daeeaa86 | 76 | |
haarkon | 3:0221daeeaa86 | 77 | if (UltraSon.isUSGReady()) pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG()); |
haarkon | 3:0221daeeaa86 | 78 | if (UltraSon.isUSBReady()) pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB()); |
haarkon | 3:0221daeeaa86 | 79 | if (UltraSon.isUSDReady()) pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD()); |
haarkon | 3:0221daeeaa86 | 80 | pc.printf ("\n"); |
haarkon | 2:e1372e7b671f | 81 | |
haarkon | 3:0221daeeaa86 | 82 | if (Pixy.checkNewImage()) { |
haarkon | 3:0221daeeaa86 | 83 | Pixy.detectedObject (&nbNM, &nbCC); |
haarkon | 3:0221daeeaa86 | 84 | pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM); |
haarkon | 3:0221daeeaa86 | 85 | while (nbCC > 0) { |
haarkon | 3:0221daeeaa86 | 86 | CCBuf = Pixy.getCCBloc (); |
haarkon | 3:0221daeeaa86 | 87 | nbCC--; |
haarkon | 3:0221daeeaa86 | 88 | 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 | 89 | } |
haarkon | 3:0221daeeaa86 | 90 | while (nbNM > 0) { |
haarkon | 3:0221daeeaa86 | 91 | NMBuf = Pixy.getNMBloc (); |
haarkon | 3:0221daeeaa86 | 92 | nbNM--; |
haarkon | 3:0221daeeaa86 | 93 | 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 | 94 | } |
haarkon | 3:0221daeeaa86 | 95 | } |
haarkon | 3:0221daeeaa86 | 96 | |
haarkon | 4:5038b4cd1088 | 97 | // Square danse !!! |
haarkon | 7:6d58adc26b78 | 98 | motor.getPosition (&x,&y, &theta); |
haarkon | 7:6d58adc26b78 | 99 | motor.getSpeed (&vG, &vD); |
haarkon | 7:6d58adc26b78 | 100 | |
haarkon | 7:6d58adc26b78 | 101 | 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 | 9:234439133426 | 102 | |
haarkon | 4:5038b4cd1088 | 103 | switch (etatmvt) { |
haarkon | 4:5038b4cd1088 | 104 | case 0 : |
haarkon | 4:5038b4cd1088 | 105 | // On avance de 50cm (coordonnés X = 500, Y = 0) |
haarkon | 7:6d58adc26b78 | 106 | motor.setSpeed (NSpeed,NSpeed); |
haarkon | 7:6d58adc26b78 | 107 | if (x > 300) { |
haarkon | 7:6d58adc26b78 | 108 | etatmvt = 1; |
haarkon | 7:6d58adc26b78 | 109 | } |
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 | 7:6d58adc26b78 | 113 | motor.setSpeed (NSpeed,-NSpeed); |
haarkon | 7:6d58adc26b78 | 114 | if (theta < (-PI/2.0)) { |
haarkon | 7:6d58adc26b78 | 115 | etatmvt = 2; |
haarkon | 7:6d58adc26b78 | 116 | } |
haarkon | 4:5038b4cd1088 | 117 | break; |
haarkon | 4:5038b4cd1088 | 118 | case 2 : |
haarkon | 4:5038b4cd1088 | 119 | // On avance de 50cm (coordonnés X = 500, Y = -500) |
haarkon | 7:6d58adc26b78 | 120 | motor.setSpeed (NSpeed,NSpeed); |
haarkon | 7:6d58adc26b78 | 121 | if (y < -300) { |
haarkon | 7:6d58adc26b78 | 122 | etatmvt = 3; |
haarkon | 7:6d58adc26b78 | 123 | } |
haarkon | 4:5038b4cd1088 | 124 | break; |
haarkon | 4:5038b4cd1088 | 125 | case 3 : |
haarkon | 4:5038b4cd1088 | 126 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) |
haarkon | 7:6d58adc26b78 | 127 | motor.setSpeed (NSpeed,-NSpeed); |
haarkon | 7:6d58adc26b78 | 128 | if (theta > 0) { |
haarkon | 7:6d58adc26b78 | 129 | etatmvt = 4; |
haarkon | 7:6d58adc26b78 | 130 | } |
haarkon | 4:5038b4cd1088 | 131 | break; |
haarkon | 4:5038b4cd1088 | 132 | case 4 : |
haarkon | 4:5038b4cd1088 | 133 | // On avance de 50cm (coordonnés X = 0, Y = -500) |
haarkon | 7:6d58adc26b78 | 134 | motor.setSpeed (NSpeed,NSpeed); |
haarkon | 7:6d58adc26b78 | 135 | if (x < 0) { |
haarkon | 7:6d58adc26b78 | 136 | etatmvt = 5; |
haarkon | 7:6d58adc26b78 | 137 | } |
haarkon | 4:5038b4cd1088 | 138 | break; |
haarkon | 4:5038b4cd1088 | 139 | case 5 : |
haarkon | 4:5038b4cd1088 | 140 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 |
haarkon | 7:6d58adc26b78 | 141 | motor.setSpeed (NSpeed,-NSpeed); |
haarkon | 7:6d58adc26b78 | 142 | if (theta < (PI/2.0)) { |
haarkon | 7:6d58adc26b78 | 143 | etatmvt = 6; |
haarkon | 7:6d58adc26b78 | 144 | } |
haarkon | 4:5038b4cd1088 | 145 | break; |
haarkon | 4:5038b4cd1088 | 146 | case 6 : |
haarkon | 4:5038b4cd1088 | 147 | // On avance de 50cm (coordonnés X = 0, Y = 0) |
haarkon | 7:6d58adc26b78 | 148 | motor.setSpeed (NSpeed,NSpeed); |
haarkon | 7:6d58adc26b78 | 149 | if (y > 0) { |
haarkon | 7:6d58adc26b78 | 150 | etatmvt = 7; |
haarkon | 7:6d58adc26b78 | 151 | } |
haarkon | 4:5038b4cd1088 | 152 | break; |
haarkon | 4:5038b4cd1088 | 153 | case 7 : |
haarkon | 4:5038b4cd1088 | 154 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 |
haarkon | 7:6d58adc26b78 | 155 | motor.setSpeed (NSpeed,-NSpeed); |
haarkon | 7:6d58adc26b78 | 156 | if (theta < 0) { |
haarkon | 7:6d58adc26b78 | 157 | etatmvt = 0; |
haarkon | 7:6d58adc26b78 | 158 | } |
haarkon | 4:5038b4cd1088 | 159 | break; |
haarkon | 4:5038b4cd1088 | 160 | } |
kyxstark | 9:234439133426 | 161 | |
haarkon | 7:6d58adc26b78 | 162 | pc.printf ("\n\n"); |
haarkon | 3:0221daeeaa86 | 163 | led1 = !led1; |
haarkon | 3:0221daeeaa86 | 164 | led2 = !led2; |
kyxstark | 8:7b0fa8a914c0 | 165 | wait (1); |
haarkon | 3:0221daeeaa86 | 166 | } |
haarkon | 0:4902b3f17137 | 167 | } |