File content as of revision 1:17ea74f31633:

#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 = 10;

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.

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"); 
                }
                else
                {
                    printf("Comandos joystick: Obstaculo adelante \r \n"); 
                }
                thread_sleep_for(1000);   
            }
            if(X >= -0.60f && X  <= 0.60f && Y <= -0.90f && Y >= -1.00f)
            {
                if(distance2 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia atras \r \n"); 
                }
                else
                {
                    printf("Comandos joystick: Obstaculo atras \r \n"); 
                } 
                thread_sleep_for(1000); 
            }
            if(Y >= -0.60f && Y  <= 0.60f && X <= -0.90f && X >= -1.00f)
            {
                if(distance3 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia izquierda \r \n"); 
                }
                else
                {
                    printf("Comandos joystick: Obstaculo izquierda \r \n"); 
                }
                thread_sleep_for(1000);    
            }
            if(Y >= -0.60f && Y  <= 0.60f && X >= 0.90f && X <= 1.00f)
            {
                if(distance4 > distanceLimit)
                {
                    printf("Comandos joystick: Hacia derecha \r \n"); 
                }
                else
                {
                    printf("Comandos joystick: Obstaculo derecha \r \n"); 
                }
                thread_sleep_for(1000);   
            }
            thread_sleep_for(5); 
        }
    }
}

void thread6_ComandosVoz()
{
    char c = pc.getc(); 
    while(1)
    {
        if(!modo1 && !modo2 && modo3 && !modo4)
        {
            if(c == 'w')
            {
                if(distance1 > distanceLimit )
                {
                    pc.printf(" Comandos de voz: Hacia adelante \r \n");
                    thread_sleep_for(500);
                }
                else
                {
                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");      
                }  
            }        
            if(c == 'd')
            {
                if(distance2 > distanceLimit)
                {
                    pc.printf(" Comandos de voz: Hacia la derecha \r \n");
                    thread_sleep_for(500);
                }
                else
                {
                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");      
                }
            }                           
            if(c == 's')
            {
                if(distance3 > distanceLimit)
                {
                    pc.printf(" Comandos de voz: Hacia atrás \r \n");
                    thread_sleep_for(500);
                }
                else
                {
                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la izquierda. \r \n");      
                }
            }
            if(c == 'a')
            { 
                if(distance4 > distanceLimit)
                {
                    pc.printf(" Comandos de voz: Hacia la izquierda \r \n");
                    thread_sleep_for(500);
                } 
                else
                {
                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la derecha. \r \n");      
                }               
            }                 
        }                        
    }
}

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);
    thread2.start(thread2_HCSR04);
    thread3.start(thread3_HCSR04);
    thread4.start(thread4_HCSR04);
    thread5.start(thread5_Joystick);
    thread6.start(thread6_ComandosVoz);
    thread7.start(thread7_IndicarModo);
}