,,,,,,,,,,,,
Dependencies: CMPS03 SRF05 mbed pixy
Fork of 0000Non_stop_code_v1 by
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
Generated on Wed Jul 13 2022 02:31:18 by 1.7.2