laberinto
Dependencies: HCSR04 Motordriver mbed
main.cpp
- Committer:
- Janice_Rafael
- Date:
- 2017-08-12
- Revision:
- 0:7e3e917c6fa8
File content as of revision 0:7e3e917c6fa8:
#include "mbed.h" #include "hcsr04.h" #include "motordriver.h" //Sensores de Distancia HCSR04 usensorA(p10,p11); HCSR04 usensorD(p6,p5); HCSR04 usensorI(p20,p19); //Motores DigitalOut STBY(p15); PwmOut PWMA(p26); DigitalOut AIN1(p30); DigitalOut AIN2(p29); PwmOut PWMB(p25); DigitalOut BIN1(p14); DigitalOut BIN2(p13); //Buttons DigitalIn btn1(p16); InterruptIn btn2(p17); //Configuración de interrupciones //p10,p11,p12,p13 InterruptIn INT0(p22); //Interrupción externa 0 CHA1 InterruptIn INT1(p21); //Interrupción externa 1 CHB1 InterruptIn INT2(p23); //Interrupción externa 2 CHA2 InterruptIn INT3(p24); //Interrupción externa 3 CHB2 int CNT1, CNT2; //Contadores de posición para encoders bool flagCHA1, flagCHB1, flagCHA2, flagCHB2; unsigned int distA, distD, distI; long ti = 0; void tiempo(double tiempoControlar, double tiempoInicial) { double ti_transcu = 0; double t = clock(); while (tiempoControlar > ti_transcu) { ti_transcu = t - tiempoInicial; t = clock(); } } void adelante(){ STBY = 1; AIN1 = 1; AIN2 = 0; PWMA.write(0.32f); BIN1 = 1; BIN2 = 0; PWMB.write(0.34f); } void giroDerecha(){ STBY = 1; AIN1 = 1; AIN2 = 0; PWMA.write(0.29f); BIN1 = 0; BIN2 = 1; PWMB.write(0.30f); } void giroIzquierda(){ STBY = 1; AIN1 = 0; AIN2 = 1; PWMA.write(0.29f); BIN1 = 1; BIN2 = 0; PWMB.write(0.30f); } /* void corrDer(){ STBY = 1; AIN1 = 1; AIN2 = 0; PWMA.write(0.1f); } void corrIzq(){ STBY = 1; BIN1 = 1; BIN2 = 0; PWMB.write(0.05f); }*/ void adelantet(long tmedir){ ti=clock(); adelante(); tiempo(tmedir,ti); } void giroDerechat(long tmedir){ ti=clock(); giroDerecha(); tiempo(tmedir,ti); } void giroIzquierdat(long tmedir){ ti=clock(); giroIzquierda(); tiempo(tmedir,ti); } void stopt(long tmedir){ ti=clock(); STBY = 0; tiempo(tmedir,ti); } void verificarSalida() { //if(distA < distD && distA < distI) if(distD >= distI) giroDerechat(90); // 180 grados else if(distI > distD) giroIzquierdat(90); // 180 grados } void btn2_recorrido() { STBY = 0; stopt(500); } int row = 10; int col = 10; int pos; int posicion(int row, int col) { if(row > 5){ int nrow = 0; nrow = (row - 6); row = (5 - nrow); } if(col > 5){ int ncol = 0; ncol = (col - 6); col = (5 - ncol); } return (9 -(row - 1) - (col - 1)); } int main(){ btn2.rise(&btn2_recorrido);//interrupcion para detener while(1){ // recorrer laberinto if(btn1.read()){ //Detect Switch Pres stopt(400); // espera de 4 s while(pos != 1){ distA=usensorA.distance(); wait_ms(90); distI=usensorI.distance(); wait_ms(90); distD=usensorD.distance(); wait_ms(90); pos = posicion(row, col); // buscar meta if(distA >= 10) { if(distD > 10) { adelante(); giroDerechat(40); //90 grados } /*else if(distD == distI){ } else{ if(distD > distI){ corrDer(); //corrige a la derecha } else{ corrIzq(); } stopt(10); }*/ adelante(); // un cuadrante stopt(10); } else if(distA < 10) { stopt(10); if(distD < 10 && distI > 10) { adelante(); giroIzquierdat(50); } else if(distI < 10 && distD > 10) { adelante(); giroDerechat(40); } else if(distI > 10 && distD > 10) { adelante(); giroDerechat(40); } else if(distD < 10 && distI < 10) { verificarSalida(); } adelante(); stopt(10); } } } } }