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?
User | Revision | Line number | New 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 | } |