FRC - Hackathon / Mbed 2 deprecated FRC2018_Bis

Programme de test avec librairies minimales et un seul ticker

Dependencies:   PID Pixy mbed

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?

UserRevisionLine numberNew 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 }