UC-GRUPO DE ESTUDIO-INTELIGENCIA ARTIFICIAL
/
RobLaberinto001
ROB LAB
main.cpp
- Committer:
- jhony
- Date:
- 2017-07-27
- Revision:
- 0:9be8543766b6
File content as of revision 0:9be8543766b6:
#include "mbed.h" #include "hcsr04.h" //Serial pc(USBTX, USBRX); //Sensores de Distancia HCSR04 usensorA(p10,p11); HCSR04 usensorD(p6,p5); HCSR04 usensorI(p20,p19); //Motores DigitalOut STBY(p15);//p7 PwmOut PWMA(p26);//p21 DigitalOut AIN1(p30);//p5 DigitalOut AIN2(p29);//p6 PwmOut PWMB(p25);//p22 DigitalOut BIN1(p14);//p8 DigitalOut BIN2(p13);//p9 //Interrupcion unsigned int distA, distD, distI; void girarIzquierda() { BIN1 = 1; BIN2 = 0; PWMB = 2; AIN1 = 0; AIN2 = 1; PWMA = 2; } void girarDerecha() { BIN1 = 0; BIN2 = 1; PWMB = 2; AIN1 = 1; AIN2 = 0; PWMA = 2; } void adelante() { BIN1 = 1; BIN2 = 0; PWMB = 2; AIN1 = 1; AIN2 = 0; PWMA = 2; } void verificarSalida() { if(distA < distD && distA < distI) { if(distD > distI) { girarDerecha(); } else if(distI>distD) { girarIzquierda(); } } else { adelante(); } } int main() { //pc.baud(115200); //pc.printf("Hello World from FRDM-K64F board.\n"); while(1) { usensorI.start(); wait_ms(500); distI=usensorI.get_dist_cm(); //pc.printf("IZQUIERDA cm:%ld",distI ); usensorA.start(); wait_ms(500); distA=usensorA.get_dist_cm(); //pc.printf("ADELANTE cm:%ld",distA ); usensorD.start(); wait_ms(500); distD=usensorD.get_dist_cm(); //pc.printf("DERECHA cm:%ld",distD ); if(distA > 20 && distD>20 && distI>20) { adelante(); } if(distA > 10 && distD<10 && distI<10) { adelante(); } if(distD>10 && distI<10) { girarDerecha(); } if(distI > 10 && distD<10 && distA<10) { girarIzquierda(); } if(distA < 10 && distD<10 && distI<10) { verificarSalida(); } } }