UC-GRUPO DE ESTUDIO-INTELIGENCIA ARTIFICIAL
laberinto
Dependencies: HCSR04 Motordriver mbed
Revision 0:7e3e917c6fa8, committed 2017-08-12
- Comitter:
- Janice_Rafael
- Date:
- Sat Aug 12 16:17:11 2017 +0000
- Commit message:
- micromouse
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HCSR04.lib Sat Aug 12 16:17:11 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/antoniolinux/code/HCSR04/#86b2086be101
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Sat Aug 12 16:17:11 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/osmeest/code/Motordriver/#83245b45ea70
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Aug 12 16:17:11 2017 +0000
@@ -0,0 +1,203 @@
+#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);
+ }
+ }
+ }
+}
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Aug 12 16:17:11 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fd96258d940d \ No newline at end of file