Dupuis sedid / Mbed 2 deprecated 0hackton_08_06_18_publish

hachkathon

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

main.cpp@0:4effd7a50c67, 2018-12-14 (annotated)

Committer:
sedid
Date:
Fri Dec 14 12:27:28 2018 +0000
Revision:
0:4effd7a50c67
Hachkathon;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sedid 0:4effd7a50c67 1 #include "mbed.h"
sedid 0:4effd7a50c67 2 #include "CMPS03.h"
sedid 0:4effd7a50c67 3 #include "CNY70.h"
sedid 0:4effd7a50c67 4 #include "GP2A.h"
sedid 0:4effd7a50c67 5 #include "RC_Servo.h"
sedid 0:4effd7a50c67 6 #include "VMA306.h"
sedid 0:4effd7a50c67 7 #include "Pixy.h"
sedid 0:4effd7a50c67 8 #include "PID.h"
sedid 0:4effd7a50c67 9
sedid 0:4effd7a50c67 10
sedid 0:4effd7a50c67 11 #define PI 3.1415926535898
sedid 0:4effd7a50c67 12
sedid 0:4effd7a50c67 13
sedid 0:4effd7a50c67 14 #define VMOY 300
sedid 0:4effd7a50c67 15 #define VMOY2 120
sedid 0:4effd7a50c67 16 #define k 0.84
sedid 0:4effd7a50c67 17 #define ouvert 0.8
sedid 0:4effd7a50c67 18 #define fermer 1.0
sedid 0:4effd7a50c67 19
sedid 0:4effd7a50c67 20 Serial pc (PA_2, PA_3, 921600);
sedid 0:4effd7a50c67 21 PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
sedid 0:4effd7a50c67 22
sedid 0:4effd7a50c67 23 CMPS03 boussole (PC_4);
sedid 0:4effd7a50c67 24
sedid 0:4effd7a50c67 25 CNY70 ligneD (PC_3);
sedid 0:4effd7a50c67 26 CNY70 ligneG (PC_2);
sedid 0:4effd7a50c67 27 CNY70 exterior (PA_7);
sedid 0:4effd7a50c67 28
sedid 0:4effd7a50c67 29 //GP2A sd1 (PB_0, 5, 80, 10);
sedid 0:4effd7a50c67 30 AnalogIn sd1(PB_0);
sedid 0:4effd7a50c67 31
sedid 0:4effd7a50c67 32 RC_Servo ballon (PB_10, 1);
sedid 0:4effd7a50c67 33 RC_Servo verrou (PA_15, 1);
sedid 0:4effd7a50c67 34
sedid 0:4effd7a50c67 35 VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
sedid 0:4effd7a50c67 36
sedid 0:4effd7a50c67 37 PIXY Pixy (PA_0, PA_1, 115200);
sedid 0:4effd7a50c67 38
sedid 0:4effd7a50c67 39 DigitalIn bp (PC_13);
sedid 0:4effd7a50c67 40
sedid 0:4effd7a50c67 41
sedid 0:4effd7a50c67 42 DigitalOut disquette (PA_12);
sedid 0:4effd7a50c67 43
sedid 0:4effd7a50c67 44 InterruptIn button1(USER_BUTTON);
sedid 0:4effd7a50c67 45
sedid 0:4effd7a50c67 46 Timer t1,t2;
sedid 0:4effd7a50c67 47 int gietat=0;
sedid 0:4effd7a50c67 48 double gCap=0;
sedid 0:4effd7a50c67 49 double gBoussole=0;
sedid 0:4effd7a50c67 50 double distance_petit=0;
sedid 0:4effd7a50c67 51 double erreur_cap;
sedid 0:4effd7a50c67 52 long taille_balle;
sedid 0:4effd7a50c67 53 void start()
sedid 0:4effd7a50c67 54 {
sedid 0:4effd7a50c67 55 gietat=1;
sedid 0:4effd7a50c67 56 gCap=boussole.getBearing();
sedid 0:4effd7a50c67 57 t2.reset();
sedid 0:4effd7a50c67 58 }
sedid 0:4effd7a50c67 59
sedid 0:4effd7a50c67 60 int main()
sedid 0:4effd7a50c67 61 {
sedid 0:4effd7a50c67 62
sedid 0:4effd7a50c67 63 int position_balle=0;
sedid 0:4effd7a50c67 64 int nbCC, nbNM;
sedid 0:4effd7a50c67 65 T_pixyCCBloc CCBuf;
sedid 0:4effd7a50c67 66 T_pixyNMBloc NMBuf;
sedid 0:4effd7a50c67 67 disquette = 0;
sedid 0:4effd7a50c67 68 motor.resetPosition();
sedid 0:4effd7a50c67 69 verrou.write(ouvert);
sedid 0:4effd7a50c67 70 button1.fall(&start);
sedid 0:4effd7a50c67 71 t1.start();
sedid 0:4effd7a50c67 72 t2.start();
sedid 0:4effd7a50c67 73
sedid 0:4effd7a50c67 74 while(1) {
sedid 0:4effd7a50c67 75 gBoussole=boussole.getBearing();
sedid 0:4effd7a50c67 76 distance_petit=(10.00/(sd1.read()*3.30-0.50))-0.42;
sedid 0:4effd7a50c67 77 if(distance_petit>20.0)distance_petit=20;
sedid 0:4effd7a50c67 78 if(distance_petit<3.5)distance_petit=3.5;
sedid 0:4effd7a50c67 79 erreur_cap=gCap-gBoussole;
sedid 0:4effd7a50c67 80 //pc.printf("etat:%d \t erreur:%0.2f \t distance:%0.2fn\n\r",gietat,abs(erreur_cap),distance_petit);
sedid 0:4effd7a50c67 81 switch(gietat) {
sedid 0:4effd7a50c67 82 case 0:
sedid 0:4effd7a50c67 83 break;
sedid 0:4effd7a50c67 84 case 1:
sedid 0:4effd7a50c67 85 if (Pixy.checkNewImage()) {
sedid 0:4effd7a50c67 86 gietat=2;
sedid 0:4effd7a50c67 87 }
sedid 0:4effd7a50c67 88 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
sedid 0:4effd7a50c67 89 gietat=6;
sedid 0:4effd7a50c67 90 t1.reset();
sedid 0:4effd7a50c67 91 }
sedid 0:4effd7a50c67 92 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 93 break;
sedid 0:4effd7a50c67 94 case 2:
sedid 0:4effd7a50c67 95 if( distance_petit<=4.50 && distance_petit>=3.8) {
sedid 0:4effd7a50c67 96 gietat=30;
sedid 0:4effd7a50c67 97 t1.reset();
sedid 0:4effd7a50c67 98 }
sedid 0:4effd7a50c67 99 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
sedid 0:4effd7a50c67 100 gietat=6;
sedid 0:4effd7a50c67 101 t1.reset();
sedid 0:4effd7a50c67 102 }
sedid 0:4effd7a50c67 103 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 104 break;
sedid 0:4effd7a50c67 105
sedid 0:4effd7a50c67 106 case 3:
sedid 0:4effd7a50c67 107 if(abs(erreur_cap)<=7.0) {
sedid 0:4effd7a50c67 108 gietat=4;
sedid 0:4effd7a50c67 109 verrou.write(ouvert);
sedid 0:4effd7a50c67 110 }
sedid 0:4effd7a50c67 111 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
sedid 0:4effd7a50c67 112 gietat=6;
sedid 0:4effd7a50c67 113 t1.reset();
sedid 0:4effd7a50c67 114 }
sedid 0:4effd7a50c67 115 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 116 break;
sedid 0:4effd7a50c67 117 case 4:
sedid 0:4effd7a50c67 118 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
sedid 0:4effd7a50c67 119 gietat=5;
sedid 0:4effd7a50c67 120 }
sedid 0:4effd7a50c67 121 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 122 break;
sedid 0:4effd7a50c67 123 case 5:
sedid 0:4effd7a50c67 124 t1.reset();
sedid 0:4effd7a50c67 125 gietat=6;
sedid 0:4effd7a50c67 126 break;
sedid 0:4effd7a50c67 127 case 6:
sedid 0:4effd7a50c67 128 if(t1.read()>=2) {
sedid 0:4effd7a50c67 129 gietat=7;
sedid 0:4effd7a50c67 130 t1.reset();
sedid 0:4effd7a50c67 131 }
sedid 0:4effd7a50c67 132 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 133 break;
sedid 0:4effd7a50c67 134 case 7:
sedid 0:4effd7a50c67 135 if (t1.read()>1.75) {
sedid 0:4effd7a50c67 136 gietat=70;
sedid 0:4effd7a50c67 137 t1.reset();
sedid 0:4effd7a50c67 138 }
sedid 0:4effd7a50c67 139 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 140 break;
sedid 0:4effd7a50c67 141 case 70:
sedid 0:4effd7a50c67 142 if (Pixy.checkNewImage()) {
sedid 0:4effd7a50c67 143 gietat=2;
sedid 0:4effd7a50c67 144 }
sedid 0:4effd7a50c67 145 if (t1.read()>0.75) {
sedid 0:4effd7a50c67 146 gietat=71;
sedid 0:4effd7a50c67 147 t1.reset();
sedid 0:4effd7a50c67 148 }
sedid 0:4effd7a50c67 149
sedid 0:4effd7a50c67 150 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 151 break;
sedid 0:4effd7a50c67 152 case 71:
sedid 0:4effd7a50c67 153 if (Pixy.checkNewImage()) {
sedid 0:4effd7a50c67 154 gietat=2;
sedid 0:4effd7a50c67 155 }
sedid 0:4effd7a50c67 156 if (t1.read()>0.75) {
sedid 0:4effd7a50c67 157 gietat=71;
sedid 0:4effd7a50c67 158 t1.reset();
sedid 0:4effd7a50c67 159 }
sedid 0:4effd7a50c67 160 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 161 break;
sedid 0:4effd7a50c67 162 case 30:
sedid 0:4effd7a50c67 163 if(t1.read()>0.75) {
sedid 0:4effd7a50c67 164 gietat=31;
sedid 0:4effd7a50c67 165 t1.reset();
sedid 0:4effd7a50c67 166 }
sedid 0:4effd7a50c67 167 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 168 break;
sedid 0:4effd7a50c67 169 case 31:
sedid 0:4effd7a50c67 170 if(t1.read()>0.2) {
sedid 0:4effd7a50c67 171 gietat=3;
sedid 0:4effd7a50c67 172 t1.reset();
sedid 0:4effd7a50c67 173 }
sedid 0:4effd7a50c67 174 if(t2.read()>=85) gietat=8;
sedid 0:4effd7a50c67 175 break;
sedid 0:4effd7a50c67 176 }
sedid 0:4effd7a50c67 177 switch(gietat) {
sedid 0:4effd7a50c67 178 case 0:
sedid 0:4effd7a50c67 179 motor.setSpeed(0,0);
sedid 0:4effd7a50c67 180 break;
sedid 0:4effd7a50c67 181 case 1:
sedid 0:4effd7a50c67 182 motor.setSpeed(-200,200);
sedid 0:4effd7a50c67 183 break;
sedid 0:4effd7a50c67 184 case 2:
sedid 0:4effd7a50c67 185 Pixy.detectedObject (&nbNM, &nbCC);
sedid 0:4effd7a50c67 186 if (nbNM > 0) {
sedid 0:4effd7a50c67 187 NMBuf = Pixy.getNMBloc ();
sedid 0:4effd7a50c67 188 nbNM--;
sedid 0:4effd7a50c67 189 //pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
sedid 0:4effd7a50c67 190 position_balle=NMBuf.x-160;
sedid 0:4effd7a50c67 191 //pc.printf("position_balle:%d \n\r",position_balle);
sedid 0:4effd7a50c67 192 motor.setSpeed((int)((VMOY+(k*position_balle))),(int)((VMOY-(k*position_balle))));
sedid 0:4effd7a50c67 193 }
sedid 0:4effd7a50c67 194 break;
sedid 0:4effd7a50c67 195 case 3:
sedid 0:4effd7a50c67 196 motor.setSpeed(-150,150);
sedid 0:4effd7a50c67 197 verrou.write(fermer);
sedid 0:4effd7a50c67 198 break;
sedid 0:4effd7a50c67 199 case 30:
sedid 0:4effd7a50c67 200 motor.setSpeed(25,25);
sedid 0:4effd7a50c67 201 break;
sedid 0:4effd7a50c67 202 case 31:
sedid 0:4effd7a50c67 203 motor.setSpeed(25,25);
sedid 0:4effd7a50c67 204 verrou.write(fermer);
sedid 0:4effd7a50c67 205 break;
sedid 0:4effd7a50c67 206 case 4:
sedid 0:4effd7a50c67 207 motor.setSpeed(600,600);
sedid 0:4effd7a50c67 208 break;
sedid 0:4effd7a50c67 209 case 5:
sedid 0:4effd7a50c67 210 motor.setSpeed(0,0);
sedid 0:4effd7a50c67 211 break;
sedid 0:4effd7a50c67 212 case 6:
sedid 0:4effd7a50c67 213 motor.setSpeed(-150,-150);
sedid 0:4effd7a50c67 214 break;
sedid 0:4effd7a50c67 215 case 7:
sedid 0:4effd7a50c67 216 motor.setSpeed(-150,150);
sedid 0:4effd7a50c67 217 break;
sedid 0:4effd7a50c67 218 case 8:
sedid 0:4effd7a50c67 219 motor.setSpeed(0,0);
sedid 0:4effd7a50c67 220 break;
sedid 0:4effd7a50c67 221 case 70:
sedid 0:4effd7a50c67 222 motor.setSpeed(-150,150);
sedid 0:4effd7a50c67 223 break;
sedid 0:4effd7a50c67 224 case 71:
sedid 0:4effd7a50c67 225 motor.setSpeed(150,-150);
sedid 0:4effd7a50c67 226 break;
sedid 0:4effd7a50c67 227
sedid 0:4effd7a50c67 228 }
sedid 0:4effd7a50c67 229 }
sedid 0:4effd7a50c67 230 }