jhonatan angel
abrayan
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "main.h" 00003 00004 DigitalOut s0(PB_13); 00005 DigitalOut s1(PB_14); 00006 DigitalOut s2(PB_15); 00007 DigitalOut s3(PB_1); 00008 DigitalIn out (PB_2); 00009 00010 DigitalOut LED(LED1); 00011 InterruptIn pulse(PC_13); 00012 float retardo = 0; 00013 00014 00015 void ISR_Poff() 00016 00017 { 00018 00019 retardo = (0.05); 00020 00021 } 00022 00023 00024 00025 Timer tiempo; 00026 00027 unsigned int lectura (void); 00028 00029 Serial command(USBTX, USBRX); 00030 00031 int main() { 00032 init_servo(); 00033 init_serial(); 00034 pulse.fall (&ISR_Poff); 00035 00036 debug_m("inicio \n"); 00037 uint32_t read_cc; 00038 while(1) 00039 { 00040 LED = !LED; 00041 wait (retardo); 00042 00043 read_cc=read_command(); 00044 00045 switch (read_cc) 00046 { 00047 case 0x01: moving(); 00048 break; 00049 00050 case 0x02: movimiento(); 00051 break; 00052 00053 case 0x03: sensor(); 00054 break; 00055 00056 case 0x04: saludo(); 00057 break; 00058 00059 default: debug_m("error de comando.\n"); 00060 break ; 00061 } 00062 00063 } 00064 00065 } 00066 00067 00068 00069 uint32_t read_command() 00070 { 00071 char intc=command.getc(); 00072 00073 while(intc != 0xff) 00074 intc=command.getc(); 00075 return command.getc(); 00076 } 00077 00078 00079 00080 void init_serial() 00081 { 00082 command.baud(9600); 00083 } 00084 00085 00086 void moving() 00087 { 00088 command.printf("se inicia el 1er comado mover servos..\n"); 00089 char nmotor=command.getc(); 00090 char grados=command.getc(); 00091 char endc=command.getc(); 00092 mover_ser(nmotor,grados); 00093 debug_m("Moviendo servos...\n"); 00094 } 00095 00096 00097 void movimiento() 00098 { 00099 command.printf("se inicia el 2do comado mover patas..\n"); 00100 char lado=command.getc(); 00101 char movi=command.getc(); 00102 char endc=command.getc(); 00103 00104 switch (lado) 00105 { 00106 case 0x01: switch (movi) //Pata Derecha_delantera 00107 { 00108 case 0x01: mover_ser(0x01,0x01); 00109 command.printf("moviendo pata derecha delantera arriba..\n"); 00110 break; 00111 00112 case 0x02: mover_ser(0x01,0x2d); 00113 command.printf("moviendo pata derecha delantera abajo..\n"); 00114 break; 00115 00116 case 0x03: mover_ser(0x02,0x5a); 00117 command.printf("moviendo pata izquierda delantera adelante..\n"); 00118 break; 00119 00120 case 0x04: mover_ser(0x02,0x0a); 00121 command.printf("moviendo pata izquierda delantera atras..\n"); 00122 break; 00123 } 00124 break; 00125 00126 case 0x02: switch (movi) //Pata Derecha_trasera 00127 { 00128 case 0x01: mover_ser(0x03,0x01); 00129 command.printf("moviendo pata derecha trasera arriba..\n"); 00130 break; 00131 00132 case 0x02: mover_ser(0x03,0x2d); 00133 command.printf("moviendo pata derecha trasera abajo..\n"); 00134 break; 00135 00136 case 0x03: mover_ser(0x04,0xaa); 00137 command.printf("moviendo pata derecha trasera adelante..\n"); 00138 break; 00139 00140 case 0x04: mover_ser(0x04,0x5a); 00141 command.printf("moviendo pata derecha trasera atras..\n"); 00142 break; 00143 } 00144 break; 00145 00146 case 0x03: switch (movi) //Pata Izquierda_delantera 00147 { 00148 case 0x01: mover_ser(0x05,0x01); 00149 command.printf("moviendo pata izquierda delantera arriba..\n"); 00150 break; 00151 00152 case 0x02: mover_ser(0x05,0x2d); 00153 command.printf("moviendo pata izquierda delantera abajo..\n"); 00154 break; 00155 00156 case 0x03: mover_ser(0x06,0x5a); 00157 command.printf("moviendo pata izquierda delantera adelante..\n"); 00158 break; 00159 00160 case 0x04: mover_ser(0x06,0xaa); 00161 command.printf("moviendo pata izquierda delantera atras..\n"); 00162 break; 00163 } 00164 break; 00165 00166 case 0x04: switch (movi) //Pata Izquierda_trasera 00167 { 00168 case 0x01: mover_ser(0x07,0x01); 00169 command.printf("moviendo pata izquierda trasera arriba..\n"); 00170 break; 00171 00172 case 0x02: mover_ser(0x07,0x2d); 00173 command.printf("moviendo pata izquierda trasera abajo..\n"); 00174 break; 00175 00176 case 0x03: mover_ser(0x08,0x0a); 00177 command.printf("moviendo pata izquierda trasera adelante..\n"); 00178 break; 00179 00180 case 0x04: mover_ser(0x08,0x5a); 00181 command.printf("moviendo pata izquierda trasera atras..\n"); 00182 break; 00183 } 00184 break; 00185 00186 default: debug_m("error de comando.\n"); 00187 break ; 00188 } 00189 //debug_m("fin del comado guardar..\n"); 00190 } 00191 00192 void sensor() 00193 { 00194 command.printf ("se inicia el 3er comando sensor de color..\n"); 00195 char leer=command.getc(); 00196 char endc=command.getc(); 00197 00198 unsigned int ROJO=0; 00199 unsigned int VERDE=0; 00200 unsigned int AZUL=0; 00201 00202 if(leer==0x01) 00203 { 00204 s0=1; 00205 s1=0; 00206 00207 s2=0; 00208 s3=0; 00209 ROJO= lectura(); 00210 00211 s2=0; 00212 s3=1; 00213 AZUL= lectura(); 00214 00215 s2=1; 00216 s3=1; 00217 VERDE= lectura(); 00218 00219 if (ROJO<VERDE && VERDE>AZUL && AZUL>ROJO) 00220 { 00221 command.printf("Color detectado: ROJO \n"); 00222 command.printf("ADELANTE....\n"); 00223 00224 for(int x=0;x<5;x++) 00225 { 00226 mover_ser(0x01,0x00); 00227 wait(0.1); 00228 mover_ser(0x02,0x32); 00229 wait(0.1); 00230 mover_ser(0x01,0x3c); 00231 wait(0.1); 00232 mover_ser(0x03,0x00); 00233 wait(0.1); 00234 mover_ser(0x04,0x5a); 00235 wait(0.1); 00236 mover_ser(0x03,0x3c); 00237 wait(0.1); 00238 mover_ser(0x02,0x5a); 00239 wait(0.1); 00240 mover_ser(0x04,0x82); 00241 wait(0.1); 00242 mover_ser(0x05,0x00); 00243 wait(0.1); 00244 mover_ser(0x06,0x82); 00245 wait(0.1); 00246 mover_ser(0x05,0x3c); 00247 wait(0.1); 00248 mover_ser(0x07,0x00); 00249 wait(0.1); 00250 mover_ser(0x08,0x5a); 00251 wait(0.1); 00252 mover_ser(0x07,0x3c); 00253 wait(0.1); 00254 mover_ser(0x06,0x5a); 00255 wait(0.1); 00256 mover_ser(0x08,0x32); 00257 wait(0.1); 00258 00259 } 00260 } 00261 else 00262 { 00263 if (VERDE<AZUL && AZUL>ROJO && ROJO>VERDE) 00264 { 00265 command.printf("Color detectado: VERDE \n"); 00266 command.printf("ROTAR DERECHA\n"); 00267 00268 for(int x=0;x<5;x++) 00269 { 00270 mover_ser(0x01,0x00); 00271 wait(0.1); 00272 mover_ser(0x02,0x32); 00273 wait(0.1); 00274 mover_ser(0x01,0x3C); 00275 wait(0.1); 00276 mover_ser(0x03,0x00); 00277 wait(0.1); 00278 mover_ser(0x04,0x32); 00279 wait(0.1); 00280 mover_ser(0x03,0x3C); 00281 wait(0.1); 00282 mover_ser(0x07,0x00); 00283 wait(0.1); 00284 mover_ser(0x08,0x32); 00285 wait(0.1); 00286 mover_ser(0x07,0x3C); 00287 wait(0.1); 00288 mover_ser(0x05,0x00); 00289 wait(0.1); 00290 mover_ser(0x06,0x32); 00291 wait(0.1); 00292 mover_ser(0x05,0x3C); 00293 wait(0.1); 00294 mover_ser(0x02,0x5a); 00295 wait(0.1); 00296 mover_ser(0x04,0x5a); 00297 wait(0.1); 00298 mover_ser(0x06,0x5a); 00299 wait(0.1); 00300 mover_ser(0x08,0x5a); 00301 wait(0.1); 00302 } 00303 } 00304 else 00305 { 00306 if (AZUL<ROJO && ROJO>VERDE && VERDE>AZUL) 00307 { 00308 command.printf("Color detectado: AZUL \n"); 00309 command.printf("ATRAS....\n"); 00310 00311 00312 for(int x=0;x<5;x++) 00313 { 00314 mover_ser(0x01,0x00); 00315 wait(0.2); 00316 mover_ser(0x02,0x5a); 00317 wait(0.2); 00318 mover_ser(0x01,0x3c); 00319 wait(0.2); 00320 mover_ser(0x03,0x00); 00321 wait(0.2); 00322 mover_ser(0x04,0x82); 00323 wait(0.2); 00324 mover_ser(0x03,0x3c); 00325 wait(0.2); 00326 mover_ser(0x02,0x32); 00327 wait(0.2); 00328 mover_ser(0x04,0x5a); 00329 wait(0.2); 00330 mover_ser(0x05,0x00); 00331 wait(0.2); 00332 mover_ser(0x06,0x5a); 00333 wait(0.2); 00334 mover_ser(0x05,0x3c); 00335 wait(0.2); 00336 mover_ser(0x07,0x00); 00337 wait(0.2); 00338 mover_ser(0x08,0x32); 00339 wait(0.2); 00340 mover_ser(0x07,0x3c); 00341 wait(0.2); 00342 mover_ser(0x06,0x82); 00343 wait(0.2); 00344 mover_ser(0x08,0x5a); 00345 wait(0.2); 00346 00347 } 00348 } 00349 else 00350 { 00351 command.printf("otro color \n"); 00352 } 00353 } 00354 } 00355 00356 wait(0.5); 00357 } 00358 } 00359 00360 00361 unsigned int lectura (void) 00362 { 00363 00364 unsigned int inicio=0, final=0, resultado=0; 00365 tiempo.start (); 00366 while (out) {} 00367 while (!out) {} 00368 while (out) {} 00369 00370 inicio= tiempo.read_us(); 00371 while (!out) {} 00372 final=tiempo.read_us(); 00373 resultado=(final-inicio); 00374 tiempo.reset (); 00375 return (resultado); 00376 00377 00378 } 00379 00380 00381 void saludo (void) 00382 { 00383 char lado=command.getc(); 00384 switch (lado) 00385 { 00386 case 0x01: command.printf("HOLA!!!!....\n"); 00387 mover_ser(0x03,0x00); 00388 mover_ser(0x07,0x00); 00389 mover_ser(0x05,0x5a); 00390 wait(0.1); 00391 mover_ser(0x01,0x00); 00392 wait(0.4); 00393 mover_ser(0x02,0x5a); 00394 wait(0.4); 00395 mover_ser(0x02,0x0a); 00396 wait(0.4); 00397 mover_ser(0x02,0x5a); 00398 wait(0.4); 00399 mover_ser(0x02,0x0a); 00400 wait(0.4); 00401 mover_ser(0x02,0x5a); 00402 wait(0.4); 00403 mover_ser(0x01,0x3c); 00404 mover_ser(0x03,0x3c); 00405 mover_ser(0x07,0x3c); 00406 mover_ser(0x05,0x3c); 00407 wait(0.1); 00408 break; 00409 00410 case 0x02: command.printf("EMOCIONADO!!!!....\n"); 00411 mover_ser(0x01,0x00); 00412 mover_ser(0x03,0x00); 00413 mover_ser(0x05,0x00); 00414 mover_ser(0x07,0x00); 00415 wait(0.3); 00416 mover_ser(0x01,0x3c); 00417 mover_ser(0x03,0x3c); 00418 mover_ser(0x05,0x3c); 00419 mover_ser(0x07,0x3c); 00420 wait(0.3); 00421 mover_ser(0x01,0x00); 00422 mover_ser(0x03,0x00); 00423 mover_ser(0x05,0x00); 00424 mover_ser(0x07,0x00); 00425 wait(0.3); 00426 mover_ser(0x01,0x3c); 00427 mover_ser(0x03,0x3c); 00428 mover_ser(0x05,0x3c); 00429 mover_ser(0x07,0x3c); 00430 wait(0.3); 00431 break; 00432 00433 case 0x03: command.printf("''GIRANDO''!!!!....\n"); 00434 mover_ser(0x01,0x00); 00435 wait(0.3); 00436 mover_ser(0x03,0x00); 00437 wait(0.3); 00438 mover_ser(0x07,0x00); 00439 wait(0.3); 00440 mover_ser(0x01,0x46); 00441 wait(0.3); 00442 mover_ser(0x05,0x00); 00443 wait(0.3); 00444 mover_ser(0x03,0x46); 00445 wait(0.3); 00446 mover_ser(0x01,0x00); 00447 wait(0.3); 00448 mover_ser(0x07,0x46); 00449 wait(0.3); 00450 mover_ser(0x03,0x00); 00451 wait(0.3); 00452 mover_ser(0x05,0x46); 00453 mover_ser(0x01,0x46); 00454 mover_ser(0x03,0x46); 00455 mover_ser(0x07,0x46); 00456 wait(0.3); 00457 break; 00458 00459 00460 default: command.printf("error de comando.\n"); 00461 break ; 00462 } 00463 00464 00465 } 00466 00467 00468 void debug_m(char *s , ... ){ 00469 #if DEBUG 00470 command.printf(s); 00471 #endif 00472 }
Generated on Thu Sep 1 2022 02:40:26 by
1.7.2