Diff: main.cpp

Revision:

0:7e3e917c6fa8

--- /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