Diff: main.cpp

Revision:

0:bd4170087cf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,199 @@
+#include "main.h"
+#include "mbed.h"
+#include "calculaVelocidad.h"
+#include "mantieneVelocidad.h"
+#include "frenar.h"
+Serial device(PA_11, PA_12);
+//Para enviar datos al pc como debug
+Serial pc(USBTX, USBRX);
+
+//PwmOut pinRuedaDerechaAdelante(PB_13);
+PwmOut pinRuedaDerechaAdelante(PC_8);
+PwmOut pinRuedaDerechaAtras(PA_7);
+//PwmOut pinRuedaIzquierdaAdelante(PC_8);
+PwmOut pinRuedaIzquierdaAdelante(PB_15);
+PwmOut pinRuedaIzquierdaAtras(PB_13);
+int pinControl = 3;
+double potenciaMotoresFrameAnterior = 0;
+double potenciaMotores = 0;
+int acumulador = 0;
+
+double velocidadDeseada = 1.0;
+double velocidadCalculada=0.0;
+
+int tiempoUltimoControlFrenado;
+int ping = 50;
+int periodo = 0;
+
+int modo = 0; //0 Frenar
+//1 VelocidadConstante
+//2 SigueLinea
+
+//Sensores de color
+AnalogIn derecha(PC_3);
+AnalogIn izquierda(PC_2);
+
+float velocidad = 0.25f;
+float velocidadBaja = 0.0f;
+float velocidadSube = 0.25f;
+float medidaDerecha;
+float medidaIzquierda;
+float velocidadIzquierda;
+float velocidadDerecha;
+bool izquierdaNegro = false;
+bool derechaNegro = false;
+char estado = 'p';
+bool estadoCambiado = false;
+
+
+
+//Esto es exclusivo de la implementación en mbed
+//Interrupcion para el calculo de velocidad
+InterruptIn sensorRuedaDerecha(A5);
+//Timer para el ping
+Timer timerPing;
+Timer timerFreno;
+
+void interrupcion()
+{
+    //El acumulador muestra el número de muescas que avanza la rueda.
+    //No se pueden poner prints en las interrupciones
+    acumulador++;
+}
+
+
+
+int main()
+{
+
+    device.baud(9600);
+    pc.printf("Conexion establecida\n");
+    char rx;
+    
+    //Inicializamos cosas (como el setup de arduino)
+    sensorRuedaDerecha.rise(&interrupcion);
+    timerPing.start();
+    timerFreno.start();
+    while(1) {
+
+        //Comprobar por bluetooth
+if(device.readable()) {
+        pc.printf("\nRecibido dato\n");
+        rx =device.getc();
+        pc.printf("\n Recibido %c \n",rx);
+        if(rx == 'a') {
+            modo = 1;
+            pc.printf("acelera");
+        }
+        if(rx == 's') {
+            modo = 0;
+            pc.printf("frena");
+        }
+        if(rx == 'l') {
+            modo = 2;
+            pc.printf("linea");
+        }
+    }
+
+
+
+
+
+        if(modo == 2) {
+            //SEGUIR LINEA
+
+
+            medidaDerecha = derecha.read();
+            medidaIzquierda = izquierda.read();
+            medidaDerecha = medidaDerecha * 3300;
+            medidaIzquierda = medidaIzquierda * 3300;
+            //  printf("measure = %.0f mV\n", meas);
+            if (medidaDerecha > 2000) {
+                derechaNegro = true;
+            } else {
+                derechaNegro = false;
+            }
+            if (medidaIzquierda > 2000) {
+                izquierdaNegro = false;
+            } else {
+                izquierdaNegro = true;
+            }
+            //pc.printf("Izquierda %d y derecha %d", izquierdaNegro, derechaNegro);
+
+            if(izquierdaNegro == 1 and derechaNegro == 1 and (estado != 'r')) {
+                pc.printf("RECTO ");
+                velocidadIzquierda = velocidad;
+                velocidadDerecha = velocidad;
+                estado = 'r';
+                estadoCambiado = true;
+
+            } else if (izquierdaNegro == 1 and derechaNegro == 0 and (estado != 'i')) {
+                velocidadIzquierda = velocidadBaja;
+                velocidadDerecha = velocidadSube;
+                pc.printf("giraIzquierda ");
+                estado = 'i';
+                estadoCambiado = true;
+
+            } else if (izquierdaNegro == 0 and derechaNegro == 1 and (estado != 'd')) {
+                velocidadIzquierda = velocidadSube;
+                velocidadDerecha = velocidadBaja;
+                pc.printf("giraDerecha ");
+                estado = 'd';
+                estadoCambiado = true;
+            } else if (izquierdaNegro == 0 and derechaNegro == 0 and (estado != 'p')) {
+                velocidadIzquierda = 0.0f;
+                velocidadDerecha = 0.0f;
+                pc.printf("parado ");
+                estado = 'p';
+                estadoCambiado = true;
+            } else {
+                estadoCambiado = false;
+            }
+
+            pc.printf("  VelocidadIzquierda %g    VelocidadDerecha %g   ", velocidadIzquierda, velocidadDerecha);
+           // if(estadoCambiado) {
+
+                pinRuedaIzquierdaAdelante.write(velocidadIzquierda);
+                pinRuedaDerechaAdelante.write(velocidadDerecha);
+            //}
+
+
+        } else {
+            periodo = timerPing.read_ms();
+            if(periodo > ping) {
+                //pc.printf("\n%d", periodo);
+                velocidadCalculada = calculaVelocidad(periodo, acumulador);
+
+                //Por si queremos mostrar la velocidad por pantalla
+                pc.printf("\n%f", velocidadCalculada);
+                acumulador = 0;
+                timerPing.reset();
+            }
+            //pc.printf("\n%d", acumulador);
+
+            //Mantener velocidad
+
+            if(modo == 1) {
+                mantenerVelocidad(velocidadDeseada, velocidadCalculada, potenciaMotoresFrameAnterior, potenciaMotores,pinRuedaDerechaAdelante, pinRuedaIzquierdaAdelante);
+
+
+            } else {
+                //Aquí va el código para frenado
+                periodo = timerFreno.read_ms();
+                if(modo == 0) {
+
+                    if(periodo>ping) {
+                        pc.printf("FRENA %f", velocidadCalculada);
+                        frenar(velocidadCalculada, potenciaMotores, potenciaMotoresFrameAnterior, pinRuedaDerechaAdelante, pinRuedaDerechaAtras, pinRuedaIzquierdaAdelante, pinRuedaIzquierdaAtras);
+                        timerFreno.reset();
+                    }
+                }
+            }
+        }
+
+
+    }
+
+}
+
+