Diff: main.cpp

Revision:

0:91600a6decef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 04 18:20:34 2017 +0000
@@ -0,0 +1,192 @@
+#include "mbed.h"
+#include "rtos.h"
+
+/********Declaracion de pines********************/
+//Motores(1-derecha y 2 izquierda)
+DigitalOut enable2(P0_8);//S1
+DigitalOut M2A(P0_13);//PWM4
+DigitalOut M2B(P0_14);//PWM3
+
+DigitalOut enable1(P1_13);//S2
+DigitalOut M1A(P0_10);//PWM1
+DigitalOut M1B(P0_9);//PWM2
+
+
+//Sensores de presencia
+InterruptIn der(P1_22);
+InterruptIn izq(P1_19);
+InterruptIn fder(P1_14);
+InterruptIn fizq(P0_21);
+
+//Sensores de linea
+InterruptIn ld(P1_25);
+InterruptIn li(P0_15);
+
+//LEDs
+DigitalOut led1(P0_18);//D8-L4
+DigitalOut led2(P0_19);//D9-L5
+DigitalOut led3(P1_26);//D10-L6
+DigitalOut led4(P1_27);//D11-L7
+DigitalOut led5(P0_4);//D12-L8
+DigitalOut led6(P0_17);//D14-L9
+
+int m = 0;
+void girar_derecha()
+{
+    M2A=0;
+    M2B=1;
+    M1A=1;
+    M1B=0;
+}
+void stop2()
+{
+    M2A = 1;
+    M2B = 1;
+    M1A = 1;
+    M1B = 1;
+}
+
+void avanzar()
+{
+    M2A=0;
+    M2B=1;
+    M1A=0;
+    M1B=1;
+}
+void reversa()
+{
+    M2A=1;
+    M2B=0;
+    M1A=1;
+    M1B=0;
+}
+
+void girar_izquierda()
+{
+    M2A=1;
+    M2B=0;
+    M1A=0;
+    M1B=1;
+}
+
+void stop()
+{
+    M2A=0;
+    M2B=0;
+    M1A=0;
+    M1B=0;
+}
+
+/************************INTERRUPT********************/
+int h = 0;
+void linea1()
+{
+    if(h == 0) {
+        stop();
+        reversa();
+        wait(0.3);
+        girar_derecha();
+        wait(0.2);
+        avanzar();
+        wait(0.1);
+        h = h + 1;
+    } else {
+        girar_derecha();
+        wait(0.2);
+        avanzar();
+        wait(0.1);
+        h = 0;
+    }
+}
+void linea2()
+{
+    if(h == 1) {
+        girar_derecha();
+        wait(0.1);
+        avanzar();
+        wait(0.1);
+        h = 0;
+    }
+}
+
+void derecha()
+{
+    led1=1;
+    while(izq == 0) {
+        girar_derecha();
+        wait(0.1);
+        stop();
+        wait(0.01);
+    }
+    led1=0;
+}
+
+void izquierda()
+{
+    led2=1;
+    while(der == 0) {
+        girar_izquierda();
+        wait(0.1);
+        stop();
+        wait(0.01);
+    }
+    led2=0;
+}
+void frented()
+{
+    while(fder == 0 && fizq == 1) {
+        girar_izquierda();
+        wait(0.01);
+        stop();
+        wait(0.01);
+    }
+    while(fder == 0 && fizq == 0) {
+        avanzar();
+        wait(0.01);
+        //stop();
+        //wait(0.01);
+    }
+}
+void frenteizq()
+{
+    while(fizq == 0 && fder == 1) {
+        girar_derecha();
+        wait(0.01);
+        stop();
+        wait(0.01);
+    }
+    while(fizq == 0 && fder == 0) {
+        avanzar();
+        wait(0.01);
+        //stop();
+        //wait(0.01);
+
+    }
+}
+//*************************************************/
+
+int main()
+{
+    //configuracion en pullup o pulldown
+    der.mode(PullDown);
+    izq.mode(PullDown);
+    fder.mode(PullDown);
+    fizq.mode(PullDown);
+    li.mode(PullUp);
+    ld.mode(PullUp);
+
+    wait(5);
+    ld.rise(&linea1);
+    li.rise(&linea2);
+    fder.fall(&frented);
+    fizq.fall(&frenteizq);
+    der.fall(&izquierda);
+    izq.fall(&derecha);
+    while(1) {
+        avanzar();
+        wait(0.3);
+        stop();
+        wait(0.08);
+
+    }
+}
\ No newline at end of file