joey QUANTIN / Mbed 2 deprecated 0000Non_stop_code_v3_copy

pixy info2

Dependencies:   CMPS03 SRF05 mbed pixy

Fork of 0000Non_stop_code_v3 by Alexandre Pirotte

main.cpp@2:48f7e5f79caa, 2017-06-22 (annotated)

Committer:
qj604184
Date:
Thu Jun 22 09:32:24 2017 +0000
Revision:
2:48f7e5f79caa
Parent:
1:99f469d63b9a 
pixy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pirottealex 0:a8cee96c9250 1 #include "mbed.h"
pirottealex 0:a8cee96c9250 2 #include "fct.h"
pirottealex 0:a8cee96c9250 3 #include "Pixy.h"
pirottealex 0:a8cee96c9250 4 #include "PixyLink.h"
pirottealex 0:a8cee96c9250 5 #include "SRF05.h"
pirottealex 0:a8cee96c9250 6 #include <CMPS03.h>
pirottealex 0:a8cee96c9250 7
pirottealex 0:a8cee96c9250 8 Serial pc (USBTX,USBRX);
pirottealex 0:a8cee96c9250 9 int main()
pirottealex 0:a8cee96c9250 10 {
qj604184 2:48f7e5f79caa 11 float infra;
pirottealex 0:a8cee96c9250 12 init();
pirottealex 0:a8cee96c9250 13 Pixy pixy= Pixy(p28,p27);
pirottealex 0:a8cee96c9250 14 pixy.setSerialOutput(&pc);
pirottealex 0:a8cee96c9250 15 pc.printf("alive");
pirottealex 0:a8cee96c9250 16
pirottealex 0:a8cee96c9250 17 while(1) {
qj604184 2:48f7e5f79caa 18 infra=1/(((SD_2.read()*3.3)-0.12766)/11.489);//valeur en cm
qj604184 2:48f7e5f79caa 19 if(infra<0||infra>25) {
qj604184 2:48f7e5f79caa 20 infra=25.0;
qj604184 2:48f7e5f79caa 21 }
pirottealex 0:a8cee96c9250 22 lecture_blanc(); // recupere valeurs capteurs ligne blanche
pirottealex 0:a8cee96c9250 23 lecture_us(); // recupere valeurs capteurs us
pirottealex 0:a8cee96c9250 24 lecture_boussole(); //gBoussole
pirottealex 0:a8cee96c9250 25 bout=bp.read();
pirottealex 0:a8cee96c9250 26 blocks = pixy.getBlocks();
qj604184 2:48f7e5f79caa 27 //pc.printf("infra : %f\n\r",infra);
qj604184 2:48f7e5f79caa 28 //pc.printf("us gauche= %f us droit= %f\n\r",us_gauche,us_droit);
qj604184 2:48f7e5f79caa 29 //wait(0.5);
pirottealex 0:a8cee96c9250 30 //pc.printf("etat=%d\n\r",etat);
pirottealex 0:a8cee96c9250 31 //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole);
pirottealex 0:a8cee96c9250 32 if (blocks) {
pirottealex 0:a8cee96c9250 33 taille=pixy.blocks[j].width*pixy.blocks[j].height;
pirottealex 0:a8cee96c9250 34 errorX = (160-pixy.blocks[j].x);
pirottealex 1:99f469d63b9a 35 //pc.printf("taille=%f,sig: %d x: %d y: %d width: %d height: %d\n\r",taille, pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
pirottealex 0:a8cee96c9250 36 //pc.printf("OK");
pirottealex 0:a8cee96c9250 37 }
qj604184 2:48f7e5f79caa 38
qj604184 2:48f7e5f79caa 39 switch(etat) {
pirottealex 0:a8cee96c9250 40 case(0) :
qj604184 2:48f7e5f79caa 41
qj604184 2:48f7e5f79caa 42 if(bout==0) {
pirottealex 0:a8cee96c9250 43 etat=100;
pirottealex 0:a8cee96c9250 44 cap_set=gBoussole;
pirottealex 0:a8cee96c9250 45 }
qj604184 2:48f7e5f79caa 46 if((captL1>=1)||(captL3>=1)) {
pirottealex 1:99f469d63b9a 47 etat=3;
pirottealex 1:99f469d63b9a 48 }
pirottealex 0:a8cee96c9250 49 break;
qj604184 2:48f7e5f79caa 50
pirottealex 0:a8cee96c9250 51 case(100):
qj604184 2:48f7e5f79caa 52
qj604184 2:48f7e5f79caa 53 if((blocks)&&(taille>50)) {
pirottealex 0:a8cee96c9250 54 etat=1;
pirottealex 0:a8cee96c9250 55 }
qj604184 2:48f7e5f79caa 56
qj604184 2:48f7e5f79caa 57 else {
pirottealex 0:a8cee96c9250 58 etat=21;
pirottealex 0:a8cee96c9250 59 }
qj604184 2:48f7e5f79caa 60 if((captL1>=1)||(captL3>=1)) {
pirottealex 1:99f469d63b9a 61 etat=3;
pirottealex 1:99f469d63b9a 62 }
pirottealex 0:a8cee96c9250 63 break;
qj604184 2:48f7e5f79caa 64 case(200):
qj604184 2:48f7e5f79caa 65 if((blocks)&&(taille>50)) {
qj604184 2:48f7e5f79caa 66 etat=1;
qj604184 2:48f7e5f79caa 67 }
qj604184 2:48f7e5f79caa 68
qj604184 2:48f7e5f79caa 69 else {
qj604184 2:48f7e5f79caa 70 etat=21;
qj604184 2:48f7e5f79caa 71 }
qj604184 2:48f7e5f79caa 72 if((captL1>=1)||(captL3>=1)) {
qj604184 2:48f7e5f79caa 73 etat=3;
pirottealex 1:99f469d63b9a 74 }
qj604184 2:48f7e5f79caa 75 break;
qj604184 2:48f7e5f79caa 76
qj604184 2:48f7e5f79caa 77 case(1) :
qj604184 2:48f7e5f79caa 78
qj604184 2:48f7e5f79caa 79 if(infra<20) {
qj604184 2:48f7e5f79caa 80
qj604184 2:48f7e5f79caa 81
pirottealex 0:a8cee96c9250 82 etat=2;
pirottealex 1:99f469d63b9a 83 }
qj604184 2:48f7e5f79caa 84
qj604184 2:48f7e5f79caa 85 if((captL1>=1)||(captL3>=1)) {
pirottealex 1:99f469d63b9a 86 etat=3;
pirottealex 1:99f469d63b9a 87 flag=0;
pirottealex 0:a8cee96c9250 88 }
pirottealex 0:a8cee96c9250 89 break;
pirottealex 0:a8cee96c9250 90 case(2) :
qj604184 2:48f7e5f79caa 91
qj604184 2:48f7e5f79caa 92 if(infra<5) {
pirottealex 0:a8cee96c9250 93 etat=3;
pirottealex 0:a8cee96c9250 94 }
qj604184 2:48f7e5f79caa 95 if(us_gauche<15 || us_droit<15){
qj604184 2:48f7e5f79caa 96 etat=200;
qj604184 2:48f7e5f79caa 97 }
qj604184 2:48f7e5f79caa 98 if((captL1>=1)||(captL3>=1)) {
pirottealex 1:99f469d63b9a 99 etat=3;
pirottealex 1:99f469d63b9a 100 }
pirottealex 0:a8cee96c9250 101 break;
pirottealex 0:a8cee96c9250 102 case(3) :
qj604184 2:48f7e5f79caa 103
qj604184 2:48f7e5f79caa 104 if(us_arriere<25) {
pirottealex 0:a8cee96c9250 105 etat=100;
pirottealex 0:a8cee96c9250 106 }
pirottealex 0:a8cee96c9250 107 break;
pirottealex 0:a8cee96c9250 108 case(21) :
pirottealex 0:a8cee96c9250 109 cap_rot=cap_set+30;
qj604184 2:48f7e5f79caa 110 if(cap_rot>360) {
pirottealex 0:a8cee96c9250 111 cap_rot=cap_rot-360;
pirottealex 0:a8cee96c9250 112 }
qj604184 2:48f7e5f79caa 113
qj604184 2:48f7e5f79caa 114 if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180)) {
pirottealex 0:a8cee96c9250 115 etat=22;
pirottealex 0:a8cee96c9250 116 }
qj604184 2:48f7e5f79caa 117 if(blocks) {
pirottealex 0:a8cee96c9250 118 etat=1;
pirottealex 0:a8cee96c9250 119 }
pirottealex 0:a8cee96c9250 120 break;
pirottealex 0:a8cee96c9250 121 case(22) :
pirottealex 0:a8cee96c9250 122 cap_rot=cap_set-30;
qj604184 2:48f7e5f79caa 123 if(cap_rot<0) {
pirottealex 0:a8cee96c9250 124 cap_rot=cap_rot+360;
pirottealex 0:a8cee96c9250 125 }
qj604184 2:48f7e5f79caa 126
qj604184 2:48f7e5f79caa 127 if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180)) {
pirottealex 0:a8cee96c9250 128 etat=21;
pirottealex 0:a8cee96c9250 129 }
qj604184 2:48f7e5f79caa 130 if(blocks) {
pirottealex 0:a8cee96c9250 131 etat=1;
pirottealex 0:a8cee96c9250 132 }
pirottealex 0:a8cee96c9250 133 break;
qj604184 2:48f7e5f79caa 134
qj604184 2:48f7e5f79caa 135 }
qj604184 2:48f7e5f79caa 136
qj604184 2:48f7e5f79caa 137 switch(etat) {
qj604184 2:48f7e5f79caa 138 case(0) :
qj604184 2:48f7e5f79caa 139 vitmoteur(0,0);
qj604184 2:48f7e5f79caa 140 break;
qj604184 2:48f7e5f79caa 141
qj604184 2:48f7e5f79caa 142 case(1) :
qj604184 2:48f7e5f79caa 143 vitmoteur(V_max-K*errorX,V_max+K*errorX);
qj604184 2:48f7e5f79caa 144 break;
qj604184 2:48f7e5f79caa 145
qj604184 2:48f7e5f79caa 146 case(2) :
qj604184 2:48f7e5f79caa 147 cap_avant=cap_set;
qj604184 2:48f7e5f79caa 148 erreur_av=cap_avant-gBoussole;
qj604184 2:48f7e5f79caa 149
qj604184 2:48f7e5f79caa 150 if(erreur_av>20) {
qj604184 2:48f7e5f79caa 151 erreur_av=20;
qj604184 2:48f7e5f79caa 152 }
qj604184 2:48f7e5f79caa 153 if(erreur_av<-20) {
qj604184 2:48f7e5f79caa 154 erreur_av=-20;
qj604184 2:48f7e5f79caa 155 }
qj604184 2:48f7e5f79caa 156 vitmoteur(V_max*1.5+erreur_av*0.2,V_max*1.5-erreur_av*0.2);
qj604184 2:48f7e5f79caa 157 //vitmoteur(V_max*1.5,V_max*1.5);
qj604184 2:48f7e5f79caa 158 break;
qj604184 2:48f7e5f79caa 159
qj604184 2:48f7e5f79caa 160 case(3) :
qj604184 2:48f7e5f79caa 161
qj604184 2:48f7e5f79caa 162 cap_arriere=cap_set;
qj604184 2:48f7e5f79caa 163 erreur_ar=cap_arriere-gBoussole;
qj604184 2:48f7e5f79caa 164
qj604184 2:48f7e5f79caa 165 if(erreur_ar>20) {
qj604184 2:48f7e5f79caa 166 erreur_ar=20;
qj604184 2:48f7e5f79caa 167 }
qj604184 2:48f7e5f79caa 168 if(erreur_ar<-20) {
qj604184 2:48f7e5f79caa 169 erreur_ar=-20;
qj604184 2:48f7e5f79caa 170 }
qj604184 2:48f7e5f79caa 171 vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);
qj604184 2:48f7e5f79caa 172
qj604184 2:48f7e5f79caa 173 break;
qj604184 2:48f7e5f79caa 174
qj604184 2:48f7e5f79caa 175 case(21) :
qj604184 2:48f7e5f79caa 176 vitmoteur(V_recherche,-V_recherche);
qj604184 2:48f7e5f79caa 177 break;
qj604184 2:48f7e5f79caa 178
qj604184 2:48f7e5f79caa 179 case(22) :
qj604184 2:48f7e5f79caa 180 vitmoteur(-V_recherche,V_recherche);
qj604184 2:48f7e5f79caa 181 break;
qj604184 2:48f7e5f79caa 182
qj604184 2:48f7e5f79caa 183 case(100) :
qj604184 2:48f7e5f79caa 184 break;
qj604184 2:48f7e5f79caa 185
qj604184 2:48f7e5f79caa 186 case(200):
qj604184 2:48f7e5f79caa 187 vitmoteur(0,20);
qj604184 2:48f7e5f79caa 188 wait(2);
qj604184 2:48f7e5f79caa 189 vitmoteur(20,20);
qj604184 2:48f7e5f79caa 190 wait(2);
qj604184 2:48f7e5f79caa 191 break;
pirottealex 0:a8cee96c9250 192
pirottealex 0:a8cee96c9250 193 }
qj604184 2:48f7e5f79caa 194
pirottealex 0:a8cee96c9250 195 }
pirottealex 0:a8cee96c9250 196 }
qj604184 2:48f7e5f79caa 197
pirottealex 0:a8cee96c9250 198
qj604184 2:48f7e5f79caa 199