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@0:a8cee96c9250, 2017-06-10 (annotated)

Committer:
pirottealex
Date:
Sat Jun 10 04:58:21 2017 +0000
Revision:
0:a8cee96c9250
Child:
1:99f469d63b9a
rrrr

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 0:a8cee96c9250 27 //pc.printf("sig: %d x: %d y: %d width: %d height: %d\n\r", 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 0:a8cee96c9250 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 0:a8cee96c9250 40 break;
pirottealex 0:a8cee96c9250 41
pirottealex 0:a8cee96c9250 42 case(100):
pirottealex 0:a8cee96c9250 43
pirottealex 0:a8cee96c9250 44 if(blocks)
pirottealex 0:a8cee96c9250 45 {
pirottealex 0:a8cee96c9250 46 etat=1;
pirottealex 0:a8cee96c9250 47 }
pirottealex 0:a8cee96c9250 48
pirottealex 0:a8cee96c9250 49 else
pirottealex 0:a8cee96c9250 50 {
pirottealex 0:a8cee96c9250 51 etat=21;
pirottealex 0:a8cee96c9250 52 }
pirottealex 0:a8cee96c9250 53 break;
pirottealex 0:a8cee96c9250 54
pirottealex 0:a8cee96c9250 55 case(1) :
pirottealex 0:a8cee96c9250 56
pirottealex 0:a8cee96c9250 57 if(!blocks)
pirottealex 0:a8cee96c9250 58 {
pirottealex 0:a8cee96c9250 59 etat=2;
pirottealex 0:a8cee96c9250 60 tempo.stop();
pirottealex 0:a8cee96c9250 61 tempo.reset();
pirottealex 0:a8cee96c9250 62 tempo.start();
pirottealex 0:a8cee96c9250 63 }
pirottealex 0:a8cee96c9250 64 break;
pirottealex 0:a8cee96c9250 65 case(2) :
pirottealex 0:a8cee96c9250 66
pirottealex 0:a8cee96c9250 67 if(tempo.read()>0.5)
pirottealex 0:a8cee96c9250 68 {
pirottealex 0:a8cee96c9250 69 etat=3;
pirottealex 0:a8cee96c9250 70 }
pirottealex 0:a8cee96c9250 71 break;
pirottealex 0:a8cee96c9250 72 case(3) :
pirottealex 0:a8cee96c9250 73
pirottealex 0:a8cee96c9250 74 if(us_arriere<25)
pirottealex 0:a8cee96c9250 75 {
pirottealex 0:a8cee96c9250 76 etat=100;
pirottealex 0:a8cee96c9250 77 }
pirottealex 0:a8cee96c9250 78 break;
pirottealex 0:a8cee96c9250 79 case(21) :
pirottealex 0:a8cee96c9250 80 cap_rot=cap_set+30;
pirottealex 0:a8cee96c9250 81 if(cap_rot>360)
pirottealex 0:a8cee96c9250 82 {
pirottealex 0:a8cee96c9250 83 cap_rot=cap_rot-360;
pirottealex 0:a8cee96c9250 84 }
pirottealex 0:a8cee96c9250 85
pirottealex 0:a8cee96c9250 86 if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
pirottealex 0:a8cee96c9250 87 {
pirottealex 0:a8cee96c9250 88 etat=22;
pirottealex 0:a8cee96c9250 89 }
pirottealex 0:a8cee96c9250 90 if(blocks)
pirottealex 0:a8cee96c9250 91 {
pirottealex 0:a8cee96c9250 92 etat=1;
pirottealex 0:a8cee96c9250 93 }
pirottealex 0:a8cee96c9250 94 break;
pirottealex 0:a8cee96c9250 95 case(22) :
pirottealex 0:a8cee96c9250 96 cap_rot=cap_set-30;
pirottealex 0:a8cee96c9250 97 if(cap_rot<0)
pirottealex 0:a8cee96c9250 98 {
pirottealex 0:a8cee96c9250 99 cap_rot=cap_rot+360;
pirottealex 0:a8cee96c9250 100 }
pirottealex 0:a8cee96c9250 101
pirottealex 0:a8cee96c9250 102 if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
pirottealex 0:a8cee96c9250 103 {
pirottealex 0:a8cee96c9250 104 etat=21;
pirottealex 0:a8cee96c9250 105 }
pirottealex 0:a8cee96c9250 106 if(blocks)
pirottealex 0:a8cee96c9250 107 {
pirottealex 0:a8cee96c9250 108 etat=1;
pirottealex 0:a8cee96c9250 109 }
pirottealex 0:a8cee96c9250 110 break;
pirottealex 0:a8cee96c9250 111
pirottealex 0:a8cee96c9250 112 }
pirottealex 0:a8cee96c9250 113
pirottealex 0:a8cee96c9250 114 switch(etat)
pirottealex 0:a8cee96c9250 115 {
pirottealex 0:a8cee96c9250 116 case(0) : vitmoteur(0,0);break;
pirottealex 0:a8cee96c9250 117
pirottealex 0:a8cee96c9250 118 case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
pirottealex 0:a8cee96c9250 119
pirottealex 0:a8cee96c9250 120 case(2) : vitmoteur(V_max,V_max);break;
pirottealex 0:a8cee96c9250 121
pirottealex 0:a8cee96c9250 122 case(3) : vitmoteur(-V_moy,-V_moy);break;
pirottealex 0:a8cee96c9250 123
pirottealex 0:a8cee96c9250 124 case(21) : vitmoteur(V_recherche,-V_recherche);break;
pirottealex 0:a8cee96c9250 125
pirottealex 0:a8cee96c9250 126 case(22) : vitmoteur(-V_recherche,V_recherche);break;
pirottealex 0:a8cee96c9250 127
pirottealex 0:a8cee96c9250 128 case(100) : break;
pirottealex 0:a8cee96c9250 129 }
pirottealex 0:a8cee96c9250 130
pirottealex 0:a8cee96c9250 131 }
pirottealex 0:a8cee96c9250 132 }
pirottealex 0:a8cee96c9250 133
pirottealex 0:a8cee96c9250 134
pirottealex 0:a8cee96c9250 135