Revision 0:bd4170087cf1, committed 2015-05-04

Comitter:

Racafe

Date:

Mon May 04 08:39:07 2015 +0000

Commit message:

Todo a?adido;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/calculaVelocidad.cpp	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,35 @@
+
+#include "calculaVelocidad.h"
+#include "main.h"
+
+double velocidadActual=0.0;
+double historialVelocidades[]={0.0,0.0,0.0};
+int nominado = 0;
+
+double suavizaVelocidad(double velocidad){
+  double res= velocidad*0.7+historialVelocidades[0]*0.1+historialVelocidades[1]*0.1+historialVelocidades[0]*0.1;
+  return res;
+ }
+
+
+//A partir de las muescas de la rueda, calcula la velocidad en km/h
+double calculaVelocidad(int periodo, int acumulador){
+    double minutos = periodo / 60000.0;
+    double revoluciones = acumulador / 20.0;
+    double revolucionesporMinuto = revoluciones/minutos;     
+    double velocidad = revolucionesporMinuto*3.4*2*3.141592/60/100*3.6;
+    velocidad = suavizaVelocidad(velocidad);
+    historialVelocidades[nominado]=velocidad;
+    nominado++;
+    if(nominado >= 3){
+      nominado = 0;
+    }
+    
+    velocidadActual = velocidad;  
+
+ return velocidad;
+}
+  
+ 
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/calculaVelocidad.h	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,3 @@
+double calculaVelocidad(int periodo, int acumulador);
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/frenar.cpp	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "main.h"
+
+Serial pc2(USBTX, USBRX);
+int sentido= 0;
+int fase = 0;
+bool iniFreno = false;
+
+void frenar(double velocidadActual, double& potenciaMotores, double& potenciaMotoresFrameAnterior, PwmOut pinDAd, PwmOut pinDAt, PwmOut pinIAd, PwmOut pinIAt){
+  double sumador;
+  
+  //Primero, comprobamos si nos estamos moviendo
+ 
+  //  Serial.println(velocidadActual);
+//  Serial.print("Sentido");
+ // Serial.println(sentido);
+  if(velocidadActual>0.2){
+  double velocidadAnterior;
+      if(nominado == 0){
+        velocidadAnterior = historialVelocidades[2];
+      } else if(nominado == 1){
+        velocidadAnterior = historialVelocidades[1];
+       }else {
+        velocidadAnterior = historialVelocidades[0];
+       }
+
+       
+       if(sentido == 0){
+         potenciaMotores = -0.6f;
+         sentido = 1;
+         }
+         else if (sentido == 1){
+           if(velocidadActual <= (velocidadAnterior+0.3)){
+             potenciaMotores = potenciaMotores-0.05;
+             }else{
+               sentido = -1;
+               potenciaMotores = -potenciaMotores/3;
+               }
+           }
+           else{
+             if(velocidadActual <= (velocidadAnterior+0.3)){
+             potenciaMotores = potenciaMotores+0.05;
+             }else{
+               sentido = 1;
+               potenciaMotores = -potenciaMotores/3;
+               }
+             }
+         
+      if(potenciaMotores > 1.0){
+        potenciaMotores = 1.0;
+        }
+      if(potenciaMotores < -1.0){
+        potenciaMotores = -1.0;
+        }
+         
+         
+  //     Serial.print("POTENCIA " );
+  //     Serial.println(potenciaMotores);
+      
+
+    }else{
+      potenciaMotores=0;
+  //    Serial.println("ESTÁ FRENADO");
+      sentido = 0;
+      fase = 0;
+      }
+           if(potenciaMotores<0){
+        
+               pinDAt.write(-potenciaMotores);
+               pinDAd.write(0.0);
+               pinIAt.write(-potenciaMotores);
+               pinIAd.write(0.0);
+       /* analogWrite(pinRuedaDerechaAtras, -potenciaMotores);
+        analogWrite(pinRuedaDerechaAdelante, 0);
+        analogWrite(pinRuedaIzquierdaAtras, -potenciaMotores);
+        analogWrite(pinRuedaIzquierdaAdelante, 0);*/
+        
+      }else{
+        /*analogWrite(pinRuedaDerechaAdelante, potenciaMotores);
+        analogWrite(pinRuedaDerechaAtras, 0);
+        analogWrite(pinRuedaIzquierdaAdelante, potenciaMotores);
+        analogWrite(pinRuedaIzquierdaAtras, 0);*/
+        
+               pinDAd.write(potenciaMotores);
+               pinDAt.write(0.0);
+               pinIAd.write(potenciaMotores);
+               pinIAt.write(0.0);
+        
+      }
+    //  tiempoUltimoControlFrenado = millis();
+  
+  }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/frenar.h	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,1 @@
+void frenar(double velocidadActual, double &potenciaMotores, double &potenciaMotoresFrameAnterior, PwmOut pinDAd, PwmOut pinDAt, PwmOut pinIAd, PwmOut pinIAt);

--- /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();
+                    }
+                }
+            }
+        }
+
+
+    }
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,7 @@
+#ifndef MAIN_C_
+    #define MAIN_C_ 1
+    double extern velocidadActual;
+    double extern historialVelocidades[3];
+    int extern nominado;
+    
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mantieneVelocidad.cpp	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+
+Serial pc3(USBTX, USBRX);
+void mantenerVelocidad(double velocidadDesead, double velocidadActua, double &potenciaMotoresFrameAnterior, double &potenciaMotores, PwmOut pinDAdelante, PwmOut pinIAdelante){
+  
+  potenciaMotoresFrameAnterior = potenciaMotores;
+
+  //Para que nunca dividamos por 0
+  if(velocidadActua == 0.0){
+    velocidadActua=velocidadActua+0.1;  
+  }
+  double indicePotencias = velocidadDesead/velocidadActua;
+  pc3.printf(" %f ", potenciaMotores);
+  if(indicePotencias>1.0){
+    //Quiere acelerar
+    potenciaMotores+= 0.01;
+    
+    }
+  else if(indicePotencias<1.0&&indicePotencias>0.0){
+      potenciaMotores-= 0.01;
+    }
+  else if (indicePotencias < 0.0){
+        //Quiere ir marcha atras
+    }
+  //Limitamos la potencia del motor
+  
+  pinDAdelante.write(potenciaMotores);
+  pinIAdelante.write(potenciaMotores);
+  
+  }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mantieneVelocidad.h	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,1 @@
+void mantenerVelocidad(double velocidadDeseada, double velocidadActual, double &potenciaMotoresFrameAnterior, double &potenciaMotores, PwmOut pinDAdelante, PwmOut pinIAdelante);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 04 08:39:07 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058
\ No newline at end of file