main.cpp

00001 #include "mbed.h"
00002 #include "fct.h"
00003 #include "Pixy.h"
00004 #include "PixyLink.h"
00005 #include "SRF05.h"
00006 #include <CMPS03.h>
00007 
00008 Serial pc (USBTX,USBRX);
00009 int main()
00010 {
00011     init();
00012     Pixy pixy= Pixy(p28,p27);
00013     pixy.setSerialOutput(&pc);
00014     pc.printf("alive");
00015 
00016     while(1) {
00017         lecture_blanc();    // recupere valeurs capteurs ligne blanche
00018         lecture_us();   // recupere valeurs capteurs us
00019         lecture_boussole(); //gBoussole
00020         bout=bp.read();
00021         blocks = pixy.getBlocks();
00022         //pc.printf("etat=%d\n\r",etat);
00023         //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);
00024         if (blocks) {
00025             taille=pixy.blocks[j].width*pixy.blocks[j].height;
00026             errorX = (160-pixy.blocks[j].x);
00027             //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);
00028             //pc.printf("OK");
00029         }
00030         
00031         switch(etat)
00032         {
00033             case(0) :
00034             
00035                 if(bout==0)
00036                 {
00037                     etat=100;
00038                     cap_set=gBoussole;
00039                 }
00040                 if((captL1>=1)||(captL3>=1))
00041                 {
00042                     etat=3;
00043                 }
00044                 break;
00045             
00046             case(100):
00047             
00048                 if((blocks)&&(taille>50))
00049                 {
00050                     etat=1;
00051                 }
00052                 
00053                 else
00054                 {
00055                     etat=21;
00056                 }
00057                 if((captL1>=1)||(captL3>=1))
00058                 {
00059                     etat=3;
00060                 }
00061                 break;
00062                  
00063             case(1) :
00064             
00065                 if(!blocks)
00066                 {
00067                     flag++;
00068                 }
00069                 if(flag>40)
00070                 {
00071                     etat=2;
00072                     tempo.stop();
00073                     tempo.reset();
00074                     tempo.start();
00075                     flag=0;
00076                 }
00077                 
00078                 if((captL1>=1)||(captL3>=1))
00079                 {
00080                     etat=3;
00081                     flag=0;
00082                 }
00083                 break;
00084             case(2) :
00085             
00086                 if(tempo.read()>0.5)
00087                 {
00088                     etat=3;
00089                 }
00090                 if((captL1>=1)||(captL3>=1))
00091                 {
00092                     etat=3;
00093                 }
00094                 break;
00095             case(3) :
00096             
00097                 if(us_arriere<25)
00098                 {
00099                     etat=100;
00100                 }
00101                 break;
00102             case(21) :
00103                 cap_rot=cap_set+30;
00104                 if(cap_rot>360)
00105                 {
00106                     cap_rot=cap_rot-360;
00107                 }
00108                 
00109                 if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
00110                 {
00111                     etat=22;
00112                 }
00113                 if(blocks)
00114                 {
00115                     etat=1;
00116                 }
00117                 break;
00118             case(22) :
00119                 cap_rot=cap_set-30;
00120                 if(cap_rot<0)
00121                 {
00122                     cap_rot=cap_rot+360;
00123                 }
00124                 
00125                 if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
00126                 {
00127                     etat=21;
00128                 }
00129                 if(blocks)
00130                 {
00131                     etat=1;
00132                 }
00133                 break;
00134             
00135         }
00136         
00137         switch(etat)
00138         {
00139             case(0) : vitmoteur(0,0);break;
00140             
00141             case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
00142             
00143             case(2) : vitmoteur(V_max*1.5,V_max*1.5);break;
00144             
00145             case(3) : 
00146             
00147                     cap_arriere=cap_set;
00148                     erreur_ar=cap_arriere-gBoussole;
00149                     
00150                     if(erreur_ar>20)
00151                     {
00152                         erreur_ar=20;
00153                     }
00154                     if(erreur_ar<-20)
00155                     {
00156                         erreur_ar=-20;
00157                     }
00158                     vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);break;
00159             
00160             case(21) : vitmoteur(V_recherche,-V_recherche);break;           
00161             
00162             case(22) : vitmoteur(-V_recherche,V_recherche);break;
00163             
00164             case(100) : break;
00165         } 
00166        
00167     }
00168 }
00169         
00170 
00171