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@1:99f469d63b9a, 2017-06-10 (annotated)

Committer:
pirottealex
Date:
Sat Jun 10 07:20:21 2017 +0000
Revision:
1:99f469d63b9a
Parent:
0:a8cee96c9250
Child:
2:48f7e5f79caa
llllllllllllllllllllllllllll;

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 {
pirottealex 0:a8cee96c9250 11 init();
pirottealex 0:a8cee96c9250 12 Pixy pixy= Pixy(p28,p27);
pirottealex 0:a8cee96c9250 13 pixy.setSerialOutput(&pc);
pirottealex 0:a8cee96c9250 14 pc.printf("alive");
pirottealex 0:a8cee96c9250 15
pirottealex 0:a8cee96c9250 16 while(1) {
pirottealex 0:a8cee96c9250 17 lecture_blanc(); // recupere valeurs capteurs ligne blanche
pirottealex 0:a8cee96c9250 18 lecture_us(); // recupere valeurs capteurs us
pirottealex 0:a8cee96c9250 19 lecture_boussole(); //gBoussole
pirottealex 0:a8cee96c9250 20 bout=bp.read();
pirottealex 0:a8cee96c9250 21 blocks = pixy.getBlocks();
pirottealex 0:a8cee96c9250 22 //pc.printf("etat=%d\n\r",etat);
pirottealex 0:a8cee96c9250 23 //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 24 if (blocks) {
pirottealex 0:a8cee96c9250 25 taille=pixy.blocks[j].width*pixy.blocks[j].height;
pirottealex 0:a8cee96c9250 26 errorX = (160-pixy.blocks[j].x);
pirottealex 1:99f469d63b9a 27 //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 28 //pc.printf("OK");
pirottealex 0:a8cee96c9250 29 }
pirottealex 1:99f469d63b9a 30
pirottealex 0:a8cee96c9250 31 switch(etat)
pirottealex 0:a8cee96c9250 32 {
pirottealex 0:a8cee96c9250 33 case(0) :
pirottealex 0:a8cee96c9250 34
pirottealex 0:a8cee96c9250 35 if(bout==0)
pirottealex 0:a8cee96c9250 36 {
pirottealex 0:a8cee96c9250 37 etat=100;
pirottealex 0:a8cee96c9250 38 cap_set=gBoussole;
pirottealex 0:a8cee96c9250 39 }
pirottealex 1:99f469d63b9a 40 if((captL1>=1)||(captL3>=1))
pirottealex 1:99f469d63b9a 41 {
pirottealex 1:99f469d63b9a 42 etat=3;
pirottealex 1:99f469d63b9a 43 }
pirottealex 0:a8cee96c9250 44 break;
pirottealex 0:a8cee96c9250 45
pirottealex 0:a8cee96c9250 46 case(100):
pirottealex 0:a8cee96c9250 47
pirottealex 1:99f469d63b9a 48 if((blocks)&&(taille>50))
pirottealex 0:a8cee96c9250 49 {
pirottealex 0:a8cee96c9250 50 etat=1;
pirottealex 0:a8cee96c9250 51 }
pirottealex 0:a8cee96c9250 52
pirottealex 0:a8cee96c9250 53 else
pirottealex 0:a8cee96c9250 54 {
pirottealex 0:a8cee96c9250 55 etat=21;
pirottealex 0:a8cee96c9250 56 }
pirottealex 1:99f469d63b9a 57 if((captL1>=1)||(captL3>=1))
pirottealex 1:99f469d63b9a 58 {
pirottealex 1:99f469d63b9a 59 etat=3;
pirottealex 1:99f469d63b9a 60 }
pirottealex 0:a8cee96c9250 61 break;
pirottealex 0:a8cee96c9250 62
pirottealex 0:a8cee96c9250 63 case(1) :
pirottealex 0:a8cee96c9250 64
pirottealex 0:a8cee96c9250 65 if(!blocks)
pirottealex 0:a8cee96c9250 66 {
pirottealex 1:99f469d63b9a 67 flag++;
pirottealex 1:99f469d63b9a 68 }
pirottealex 1:99f469d63b9a 69 if(flag>40)
pirottealex 1:99f469d63b9a 70 {
pirottealex 0:a8cee96c9250 71 etat=2;
pirottealex 0:a8cee96c9250 72 tempo.stop();
pirottealex 0:a8cee96c9250 73 tempo.reset();
pirottealex 0:a8cee96c9250 74 tempo.start();
pirottealex 1:99f469d63b9a 75 flag=0;
pirottealex 1:99f469d63b9a 76 }
pirottealex 1:99f469d63b9a 77
pirottealex 1:99f469d63b9a 78 if((captL1>=1)||(captL3>=1))
pirottealex 1:99f469d63b9a 79 {
pirottealex 1:99f469d63b9a 80 etat=3;
pirottealex 1:99f469d63b9a 81 flag=0;
pirottealex 0:a8cee96c9250 82 }
pirottealex 0:a8cee96c9250 83 break;
pirottealex 0:a8cee96c9250 84 case(2) :
pirottealex 0:a8cee96c9250 85
pirottealex 0:a8cee96c9250 86 if(tempo.read()>0.5)
pirottealex 0:a8cee96c9250 87 {
pirottealex 0:a8cee96c9250 88 etat=3;
pirottealex 0:a8cee96c9250 89 }
pirottealex 1:99f469d63b9a 90 if((captL1>=1)||(captL3>=1))
pirottealex 1:99f469d63b9a 91 {
pirottealex 1:99f469d63b9a 92 etat=3;
pirottealex 1:99f469d63b9a 93 }
pirottealex 0:a8cee96c9250 94 break;
pirottealex 0:a8cee96c9250 95 case(3) :
pirottealex 0:a8cee96c9250 96
pirottealex 0:a8cee96c9250 97 if(us_arriere<25)
pirottealex 0:a8cee96c9250 98 {
pirottealex 0:a8cee96c9250 99 etat=100;
pirottealex 0:a8cee96c9250 100 }
pirottealex 0:a8cee96c9250 101 break;
pirottealex 0:a8cee96c9250 102 case(21) :
pirottealex 0:a8cee96c9250 103 cap_rot=cap_set+30;
pirottealex 0:a8cee96c9250 104 if(cap_rot>360)
pirottealex 0:a8cee96c9250 105 {
pirottealex 0:a8cee96c9250 106 cap_rot=cap_rot-360;
pirottealex 0:a8cee96c9250 107 }
pirottealex 0:a8cee96c9250 108
pirottealex 0:a8cee96c9250 109 if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
pirottealex 0:a8cee96c9250 110 {
pirottealex 0:a8cee96c9250 111 etat=22;
pirottealex 0:a8cee96c9250 112 }
pirottealex 0:a8cee96c9250 113 if(blocks)
pirottealex 0:a8cee96c9250 114 {
pirottealex 0:a8cee96c9250 115 etat=1;
pirottealex 0:a8cee96c9250 116 }
pirottealex 0:a8cee96c9250 117 break;
pirottealex 0:a8cee96c9250 118 case(22) :
pirottealex 0:a8cee96c9250 119 cap_rot=cap_set-30;
pirottealex 0:a8cee96c9250 120 if(cap_rot<0)
pirottealex 0:a8cee96c9250 121 {
pirottealex 0:a8cee96c9250 122 cap_rot=cap_rot+360;
pirottealex 0:a8cee96c9250 123 }
pirottealex 0:a8cee96c9250 124
pirottealex 0:a8cee96c9250 125 if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
pirottealex 0:a8cee96c9250 126 {
pirottealex 0:a8cee96c9250 127 etat=21;
pirottealex 0:a8cee96c9250 128 }
pirottealex 0:a8cee96c9250 129 if(blocks)
pirottealex 0:a8cee96c9250 130 {
pirottealex 0:a8cee96c9250 131 etat=1;
pirottealex 0:a8cee96c9250 132 }
pirottealex 0:a8cee96c9250 133 break;
pirottealex 0:a8cee96c9250 134
pirottealex 0:a8cee96c9250 135 }
pirottealex 0:a8cee96c9250 136
pirottealex 0:a8cee96c9250 137 switch(etat)
pirottealex 0:a8cee96c9250 138 {
pirottealex 0:a8cee96c9250 139 case(0) : vitmoteur(0,0);break;
pirottealex 0:a8cee96c9250 140
pirottealex 0:a8cee96c9250 141 case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
pirottealex 0:a8cee96c9250 142
pirottealex 1:99f469d63b9a 143 case(2) : vitmoteur(V_max*1.5,V_max*1.5);break;
pirottealex 1:99f469d63b9a 144
pirottealex 1:99f469d63b9a 145 case(3) :
pirottealex 0:a8cee96c9250 146
pirottealex 1:99f469d63b9a 147 cap_arriere=cap_set;
pirottealex 1:99f469d63b9a 148 erreur_ar=cap_arriere-gBoussole;
pirottealex 1:99f469d63b9a 149
pirottealex 1:99f469d63b9a 150 if(erreur_ar>20)
pirottealex 1:99f469d63b9a 151 {
pirottealex 1:99f469d63b9a 152 erreur_ar=20;
pirottealex 1:99f469d63b9a 153 }
pirottealex 1:99f469d63b9a 154 if(erreur_ar<-20)
pirottealex 1:99f469d63b9a 155 {
pirottealex 1:99f469d63b9a 156 erreur_ar=-20;
pirottealex 1:99f469d63b9a 157 }
pirottealex 1:99f469d63b9a 158 vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);break;
pirottealex 0:a8cee96c9250 159
pirottealex 0:a8cee96c9250 160 case(21) : vitmoteur(V_recherche,-V_recherche);break;
pirottealex 0:a8cee96c9250 161
pirottealex 0:a8cee96c9250 162 case(22) : vitmoteur(-V_recherche,V_recherche);break;
pirottealex 0:a8cee96c9250 163
pirottealex 0:a8cee96c9250 164 case(100) : break;
pirottealex 0:a8cee96c9250 165 }
pirottealex 0:a8cee96c9250 166
pirottealex 0:a8cee96c9250 167 }
pirottealex 0:a8cee96c9250 168 }
pirottealex 0:a8cee96c9250 169
pirottealex 0:a8cee96c9250 170
pirottealex 0:a8cee96c9250 171