File content as of revision 3:3d54fd4109c0:

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "SparkfunAnalogJoystick.h"

#define M_PI 3.14159265358979323846

Thread thread1;
Thread thread2;
Thread thread3;
Thread thread4;
Thread thread5;
Thread thread6;
Thread thread7;

int distance1 = 0; //distancia  adelante.  
int distance2 = 0; //distancia atras.
int distance3 = 0; // distancia izquierda.
int distance4 = 0; // distancia derecha. 

int distanceLimit = 15;

DigitalIn modo1(D8);  //Modo manual
DigitalIn modo2(D9);  //Modo comandos joystick
DigitalIn modo3(D10); //Modo comandos de voz
DigitalIn modo4(D11); //Modo rutas autonomas

Serial pc(USBTX, USBRX); //Para recibir los comandos de voz.


//motor driver
DigitalOut direccion1(PC_6);
PwmOut PWM_velocidad1(PB_15);

DigitalOut direccion2(D14);
PwmOut PWM_velocidad2(D15);


void flushSerialBuffer(void)
{
    char char1 = 0;
    while (pc.readable())
    {
        char1 = pc.getc();
    }
    return;
}


void mover_hacia_adelante(int tiempo){
    direccion1 = 0; // En direccion de las manecillas del reloj.
    direccion2 = 0; // En direccion de las manecillas del reloj.
    
    
    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
    PWM_velocidad1.write(0.15f); //%25
    
    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
    PWM_velocidad2.write(0.15f); //25%
    
    
    thread_sleep_for(tiempo); 
    
    PWM_velocidad1.write(0.0f);
    PWM_velocidad2.write(0.0f);
}

void mover_hacia_atras(int tiempo){
    direccion1 = 1; // En direccion de las manecillas del reloj.
    direccion2 = 1; // En direccion de las manecillas del reloj.
    
    
    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
    PWM_velocidad1.write(0.15f); //%25
    
    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
    PWM_velocidad2.write(0.15f); //25%
    
    
    thread_sleep_for(tiempo); 
    
    PWM_velocidad1.write(0.0f);
    PWM_velocidad2.write(0.0f);
}


void mover_hacia_izquierda(int tiempo){
    direccion1 = 1; // En direccion de las manecillas del reloj.
    direccion2 = 0; // En direccion de las manecillas del reloj.
    
    
    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
    PWM_velocidad1.write(0.15f); //%25
    
    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
    PWM_velocidad2.write(0.15f); //25%
    
    
    thread_sleep_for(tiempo); 
    
    PWM_velocidad1.write(0.0f);
    PWM_velocidad2.write(0.0f);
}


void mover_hacia_derecha(int tiempo){
    direccion1 = 0; // En direccion de las manecillas del reloj.
    direccion2 = 1; // En direccion de las manecillas del reloj.
    
    
    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
    PWM_velocidad1.write(0.15f); //%25
    
    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
    PWM_velocidad2.write(0.15f); //25%
    
    
    thread_sleep_for(tiempo); 
    
    PWM_velocidad1.write(0.0f);
    PWM_velocidad2.write(0.0f);
}



void thread1_HCSR04()
{
    DigitalOut trigger(D0);
    DigitalIn  echo(D1);
    Timer sonar;
    int correction = 0;
    sonar.reset();
    // measure actual software polling timer delays
    // delay used later in time correction
    // start timer
    sonar.start();
    // min software polling delay to read echo pin
    while (echo==2)
    {
        
    };
    // stop timer
    sonar.stop();
    // read timer
    correction = sonar.read_us();
    printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
    //Loop to read Sonar distance values, scale, and print
    while(1)
    {
        //trigger sonar to send a ping
        trigger = 1;
        sonar.reset();
        wait_us(10.0);
        trigger = 0;
        //wait for echo high
        while (echo==0)
        {
            
        };
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echo==1) {};
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance1 = (sonar.read_us()-correction)/58.0;
        //printf("Sensor proximidad 1: %d cm \n\r",distance1);
        //wait so that any echo(s) return before sending another ping
        thread_sleep_for(1000);
    }
}

void thread2_HCSR04()
{
    DigitalOut trigger(D2);
    DigitalIn  echo(D3);
    Timer sonar;
    int correction = 0;
    sonar.reset();
    // measure actual software polling timer delays
    // delay used later in time correction
    // start timer
    sonar.start();
    // min software polling delay to read echo pin
    while (echo==2)
    {
        
    };
    // stop timer
    sonar.stop();
    // read timer
    correction = sonar.read_us();
    printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
    //Loop to read Sonar distance values, scale, and print
    while(1)
    {
        // trigger sonar to send a ping
        trigger = 1;
        sonar.reset();
        wait_us(10.0);
        trigger = 0;
        //wait for echo high
        while (echo==0)
        {
            
        };
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echo==1)
        {
            
        };
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance2 = (sonar.read_us()-correction)/58.0;
        //printf("Sensor proximidad 2: %d cm \n\r",distance2);
        //wait so that any echo(s) return before sending another ping
        thread_sleep_for(1000);
    }
}

void thread3_HCSR04()
{
    DigitalOut trigger(D4);
    DigitalIn  echo(D5);
    Timer sonar;
    int correction = 0;
    sonar.reset();
    // measure actual software polling timer delays
    // delay used later in time correction
    // start timer
    sonar.start();
    // min software polling delay to read echo pin
    while (echo==2)
    {
        
    };
    // stop timer
    sonar.stop();
    // read timer
    correction = sonar.read_us();
    printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
    //Loop to read Sonar distance values, scale, and print
    while(1)
    {
        // trigger sonar to send a ping
        trigger = 1;
        sonar.reset();
        wait_us(10.0);
        trigger = 0;
        //wait for echo high
        while (echo==0)
        {
            
        };
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echo==1)
        {
            
        };
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance3 = (sonar.read_us()-correction)/58.0;
        //printf("Sensor proximidad 3: %d cm \n\r",distance3);
        //wait so that any echo(s) return before sending another ping
        thread_sleep_for(1000);
    }
}

void thread4_HCSR04()
{
    DigitalOut trigger(D6);
    DigitalIn  echo(D7);
    Timer sonar;
    int correction = 0;
    sonar.reset();
    // measure actual software polling timer delays
    // delay used later in time correction
    // start timer
    sonar.start();
    // min software polling delay to read echo pin
    while (echo==2)
    {
        
    };
    // stop timer
    sonar.stop();
    // read timer
    correction = sonar.read_us();
    printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction);
    //Loop to read Sonar distance values, scale, and print
    while(1)
    {
        // trigger sonar to send a ping
        trigger = 1;
        sonar.reset();
        wait_us(10.0);
        trigger = 0;
        //wait for echo high
        while (echo==0)
        {
            
        };
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echo==1)
        {
            
        };
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance4 = (sonar.read_us()-correction)/58.0;
        //printf("Sensor proximidad 4: %d cm \n\r",distance4);
        //wait so that any echo(s) return before sending another ping
        thread_sleep_for(1000);
    }
}

void thread5_Joystick()
{
    SparkfunAnalogJoystick JoyStick(A1, A0, D12);
    float X;
    float Y;
    while(1)
    {
        if(!modo1 && modo2 && !modo3 && !modo4)
        {
            X = JoyStick.xAxis();
            Y = JoyStick.yAxis();
            /*
            printf("X-Axis: %f\n\r", X);
            printf("Y-Axis: %f\n\r", Y);
            printf(" \n\r");
            */
            if(X >= -0.60f && X  <= 0.60f && Y >= 0.90f && Y <= 1.00f )
            {        
                if(distance1 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia adelante \r \n"); 
                    mover_hacia_adelante(1000); // un segundo 
                }
                else
                {
                    printf("Comandos joystick: Obstaculo adelante \r \n"); 
                }
                thread_sleep_for(500);   
            }
            if(X >= -0.60f && X  <= 0.60f && Y <= -0.90f && Y >= -1.00f)
            {
                if(distance2 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia atras \r \n"); 
                    mover_hacia_atras(1000); // un segundo
                }
                else
                {
                    printf("Comandos joystick: Obstaculo atras \r \n"); 
                } 
                thread_sleep_for(500); 
            }
            if(Y >= -0.60f && Y  <= 0.60f && X <= -0.90f && X >= -1.00f)
            {
                if(distance3 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia izquierda \r \n"); 
                    mover_hacia_izquierda(1000); //un segundo
                }
                else
                {
                    printf("Comandos joystick: Obstaculo izquierda \r \n"); 
                }
                thread_sleep_for(500);    
            }
            if(Y >= -0.60f && Y  <= 0.60f && X >= 0.90f && X <= 1.00f)
            {
                if(distance4 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia derecha \r \n"); 
                    mover_hacia_derecha(1000); // segundo
                }
                else
                {
                    printf("Comandos joystick: Obstaculo derecha \r \n"); 
                }
                thread_sleep_for(500);
            }
            thread_sleep_for(5); 
        }
    }
}

void thread6_ComandosVoz()
{
    while(1)
    {
        if(!modo1 && !modo2 && modo3 && !modo4)
        {
            flushSerialBuffer();
            char c = pc.getc();
            
            thread_sleep_for(5); //timer necesario para que el compilar se de cuenta el orden correcto de ejecucion
            
            int m = modo3.read();
            printf("estado modo 3 (comandos de voz): %d \r \n",m);
        
            if(m == 1){     
                if(c == 'w')
                {
                    //printf("Distance1 - %d \r \n",distance1);
                    if(distance1 > distanceLimit)
                    {
                        pc.printf(" Comandos de voz: Hacia adelante \r \n");
                        mover_hacia_adelante(3000);
                    }
                    else
                    {
                        printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");      
                    }
                    thread_sleep_for(1000);
                }
                if(c == 's')
                {
                    //printf("Distance2 - %d \r \n",distance2);
                    if(distance2 > distanceLimit)
                    {
                        pc.printf(" Comandos de voz: Hacia atras \r \n");
                        mover_hacia_atras(3000); 
                    }
                    else
                    {
                        printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");      
                    }
                    thread_sleep_for(1000);
                }                           
                if(c == 'a')
                {
                    //printf("Distance3 - %d \r \n",distance3);
                    if(distance3 > distanceLimit)
                    {
                        pc.printf(" Comandos de voz: Hacia la izquierda  \r \n");
                        mover_hacia_izquierda(3000);
                    }
                    else
                    {
                        printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la izquierda. \r \n");      
                    }
                    thread_sleep_for(1000);
                }
                if(c == 'd')
                { 
                    //printf("Distance4 - %d \r \n",distance4);
                    if(distance4 > distanceLimit)
                    {
                        pc.printf(" Comandos de voz: Hacia la derecha \r \n");
                        mover_hacia_derecha(3000);
                    } 
                    else
                    {
                        printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la derecha. \r \n");      
                    }
                    thread_sleep_for(1000);
                }
            }
            c = ' ';
            thread_sleep_for(5); 
        }
    }
}

void thread7_IndicarModo()
{
    bool estadomodo1 = false;
    bool estadomodo2 = false;
    bool estadomodo3 = false;
    bool estadomodo4 = false;
    while (true)
    {
        if(modo1 && !modo2 && !modo3 && !modo4 && !estadomodo1)
        {
            printf("Modo manual  \r \n");
            estadomodo1 = true;
            estadomodo2 = false;
            estadomodo3 = false;
            estadomodo4 = false;
        }
        if(!modo1 && modo2 && !modo3 && !modo4 && !estadomodo2)
        {
            printf("Modo comandos joystick  \r \n");
            estadomodo1 = false;
            estadomodo2 = true;
            estadomodo3 = false;
            estadomodo4 = false;
        }   
        if(!modo1 && !modo2 && modo3 && !modo4 && !estadomodo3)
        {
            printf("Modo comandos de voz  \r \n");
            estadomodo1 = false;
            estadomodo2 = false;
            estadomodo3 = true;
            estadomodo4 = false;
        }   
        if(!modo1 && !modo2 && !modo3 && modo4 && !estadomodo4)
        {
            printf("Modo rutas autonomas \r \n");
            estadomodo1 = false;
            estadomodo2 = false;
            estadomodo3 = false;
            estadomodo4 = true;
        }       
    }
}

int main()
{
    thread1.start(thread1_HCSR04);
    thread_sleep_for(200);
    thread2.start(thread2_HCSR04);
    thread_sleep_for(200);
    thread3.start(thread3_HCSR04);
    thread_sleep_for(200);
    thread4.start(thread4_HCSR04);
    thread_sleep_for(200);
    thread5.start(thread5_Joystick);
    thread_sleep_for(200);
    thread6.start(thread6_ComandosVoz);
    thread_sleep_for(200);
    thread7.start(thread7_IndicarModo);
    thread_sleep_for(200);
}