Alexandre Pirotte / Mbed 2 deprecated 0concours_cachan_programme_ok

ok

Dependencies:   mbed

Fork of _test_suivi_mur by christophe vermaelen

main.cpp@7:2f4660e9cf92, 2017-06-23 (annotated)

Committer:
pirottealex
Date:
Fri Jun 23 11:19:58 2017 +0000
Revision:
7:2f4660e9cf92
Parent:
6:5f7df5c74a77 
pixy_ok2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 0:dcb865a03d57 1 #include "mbed.h"
vermaelen 2:82b72fa8dbcd 2 #include "fct.h"
pirottealex 7:2f4660e9cf92 3 #include "Pixy.h"
pirottealex 7:2f4660e9cf92 4 #include "PixyLink.h"
pirottealex 7:2f4660e9cf92 5 #include "CMPS03.h"
pirottealex 7:2f4660e9cf92 6
pirottealex 7:2f4660e9cf92 7
pirottealex 7:2f4660e9cf92 8 Serial pc(USBTX,USBRX);
pirottealex 7:2f4660e9cf92 9 CMPS03 compass(p9, p10, CMPS03_DEFAULT_I2C_ADDRESS);
vermaelen 0:dcb865a03d57 10
vermaelen 0:dcb865a03d57 11 int main()
vermaelen 0:dcb865a03d57 12 {
pirottealex 7:2f4660e9cf92 13
vermaelen 2:82b72fa8dbcd 14 init();
pirottealex 7:2f4660e9cf92 15 Pixy pixy = Pixy(p28,p27);
pirottealex 7:2f4660e9cf92 16 pixy.setSerialOutput(&pc);
pirottealex 7:2f4660e9cf92 17 int tempo=0;
pirottealex 7:2f4660e9cf92 18 int i=0;
pirottealex 7:2f4660e9cf92 19 while(true) {
pirottealex 7:2f4660e9cf92 20 //capg=capb1.read();
pirottealex 7:2f4660e9cf92 21 //capd=capb2.read();
pirottealex 7:2f4660e9cf92 22 capg=0;
pirottealex 7:2f4660e9cf92 23 capd=0;
pirottealex 7:2f4660e9cf92 24
pirottealex 7:2f4660e9cf92 25 //i++;pc.printf("%d\n\r",i);
pirottealex 7:2f4660e9cf92 26 jck=jack.read();
pirottealex 7:2f4660e9cf92 27 leds.write(etat);
pirottealex 7:2f4660e9cf92 28 //pc.printf("g=%d,d=%d\n\r",capd,capg);
pirottealex 7:2f4660e9cf92 29 //pc.printf("etat=%d,taille=%f,capt_eg=%3.0f,capt_g=%3.0f,capt_m=%3.0f,capt_d=%3.0f,capt_ed=%3.0f\n\r",etat,taille,capt_eg,capt_g,capt_m,capt_d,capt_ed);
pirottealex 7:2f4660e9cf92 30 //pc.printf("etat=%d, cap_lum=%f, cap=%f, block=%d,taille=%f\n\r",etat,cap_lum,cap,blocks,taille);
pirottealex 7:2f4660e9cf92 31 cap=compass.readBearing()/10.0;
pirottealex 7:2f4660e9cf92 32 blocks = pixy.getBlocks();
pirottealex 7:2f4660e9cf92 33 if (blocks) {
pirottealex 7:2f4660e9cf92 34 taille=pixy.blocks[j].width*pixy.blocks[j].height;
pirottealex 7:2f4660e9cf92 35 errorX = (160-pixy.blocks[j].x);
pirottealex 7:2f4660e9cf92 36 errorY = (100-pixy.blocks[j].y);
pirottealex 7:2f4660e9cf92 37 } /*else if(!blocks) {
pirottealex 7:2f4660e9cf92 38 blocks = pixy.getBlocks();
pirottealex 7:2f4660e9cf92 39 taille=pixy.blocks[j].width*pixy.blocks[j].height;
pirottealex 7:2f4660e9cf92 40 errorX = (160-pixy.blocks[j].x);
pirottealex 7:2f4660e9cf92 41 errorY = (100-pixy.blocks[j].y);
pirottealex 7:2f4660e9cf92 42 }*/
pirottealex 7:2f4660e9cf92 43 if(tr.read()>10) {
pirottealex 7:2f4660e9cf92 44 tempo=1;
pirottealex 7:2f4660e9cf92 45 }
pirottealex 7:2f4660e9cf92 46
pirottealex 7:2f4660e9cf92 47 //pc.printf("taille=%f\n\r",taille);
vermaelen 2:82b72fa8dbcd 48 switch(etat) {
pirottealex 7:2f4660e9cf92 49 case(0) :
pirottealex 7:2f4660e9cf92 50 if(jck==1) {
vermaelen 2:82b72fa8dbcd 51 etat=1;
pirottealex 7:2f4660e9cf92 52 cap_lum=cap;
pirottealex 7:2f4660e9cf92 53 tr.stop();
pirottealex 7:2f4660e9cf92 54 tr.reset();
pirottealex 7:2f4660e9cf92 55 tr.start();
pirottealex 7:2f4660e9cf92 56 }
pirottealex 7:2f4660e9cf92 57 break;
pirottealex 7:2f4660e9cf92 58
pirottealex 7:2f4660e9cf92 59 case(1) :
pirottealex 7:2f4660e9cf92 60
pirottealex 7:2f4660e9cf92 61 cap_lum=cap;
pirottealex 7:2f4660e9cf92 62 if(!blocks) {
pirottealex 7:2f4660e9cf92 63 etat=3;
pirottealex 7:2f4660e9cf92 64 }
pirottealex 7:2f4660e9cf92 65 if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
pirottealex 7:2f4660e9cf92 66 etat=2;
vermaelen 2:82b72fa8dbcd 67 }
pirottealex 7:2f4660e9cf92 68
pirottealex 7:2f4660e9cf92 69
pirottealex 7:2f4660e9cf92 70 if((taille>seuil_taille)&&(tempo==1)) {
pirottealex 7:2f4660e9cf92 71 tb.reset();
pirottealex 7:2f4660e9cf92 72 tb.start();
pirottealex 7:2f4660e9cf92 73 etat=100;
pirottealex 7:2f4660e9cf92 74 }
pirottealex 7:2f4660e9cf92 75 break;
pirottealex 7:2f4660e9cf92 76
pirottealex 7:2f4660e9cf92 77 case(2) :
pirottealex 7:2f4660e9cf92 78 if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(blocks)) {
pirottealex 7:2f4660e9cf92 79 etat=1;
pirottealex 7:2f4660e9cf92 80 }
pirottealex 7:2f4660e9cf92 81 if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(!blocks)) {
pirottealex 7:2f4660e9cf92 82
pirottealex 7:2f4660e9cf92 83 etat=3;
pirottealex 7:2f4660e9cf92 84 }
pirottealex 7:2f4660e9cf92 85
pirottealex 7:2f4660e9cf92 86 if((taille>seuil_taille)&&(tempo==1)) {
pirottealex 7:2f4660e9cf92 87 tb.reset();
pirottealex 7:2f4660e9cf92 88 tb.start();
pirottealex 7:2f4660e9cf92 89 etat=100;
vermaelen 3:b91371837109 90 }
vermaelen 2:82b72fa8dbcd 91 break;
pirottealex 7:2f4660e9cf92 92
pirottealex 7:2f4660e9cf92 93
pirottealex 7:2f4660e9cf92 94 case(3) :
pirottealex 7:2f4660e9cf92 95 if(cap_lum<180) {
pirottealex 7:2f4660e9cf92 96 if(cap<=cap_lum+180 && cap>cap_lum) {
pirottealex 7:2f4660e9cf92 97 etat=5;
pirottealex 7:2f4660e9cf92 98 } else {
pirottealex 7:2f4660e9cf92 99 etat=4;
pirottealex 7:2f4660e9cf92 100 }
pirottealex 7:2f4660e9cf92 101 } else {
pirottealex 7:2f4660e9cf92 102 if(cap<=cap_lum && cap>cap_lum-180) {
pirottealex 7:2f4660e9cf92 103 etat=4;
pirottealex 7:2f4660e9cf92 104 }
pirottealex 7:2f4660e9cf92 105
pirottealex 7:2f4660e9cf92 106 else {
pirottealex 7:2f4660e9cf92 107 etat=5;
pirottealex 7:2f4660e9cf92 108 }
pirottealex 7:2f4660e9cf92 109 }
pirottealex 7:2f4660e9cf92 110
pirottealex 7:2f4660e9cf92 111 if((taille>seuil_taille)&&(tempo==1)) {
pirottealex 7:2f4660e9cf92 112 tb.reset();
pirottealex 7:2f4660e9cf92 113 tb.start();
pirottealex 7:2f4660e9cf92 114 etat=100;
pirottealex 7:2f4660e9cf92 115 }
pirottealex 7:2f4660e9cf92 116 break;
pirottealex 7:2f4660e9cf92 117
pirottealex 7:2f4660e9cf92 118 case(4) : //pc.printf("tourner a gauche");
pirottealex 7:2f4660e9cf92 119
pirottealex 7:2f4660e9cf92 120 if (blocks) {
pirottealex 7:2f4660e9cf92 121 etat=1;
pirottealex 7:2f4660e9cf92 122 }
pirottealex 7:2f4660e9cf92 123 if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
vermaelen 4:78a9354fcee8 124 etat=2;
pirottealex 7:2f4660e9cf92 125 }
pirottealex 7:2f4660e9cf92 126
pirottealex 7:2f4660e9cf92 127 if((taille>seuil_taille)&&(tempo==1)) {
pirottealex 7:2f4660e9cf92 128 tb.reset();
pirottealex 7:2f4660e9cf92 129 tb.start();
pirottealex 7:2f4660e9cf92 130 etat=100;
vermaelen 4:78a9354fcee8 131 }
pirottealex 7:2f4660e9cf92 132 break;
pirottealex 7:2f4660e9cf92 133
pirottealex 7:2f4660e9cf92 134 case(5) :
pirottealex 7:2f4660e9cf92 135
pirottealex 7:2f4660e9cf92 136 //pc.printf("touner a droite");
pirottealex 7:2f4660e9cf92 137 if (blocks) {
pirottealex 7:2f4660e9cf92 138 etat=1;
pirottealex 7:2f4660e9cf92 139 }
pirottealex 7:2f4660e9cf92 140 if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
pirottealex 7:2f4660e9cf92 141 etat=2;
pirottealex 7:2f4660e9cf92 142 }
pirottealex 7:2f4660e9cf92 143
pirottealex 7:2f4660e9cf92 144 if((taille>seuil_taille)&&(tempo==1)) {
pirottealex 7:2f4660e9cf92 145 tb.reset();
pirottealex 7:2f4660e9cf92 146 tb.start();
pirottealex 7:2f4660e9cf92 147 etat=100;
pirottealex 7:2f4660e9cf92 148 }
pirottealex 7:2f4660e9cf92 149 break;
pirottealex 7:2f4660e9cf92 150
pirottealex 7:2f4660e9cf92 151 case(10) :
pirottealex 7:2f4660e9cf92 152
pirottealex 7:2f4660e9cf92 153 if(capt_m>=40) {
pirottealex 7:2f4660e9cf92 154 etat=2;
pirottealex 7:2f4660e9cf92 155 }
pirottealex 7:2f4660e9cf92 156 break;
pirottealex 7:2f4660e9cf92 157
pirottealex 7:2f4660e9cf92 158
pirottealex 7:2f4660e9cf92 159 case(100) :
pirottealex 7:2f4660e9cf92 160
pirottealex 7:2f4660e9cf92 161 if((capt_m<30) || (capt_g<25)) {
pirottealex 7:2f4660e9cf92 162 etat=101;
pirottealex 7:2f4660e9cf92 163 t.stop();
pirottealex 7:2f4660e9cf92 164 t.reset();
pirottealex 7:2f4660e9cf92 165 t.start();
pirottealex 7:2f4660e9cf92 166 }
pirottealex 7:2f4660e9cf92 167 break;
pirottealex 7:2f4660e9cf92 168
pirottealex 7:2f4660e9cf92 169 case(101) :
pirottealex 7:2f4660e9cf92 170 if(t.read()<10) {
pirottealex 7:2f4660e9cf92 171 servo_start();
pirottealex 7:2f4660e9cf92 172 } else if(t.read()>=10) {
pirottealex 7:2f4660e9cf92 173 servo_stop();
vermaelen 2:82b72fa8dbcd 174 }
vermaelen 4:78a9354fcee8 175
vermaelen 2:82b72fa8dbcd 176 break;
pirottealex 7:2f4660e9cf92 177
pirottealex 7:2f4660e9cf92 178
vermaelen 2:82b72fa8dbcd 179 }
vermaelen 2:82b72fa8dbcd 180 switch(etat) {
pirottealex 7:2f4660e9cf92 181 case(0) :
pirottealex 7:2f4660e9cf92 182 vitesse(0,0);
pirottealex 7:2f4660e9cf92 183 break;
pirottealex 7:2f4660e9cf92 184
pirottealex 7:2f4660e9cf92 185 case(1) :
pirottealex 7:2f4660e9cf92 186 vitesse(V_max-K*errorX,V_max+K*errorX);
vermaelen 2:82b72fa8dbcd 187 break;
pirottealex 7:2f4660e9cf92 188 case(2) :
pirottealex 7:2f4660e9cf92 189 erreur_cap=(cap_lum+(80-(capt_eg+capt_g+capt_m)/3)*Kdist-(80-(capt_ed+capt_d)/2)*Kdist)-cap;
pirottealex 7:2f4660e9cf92 190 if(erreur_cap>180) {
pirottealex 7:2f4660e9cf92 191 erreur_cap=erreur_cap-360;
pirottealex 7:2f4660e9cf92 192 }
pirottealex 7:2f4660e9cf92 193 if(erreur_cap<-180) {
pirottealex 7:2f4660e9cf92 194 erreur_cap=erreur_cap+360;
pirottealex 7:2f4660e9cf92 195 }
pirottealex 7:2f4660e9cf92 196 if(erreur_cap>0) {
pirottealex 7:2f4660e9cf92 197 cmdD=V_max-Kcap*erreur_cap;
pirottealex 7:2f4660e9cf92 198 cmdG=V_max;
pirottealex 7:2f4660e9cf92 199 } else {
pirottealex 7:2f4660e9cf92 200 cmdD=V_max;
pirottealex 7:2f4660e9cf92 201 cmdG=V_max+Kcap*erreur_cap;
pirottealex 7:2f4660e9cf92 202 }
pirottealex 7:2f4660e9cf92 203 if(cmdD<0) {
pirottealex 7:2f4660e9cf92 204 cmdD=0;
pirottealex 7:2f4660e9cf92 205 }
pirottealex 7:2f4660e9cf92 206 if(cmdG<0) {
pirottealex 7:2f4660e9cf92 207 cmdG=0;
pirottealex 7:2f4660e9cf92 208 }
pirottealex 7:2f4660e9cf92 209 vitesse(cmdG,cmdD);
pirottealex 7:2f4660e9cf92 210 break;
pirottealex 7:2f4660e9cf92 211 case(3) :
vermaelen 2:82b72fa8dbcd 212 break;
pirottealex 7:2f4660e9cf92 213
pirottealex 7:2f4660e9cf92 214 case(4) :
pirottealex 7:2f4660e9cf92 215 vitesse(V_moy,lent);
pirottealex 7:2f4660e9cf92 216 break;
pirottealex 7:2f4660e9cf92 217
pirottealex 7:2f4660e9cf92 218 case(5) :
pirottealex 7:2f4660e9cf92 219 vitesse(lent,V_moy);
pirottealex 7:2f4660e9cf92 220 break;
pirottealex 7:2f4660e9cf92 221
pirottealex 7:2f4660e9cf92 222 case(10):
pirottealex 7:2f4660e9cf92 223 vitesse(lent,lent);
vermaelen 3:b91371837109 224 break;
pirottealex 7:2f4660e9cf92 225
pirottealex 7:2f4660e9cf92 226 case(100) :
pirottealex 7:2f4660e9cf92 227
pirottealex 7:2f4660e9cf92 228 vitesse(V_max-K*errorX,V_max+K*errorX);
pirottealex 7:2f4660e9cf92 229 break;
pirottealex 7:2f4660e9cf92 230
pirottealex 7:2f4660e9cf92 231 case(101) :
pirottealex 7:2f4660e9cf92 232
pirottealex 7:2f4660e9cf92 233 vitesse(0,0);
pirottealex 7:2f4660e9cf92 234 break;
pirottealex 7:2f4660e9cf92 235 case(111) :
pirottealex 7:2f4660e9cf92 236 vitesse(-30,0);
pirottealex 7:2f4660e9cf92 237 break;
pirottealex 7:2f4660e9cf92 238
pirottealex 7:2f4660e9cf92 239 case(112) :
pirottealex 7:2f4660e9cf92 240 vitesse(0,-30);
pirottealex 7:2f4660e9cf92 241 break;
pirottealex 7:2f4660e9cf92 242
vermaelen 2:82b72fa8dbcd 243 }
vermaelen 0:dcb865a03d57 244 }
vermaelen 0:dcb865a03d57 245 }