FRC - Hackathon
/
FRC2018_Bis
Programme de test avec librairies minimales et un seul ticker
main.cpp@0:c3b54b05bf77, 2018-06-06 (annotated)
- Committer:
- haarkon
- Date:
- Wed Jun 06 12:48:02 2018 +0000
- Revision:
- 0:c3b54b05bf77
Programme de test n'utilisant que les biblioth?ques minimales;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:c3b54b05bf77 | 1 | #include "mbed.h" |
haarkon | 0:c3b54b05bf77 | 2 | #include "Pixy.h" |
haarkon | 0:c3b54b05bf77 | 3 | #include "PID.h" |
haarkon | 0:c3b54b05bf77 | 4 | |
haarkon | 0:c3b54b05bf77 | 5 | Serial pc (PA_2, PA_3, 921600); |
haarkon | 0:c3b54b05bf77 | 6 | |
haarkon | 0:c3b54b05bf77 | 7 | PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); |
haarkon | 0:c3b54b05bf77 | 8 | |
haarkon | 0:c3b54b05bf77 | 9 | InterruptIn boussole (PC_4); |
haarkon | 0:c3b54b05bf77 | 10 | |
haarkon | 0:c3b54b05bf77 | 11 | AnalogIn CNY70ligneD (PC_3); |
haarkon | 0:c3b54b05bf77 | 12 | AnalogIn CNY70ligneG (PC_2); |
haarkon | 0:c3b54b05bf77 | 13 | AnalogIn CNY70exterior (PA_7); |
haarkon | 0:c3b54b05bf77 | 14 | |
haarkon | 0:c3b54b05bf77 | 15 | AnalogIn GP2sensorL (PA_4); |
haarkon | 0:c3b54b05bf77 | 16 | AnalogIn GP2sensorS (PB_0); |
haarkon | 0:c3b54b05bf77 | 17 | |
haarkon | 0:c3b54b05bf77 | 18 | DigitalOut trig1 (PB_15); |
haarkon | 0:c3b54b05bf77 | 19 | InterruptIn echo1 (PA_6); |
haarkon | 0:c3b54b05bf77 | 20 | DigitalOut trig2 (PB_14); |
haarkon | 0:c3b54b05bf77 | 21 | InterruptIn echo2 (PC_7); |
haarkon | 0:c3b54b05bf77 | 22 | DigitalOut trig3 (PB_13); |
haarkon | 0:c3b54b05bf77 | 23 | InterruptIn echo3 (PB_2); |
haarkon | 0:c3b54b05bf77 | 24 | |
haarkon | 0:c3b54b05bf77 | 25 | //PwmOut ballon (PB_10); |
haarkon | 0:c3b54b05bf77 | 26 | //PwmOut verrou (PA_15); |
haarkon | 0:c3b54b05bf77 | 27 | |
haarkon | 0:c3b54b05bf77 | 28 | PIXY pixy (PA_0, PA_1, 230400); |
haarkon | 0:c3b54b05bf77 | 29 | |
haarkon | 0:c3b54b05bf77 | 30 | DigitalIn bp (PC_13); |
haarkon | 0:c3b54b05bf77 | 31 | |
haarkon | 0:c3b54b05bf77 | 32 | DigitalOut led1 (PA_5); |
haarkon | 0:c3b54b05bf77 | 33 | DigitalOut led2 (PD_2); |
haarkon | 0:c3b54b05bf77 | 34 | DigitalOut disquette (PA_12); |
haarkon | 0:c3b54b05bf77 | 35 | |
haarkon | 0:c3b54b05bf77 | 36 | Timer temps; |
haarkon | 0:c3b54b05bf77 | 37 | Ticker tick; |
haarkon | 0:c3b54b05bf77 | 38 | |
haarkon | 0:c3b54b05bf77 | 39 | bool CMPS03flag, VMA306Flag1, VMA306Flag2, VMA306Flag3; |
haarkon | 0:c3b54b05bf77 | 40 | long CMPS03startTime, VMA306startTime1, VMA306startTime2, VMA306startTime3; |
haarkon | 0:c3b54b05bf77 | 41 | double CMPS03bearing, VMA306dist1, VMA306dist2, VMA306dist3; |
haarkon | 0:c3b54b05bf77 | 42 | double _Kp, _Ki, _Kd; |
haarkon | 0:c3b54b05bf77 | 43 | double _SpeedG, _SpeedD; |
haarkon | 0:c3b54b05bf77 | 44 | double _measDistG, _measDistD; |
haarkon | 0:c3b54b05bf77 | 45 | int _WheelStuckG, _WheelStuckD; |
haarkon | 0:c3b54b05bf77 | 46 | double _X, _Y, _THETA; |
haarkon | 0:c3b54b05bf77 | 47 | |
haarkon | 0:c3b54b05bf77 | 48 | void VMA306rise1(void) |
haarkon | 0:c3b54b05bf77 | 49 | { |
haarkon | 0:c3b54b05bf77 | 50 | VMA306startTime1 = temps.read_us(); |
haarkon | 0:c3b54b05bf77 | 51 | } |
haarkon | 0:c3b54b05bf77 | 52 | |
haarkon | 0:c3b54b05bf77 | 53 | void VMA306fall1(void) |
haarkon | 0:c3b54b05bf77 | 54 | { |
haarkon | 0:c3b54b05bf77 | 55 | VMA306dist1 = (double)(temps.read_us() - VMA306startTime1)/58.0; |
haarkon | 0:c3b54b05bf77 | 56 | VMA306Flag1 = 1; |
haarkon | 0:c3b54b05bf77 | 57 | } |
haarkon | 0:c3b54b05bf77 | 58 | |
haarkon | 0:c3b54b05bf77 | 59 | // Clear and start the timer at the begining of the echo pulse |
haarkon | 0:c3b54b05bf77 | 60 | void VMA306rise2(void) |
haarkon | 0:c3b54b05bf77 | 61 | { |
haarkon | 0:c3b54b05bf77 | 62 | VMA306startTime2 = temps.read_us(); |
haarkon | 0:c3b54b05bf77 | 63 | } |
haarkon | 0:c3b54b05bf77 | 64 | |
haarkon | 0:c3b54b05bf77 | 65 | // Stop and read the timer at the end of the pulse |
haarkon | 0:c3b54b05bf77 | 66 | void VMA306fall2(void) |
haarkon | 0:c3b54b05bf77 | 67 | { |
haarkon | 0:c3b54b05bf77 | 68 | VMA306dist2 = (double)(temps.read_us() - VMA306startTime2)/58.0; |
haarkon | 0:c3b54b05bf77 | 69 | VMA306Flag2 = 1; |
haarkon | 0:c3b54b05bf77 | 70 | } |
haarkon | 0:c3b54b05bf77 | 71 | |
haarkon | 0:c3b54b05bf77 | 72 | // Clear and start the timer at the begining of the echo pulse |
haarkon | 0:c3b54b05bf77 | 73 | void VMA306rise3(void) |
haarkon | 0:c3b54b05bf77 | 74 | { |
haarkon | 0:c3b54b05bf77 | 75 | VMA306startTime3 = temps.read_us(); |
haarkon | 0:c3b54b05bf77 | 76 | } |
haarkon | 0:c3b54b05bf77 | 77 | |
haarkon | 0:c3b54b05bf77 | 78 | // Stop and read the timer at the end of the pulse |
haarkon | 0:c3b54b05bf77 | 79 | void VMA306fall3(void) |
haarkon | 0:c3b54b05bf77 | 80 | { |
haarkon | 0:c3b54b05bf77 | 81 | VMA306dist3 = (double)(temps.read_us() - VMA306startTime3)/58.0; |
haarkon | 0:c3b54b05bf77 | 82 | VMA306Flag3 = 1; |
haarkon | 0:c3b54b05bf77 | 83 | } |
haarkon | 0:c3b54b05bf77 | 84 | |
haarkon | 0:c3b54b05bf77 | 85 | void CMPS03rise (void) |
haarkon | 0:c3b54b05bf77 | 86 | { |
haarkon | 0:c3b54b05bf77 | 87 | CMPS03startTime = temps.read_us(); |
haarkon | 0:c3b54b05bf77 | 88 | } |
haarkon | 0:c3b54b05bf77 | 89 | |
haarkon | 0:c3b54b05bf77 | 90 | void CMPS03fall (void) |
haarkon | 0:c3b54b05bf77 | 91 | { |
haarkon | 0:c3b54b05bf77 | 92 | CMPS03bearing = ((double)(temps.read_us() - CMPS03startTime - 1000)/100.0); |
haarkon | 0:c3b54b05bf77 | 93 | } |
haarkon | 0:c3b54b05bf77 | 94 | |
haarkon | 0:c3b54b05bf77 | 95 | double getGP2Distance (double sensorValue, double slope) |
haarkon | 0:c3b54b05bf77 | 96 | { |
haarkon | 0:c3b54b05bf77 | 97 | return slope/(sensorValue * 3.3); |
haarkon | 0:c3b54b05bf77 | 98 | } |
haarkon | 0:c3b54b05bf77 | 99 | |
haarkon | 0:c3b54b05bf77 | 100 | bool whatIsUnderMyCNY (double sensorValue) |
haarkon | 0:c3b54b05bf77 | 101 | { |
haarkon | 0:c3b54b05bf77 | 102 | return (sensorValue > 0.5) ? 0 : 1; |
haarkon | 0:c3b54b05bf77 | 103 | } |
haarkon | 0:c3b54b05bf77 | 104 | |
haarkon | 0:c3b54b05bf77 | 105 | void baisser (PwmOut *ptr) |
haarkon | 0:c3b54b05bf77 | 106 | { |
haarkon | 0:c3b54b05bf77 | 107 | ptr->pulsewidth_ms(2); |
haarkon | 0:c3b54b05bf77 | 108 | } |
haarkon | 0:c3b54b05bf77 | 109 | |
haarkon | 0:c3b54b05bf77 | 110 | void lever (PwmOut *ptr) |
haarkon | 0:c3b54b05bf77 | 111 | { |
haarkon | 0:c3b54b05bf77 | 112 | ptr->pulsewidth_ms(1); |
haarkon | 0:c3b54b05bf77 | 113 | } |
haarkon | 0:c3b54b05bf77 | 114 | |
haarkon | 0:c3b54b05bf77 | 115 | |
haarkon | 0:c3b54b05bf77 | 116 | main () |
haarkon | 0:c3b54b05bf77 | 117 | { |
haarkon | 0:c3b54b05bf77 | 118 | int nbCC, nbNM, objet = 0; |
haarkon | 0:c3b54b05bf77 | 119 | T_pixyCCBloc CCBuf[20]; |
haarkon | 0:c3b54b05bf77 | 120 | T_pixyNMBloc NMBuf[20]; |
haarkon | 0:c3b54b05bf77 | 121 | double topDepart, capAuDepart; |
haarkon | 0:c3b54b05bf77 | 122 | double x, y, theta; |
haarkon | 0:c3b54b05bf77 | 123 | double speedG = 1000, speedD = 1000; |
haarkon | 0:c3b54b05bf77 | 124 | PwmOut *ptr; |
haarkon | 0:c3b54b05bf77 | 125 | |
haarkon | 0:c3b54b05bf77 | 126 | boussole.rise (&CMPS03rise); |
haarkon | 0:c3b54b05bf77 | 127 | boussole.fall (&CMPS03fall); |
haarkon | 0:c3b54b05bf77 | 128 | |
haarkon | 0:c3b54b05bf77 | 129 | echo1.rise (&VMA306rise1); |
haarkon | 0:c3b54b05bf77 | 130 | echo1.fall (&VMA306fall1); |
haarkon | 0:c3b54b05bf77 | 131 | echo2.rise (&VMA306rise2); |
haarkon | 0:c3b54b05bf77 | 132 | echo2.fall (&VMA306fall2); |
haarkon | 0:c3b54b05bf77 | 133 | echo3.rise (&VMA306rise3); |
haarkon | 0:c3b54b05bf77 | 134 | echo3.fall (&VMA306fall3); |
haarkon | 0:c3b54b05bf77 | 135 | |
haarkon | 0:c3b54b05bf77 | 136 | temps.start(); |
haarkon | 0:c3b54b05bf77 | 137 | |
haarkon | 0:c3b54b05bf77 | 138 | /* ptr = new PwmOut(PB_10); |
haarkon | 0:c3b54b05bf77 | 139 | ptr->period_ms(20); |
haarkon | 0:c3b54b05bf77 | 140 | baisser(ptr); |
haarkon | 0:c3b54b05bf77 | 141 | wait (2); |
haarkon | 0:c3b54b05bf77 | 142 | delete ptr; |
haarkon | 0:c3b54b05bf77 | 143 | |
haarkon | 0:c3b54b05bf77 | 144 | ptr = new PwmOut(PA_15); |
haarkon | 0:c3b54b05bf77 | 145 | ptr->period_ms(20); |
haarkon | 0:c3b54b05bf77 | 146 | lever(ptr); |
haarkon | 0:c3b54b05bf77 | 147 | wait (2); |
haarkon | 0:c3b54b05bf77 | 148 | delete ptr;*/ |
haarkon | 0:c3b54b05bf77 | 149 | |
haarkon | 0:c3b54b05bf77 | 150 | pc.printf ("\n\rHelloWorld\n"); |
haarkon | 0:c3b54b05bf77 | 151 | led1 = 1; |
haarkon | 0:c3b54b05bf77 | 152 | led2 = 0; |
haarkon | 0:c3b54b05bf77 | 153 | disquette = 0; |
haarkon | 0:c3b54b05bf77 | 154 | motor.resetPosition(); |
haarkon | 0:c3b54b05bf77 | 155 | wait(3); |
haarkon | 0:c3b54b05bf77 | 156 | |
haarkon | 0:c3b54b05bf77 | 157 | pc.printf("\rcheck Pixy\n"); |
haarkon | 0:c3b54b05bf77 | 158 | if (pixy.checkPixy()) led2=1; |
haarkon | 0:c3b54b05bf77 | 159 | else { |
haarkon | 0:c3b54b05bf77 | 160 | pc.printf("\rLa pixy ne parle pas\n"); |
haarkon | 0:c3b54b05bf77 | 161 | while(1); |
haarkon | 0:c3b54b05bf77 | 162 | } |
haarkon | 0:c3b54b05bf77 | 163 | pc.printf("\rApprochez une balle\n"); |
haarkon | 0:c3b54b05bf77 | 164 | do { |
haarkon | 0:c3b54b05bf77 | 165 | pixy.detectedObject(&nbNM, &nbCC); |
haarkon | 0:c3b54b05bf77 | 166 | } while(nbNM!=1); |
haarkon | 0:c3b54b05bf77 | 167 | pc.printf("\rOK !\n"); |
haarkon | 0:c3b54b05bf77 | 168 | led1 = 0; |
haarkon | 0:c3b54b05bf77 | 169 | |
haarkon | 0:c3b54b05bf77 | 170 | pc.printf("\rApprochez une balise\n"); |
haarkon | 0:c3b54b05bf77 | 171 | do { |
haarkon | 0:c3b54b05bf77 | 172 | pixy.detectedObject(&nbNM, &nbCC); |
haarkon | 0:c3b54b05bf77 | 173 | } while(nbCC!=1); |
haarkon | 0:c3b54b05bf77 | 174 | pc.printf("\rOK !\n"); |
haarkon | 0:c3b54b05bf77 | 175 | led2 = 0; |
haarkon | 0:c3b54b05bf77 | 176 | |
haarkon | 0:c3b54b05bf77 | 177 | pc.printf("\n\rcheck Moteur\n"); |
haarkon | 0:c3b54b05bf77 | 178 | while(bp.read()); |
haarkon | 0:c3b54b05bf77 | 179 | led1 = 1; |
haarkon | 0:c3b54b05bf77 | 180 | |
haarkon | 0:c3b54b05bf77 | 181 | //Avance |
haarkon | 0:c3b54b05bf77 | 182 | motor.setSpeed (80,80); |
haarkon | 0:c3b54b05bf77 | 183 | do { |
haarkon | 0:c3b54b05bf77 | 184 | motor.getPosition (&x,&y, &theta); |
haarkon | 0:c3b54b05bf77 | 185 | } while (x<50); |
haarkon | 0:c3b54b05bf77 | 186 | motor.setSpeed (0,0); |
haarkon | 0:c3b54b05bf77 | 187 | wait_ms(10); |
haarkon | 0:c3b54b05bf77 | 188 | |
haarkon | 0:c3b54b05bf77 | 189 | //Recule |
haarkon | 0:c3b54b05bf77 | 190 | motor.setSpeed (-80,-80); |
haarkon | 0:c3b54b05bf77 | 191 | do { |
haarkon | 0:c3b54b05bf77 | 192 | motor.getPosition (&x,&y, &theta); |
haarkon | 0:c3b54b05bf77 | 193 | } while (x>5); |
haarkon | 0:c3b54b05bf77 | 194 | motor.setSpeed (0,0); |
haarkon | 0:c3b54b05bf77 | 195 | wait_ms(10); |
haarkon | 0:c3b54b05bf77 | 196 | |
haarkon | 0:c3b54b05bf77 | 197 | //Tourne à droite |
haarkon | 0:c3b54b05bf77 | 198 | motor.setSpeed (80,-80); |
haarkon | 0:c3b54b05bf77 | 199 | do { |
haarkon | 0:c3b54b05bf77 | 200 | motor.getPosition (&x,&y, &theta); |
haarkon | 0:c3b54b05bf77 | 201 | } while (theta > -PI/2.0); |
haarkon | 0:c3b54b05bf77 | 202 | |
haarkon | 0:c3b54b05bf77 | 203 | motor.setSpeed (0,0); |
haarkon | 0:c3b54b05bf77 | 204 | pc.printf("\rMoteurs OK\n"); |
haarkon | 0:c3b54b05bf77 | 205 | while (bp.read()); |
haarkon | 0:c3b54b05bf77 | 206 | |
haarkon | 0:c3b54b05bf77 | 207 | pc.printf("\rScan de la zone\n"); |
haarkon | 0:c3b54b05bf77 | 208 | |
haarkon | 0:c3b54b05bf77 | 209 | motor.setSpeed (-50,50); |
haarkon | 0:c3b54b05bf77 | 210 | do { |
haarkon | 0:c3b54b05bf77 | 211 | pixy.detectedObject(&nbNM, &nbCC); |
haarkon | 0:c3b54b05bf77 | 212 | } while (nbNM != 1); |
haarkon | 0:c3b54b05bf77 | 213 | |
haarkon | 0:c3b54b05bf77 | 214 | pc.printf("\rBalle trouvee\n"); |
haarkon | 0:c3b54b05bf77 | 215 | pc.printf("\rAlignement\n"); |
haarkon | 0:c3b54b05bf77 | 216 | |
haarkon | 0:c3b54b05bf77 | 217 | motor.setSpeed (-30,30); |
haarkon | 0:c3b54b05bf77 | 218 | do { |
haarkon | 0:c3b54b05bf77 | 219 | pixy.detectedObject(&nbNM, &nbCC); |
haarkon | 0:c3b54b05bf77 | 220 | if (nbNM==1) { |
haarkon | 0:c3b54b05bf77 | 221 | NMBuf[objet] = pixy.getNMBloc (); |
haarkon | 0:c3b54b05bf77 | 222 | nbNM--; |
haarkon | 0:c3b54b05bf77 | 223 | } |
haarkon | 0:c3b54b05bf77 | 224 | } while (NMBuf[objet].x != 160); |
haarkon | 0:c3b54b05bf77 | 225 | |
haarkon | 0:c3b54b05bf77 | 226 | pc.printf("\rBalle alignee\n"); |
haarkon | 0:c3b54b05bf77 | 227 | pc.printf("\n\rREADY TO GO\n"); |
haarkon | 0:c3b54b05bf77 | 228 | |
haarkon | 0:c3b54b05bf77 | 229 | do { |
haarkon | 0:c3b54b05bf77 | 230 | led1 = !led1; |
haarkon | 0:c3b54b05bf77 | 231 | wait(0.1); |
haarkon | 0:c3b54b05bf77 | 232 | } while (bp); |
haarkon | 0:c3b54b05bf77 | 233 | |
haarkon | 0:c3b54b05bf77 | 234 | topDepart = temps.read(); |
haarkon | 0:c3b54b05bf77 | 235 | capAuDepart = CMPS03bearing; |
haarkon | 0:c3b54b05bf77 | 236 | |
haarkon | 0:c3b54b05bf77 | 237 | while (1) { |
haarkon | 0:c3b54b05bf77 | 238 | do { |
haarkon | 0:c3b54b05bf77 | 239 | motor.setSpeed (speedG, speedD); |
haarkon | 0:c3b54b05bf77 | 240 | pixy.detectedObject(&nbNM, &nbCC); |
haarkon | 0:c3b54b05bf77 | 241 | if (nbNM == 1) { |
haarkon | 0:c3b54b05bf77 | 242 | NMBuf[0] = pixy.getNMBloc (); |
haarkon | 0:c3b54b05bf77 | 243 | nbNM--; |
haarkon | 0:c3b54b05bf77 | 244 | } |
haarkon | 0:c3b54b05bf77 | 245 | speedG = 1000 - 10*(double)(NMBuf[0].x - 160); |
haarkon | 0:c3b54b05bf77 | 246 | speedD = 1000 - 10*(double)(NMBuf[0].x + 160); |
haarkon | 0:c3b54b05bf77 | 247 | } while (NMBuf[0].y < 170); |
haarkon | 0:c3b54b05bf77 | 248 | |
haarkon | 0:c3b54b05bf77 | 249 | |
haarkon | 0:c3b54b05bf77 | 250 | |
haarkon | 0:c3b54b05bf77 | 251 | |
haarkon | 0:c3b54b05bf77 | 252 | } |
haarkon | 0:c3b54b05bf77 | 253 | } |