File content as of revision 0:bd4170087cf1:

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


    }

}