File content as of revision 10:c036607dbf6e:

// Librerías para abstraer funciones de MBed para manejar el microprocesador y los sensores.
#include "mbed.h"
#include "platform/mbed_thread.h"
#include "SparkfunAnalogJoystick.h"
#include "MPU6050.h"

// Constantes de programación para procesos y funciones.
// Valor en frecuencia de las notas reproducidas por el Buzzer que da avisos al usuario de la silla.
#define Nota_C4 262
#define Nota_A4 440
#define Nota_E4 659

// Hilos para ejecutar tareas concurrentes.
Thread Thread1; // Primer hilo para sensor de proximidad.
Thread Thread2; // Segundo hilo para sensor de proximidad.
Thread Thread3; // Tercer hilo para sensor de proximidad.
Thread Thread4; // Cuarto hilo para sensor de proximidad.
Thread Thread5; // Hilo para manejar el joystick.
Thread Thread6; // Hilo para manejar la recepcion de caracteres.
Thread Thread7; // Hilo para manejar la condicion de parada de los comandos de voz.
Thread Thread8; // Hilo para manejar los comandos de voz recibidos por la Raspberry PI.
Thread Thread9; // Hilo para manejar la selección de modo de la silla.
Thread Thread10; // Hilo para manejar la detección de caídas y movimientos bruscos.

// Variables globales de distancias en el entorno de la silla de ruedas.
int Distance1 = 0; // Distancia adelante de la silla.
int Distance2 = 0; // Distancia atrás de la silla.
int Distance3 = 0; // Distancia a la izquierda de la silla.
int Distance4 = 0; // Distancia a la derecha de la silla.
int DistanceLimit = 100; // Distancia límite de acercamiento a un Obstaculo permitida por la silla.

// Entradas digitales para selección de modos de la silla.
DigitalIn modo1(D8); // Modo manual.
DigitalIn modo2(D9); // Modo por comandos del joystick.
DigitalIn modo3(D10); // Modo por comandos de voz.
DigitalIn modo4(D11); // Modo por rutas autónomas

// Variables modos globales.
int Modo1 = 0;
int Modo2 = 0;
int Modo3 = 0;
int Modo4 = 0;

// Variables de orientación de movimiento de la silla. (Para saber a dónde se mueve la silla en cada momento.)
int Orientacion_Adelante = 0;
int Orientacion_Atras = 0;
int Orientacion_Izquierda = 0;
int Orientacion_Derecha = 0;

// Interfaz serial para comunicación con la Raspberry PI.
Serial PC(USBTX,USBRX); // Por aquí se reciben caracteres que el miprocesador interpreta para ejecutar una acción, como los comandos de voz o alguna alerta.

// Salidas digitales y PWM para controlar el driver de los motores.
DigitalOut Direccion1(D15); // Dirección del motor 1.
PwmOut PWM_Velocidad1(D14); // Velocidad del motor 1.
DigitalOut Direccion2(PC_6); // Dirección del motor 2.
PwmOut PWM_Velocidad2(PB_15); // Velocidad del motor 2.

// Salida para manejar la señal del buzzer de alertas.
PwmOut Buzzer(PE_14);

// Objeto manejador del sensor acelerómetro y giroscopio.
MPU6050 MPUsensor(PB_11,PB_10);

//Variable global para los comandos de voz recibidos desde la raspberry PI.
char c;

// Función para reproducir un sonido del buzzer cuando se detecta un movimiento brusco.
void Reproducir_Buzzer_Caidas()
{
   Timer BuzzTime;
   BuzzTime.reset();
   BuzzTime.start();
   while(BuzzTime.read_us() < 1000000) // Ejecutar sonido del buzzer por 3 segundos.
   {
        Buzzer.period(1.0/Nota_C4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo).
        Buzzer.write(0.5);
        thread_sleep_for(200);
   }
   Buzzer.write(0);
   BuzzTime.stop();
}

// Función para detectar caídas y movimientos bruscos de la silla.
void Thread8_MPU6050()
{
    while(1)
    {
        float Acelerometro[3]; // Acelerometro[0] = X, Acelerometro[1] = Y, Acelerometro[2] = Z.
        MPUsensor.getAccelero(Acelerometro); // Obtener lectura del sensor.
        if(Acelerometro[0] >= 3)
        {
            //printf("Sensor de aceleracion: Movimiento brusco a la derecha.\n\r");
            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);

            PC.printf("Fall Event \n"); // Evento de caída detectado.
            Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso.
        }
        if(Acelerometro[0] <= -3)
        {
            //printf("Sensor de aceleracion: Movimiento brusco a la izquierda.\n\r");
            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
            
            PC.printf("Fall Event \n"); // Evento de caída detectado.
            Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso.
           
        }
        if(Acelerometro[1] >= 1)
        {
            //printf("Sensor de aceleracion: Movimiento brusco adelante.\n\r");
            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
        
            PC.printf("Fall Event \n"); // Evento de caída detectado.
            Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso.
            
        }
        if(Acelerometro[1] <= -6)
        {
            //printf("Sensor de aceleracion: Movimiento brusco atras.\n\r");
            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
            
            PC.printf("Fall Event \n"); // Evento de caída detectado.
            Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso.
        }
        if(Acelerometro[2] <= -8)
        {
            //printf("Sensor de aceleracion: Silla girada de cabeza.\n\r");
            //printf("Acelerometro en x: %f \n\r ",Acelerometro[0]);
            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
            
            PC.printf("Fall Event \n"); // Evento de caída detectado.
            Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso.
            
        }
        thread_sleep_for(200); // Delay.
    }
}

// Función para reproducir un sonido del buzzer cuando se detecta proximidad a un obstáculo.
void Reproducir_Buzzer_Proximidad()
{
   Timer BuzzTime;
   BuzzTime.reset();
   BuzzTime.start();
   while(BuzzTime.read_us() < 1000000) // Ejecutar sonido del buzzer por 1 segundo.
   {
        Buzzer.period(1.0/Nota_A4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo).
        Buzzer.write(0.5); // Duty cycle.
        thread_sleep_for(200); // Delay.
   }
   // Deteniendo el buzzer.
   Buzzer.write(0);
   BuzzTime.stop();
}

// Función para mover la silla hacia adelante.
void Mover_Hacia_Adelante(int Tiempo)
{
    Direccion1 = 0; // En dirección de las manecillas del reloj.
    Direccion2 = 0; // En dirección de las manecillas del reloj.

    PWM_Velocidad1.period(0.0003f); // Declaramos el período.
    PWM_Velocidad1.write(0.6f); // Duty cycle.

    PWM_Velocidad2.period(0.0003f); // Declaramos el período.
    PWM_Velocidad2.write(0.6f); // Duty cycle.

    thread_sleep_for(Tiempo);

    PWM_Velocidad1.write(0.0f);
    PWM_Velocidad2.write(0.0f);

    Orientacion_Adelante = 0;
    Orientacion_Atras = 0;
    Orientacion_Izquierda = 0;
    Orientacion_Derecha = 0;
}

// Función para mover la silla hacia atrás.
void Mover_Hacia_Atras(int Tiempo)
{
    Direccion1 = 1; // En dirección contraria a las manecillas del reloj.
    Direccion2 = 1; // En dirección contraria a las manecillas del reloj.

    PWM_Velocidad1.period(0.0003f); // Declaramos el período.
    PWM_Velocidad1.write(0.6f); // Duty cycle.
    
    PWM_Velocidad2.period(0.0003f); // Declaramos el período.
    PWM_Velocidad2.write(0.6f); // Duty cycle.

    thread_sleep_for(Tiempo); 

    PWM_Velocidad1.write(0.0f);
    PWM_Velocidad2.write(0.0f);

    Orientacion_Adelante = 0;
    Orientacion_Atras = 0;
    Orientacion_Izquierda = 0;
    Orientacion_Derecha = 0;
}

// Función para mover la silla hacia la izquierda.
void Mover_Hacia_Izquierda(int Tiempo)
{
    Direccion1 = 0; // En dirección contraria a las manecillas del reloj.
    Direccion2 = 1; // En dirección de las manecillas del reloj.

    PWM_Velocidad1.period(0.0003f); // Declaramos el período.
    PWM_Velocidad1.write(0.6f); // Duty cycle.

    PWM_Velocidad2.period(0.0003f); // Declaramos el período.
    PWM_Velocidad2.write(0.6f); // Duty cycle.

    thread_sleep_for(Tiempo); 

    PWM_Velocidad1.write(0.0f);
    PWM_Velocidad2.write(0.0f);

    Orientacion_Adelante = 0;
    Orientacion_Atras = 0;
    Orientacion_Izquierda = 0;
    Orientacion_Derecha = 0;
}

// Función para mover la silla hacia la derecha.
void Mover_Hacia_Derecha(int Tiempo)
{
    Direccion1 = 1; // En dirección de las manecillas del reloj.
    Direccion2 = 0; // En dirección contraria a las manecillas del reloj.

    PWM_Velocidad1.period(0.0003f); // Declaramos el período.
    PWM_Velocidad1.write(0.6f); // Duty cycle.

    PWM_Velocidad2.period(0.0003f); // Declaramos el período.
    PWM_Velocidad2.write(0.6f); // Duty cycle.

    thread_sleep_for(Tiempo);

    PWM_Velocidad1.write(0.0f);
    PWM_Velocidad2.write(0.0f);

    Orientacion_Adelante = 0;
    Orientacion_Atras = 0;
    Orientacion_Izquierda = 0;
    Orientacion_Derecha = 0;
}

// Función para detener la silla.
void Detener(int Tiempo)
{
    PWM_Velocidad1.write(0.0f); // Duty cicle.
    PWM_Velocidad2.write(0.0f); // Duty cicle.

    printf("    Deteniendo silla. \n\r");
    thread_sleep_for(Tiempo);

    Orientacion_Adelante = 0;
    Orientacion_Atras = 0;
    Orientacion_Izquierda = 0;
    Orientacion_Derecha = 0;
}

// Función para leer el sensor de proximidad 1. ADELANTE
void Thread1_HCSR04()
{
    DigitalOut Trigger(D0);
    DigitalIn Echo(D1);
    Timer Sonar;
    int Correccion = 0;
    Sonar.reset();
    Sonar.start();
    while(Echo == 2)
    {
        
    };
    Sonar.stop();
    Correccion = Sonar.read_us();
    printf("    Sensor de proximidad 1: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
    while(1)
    {
        Trigger = 1;
        Sonar.reset();
        thread_sleep_for(10);
        Trigger = 0;
        while(Echo == 0)
        {
            
        };
        Sonar.start();
        while(Echo == 1)
        {
            
        };
        Sonar.stop();
        Distance1 = (Sonar.read_us()-Correccion)/58.0;
        if(Distance1 < DistanceLimit && Orientacion_Adelante == 1)
        {
            Detener(1000);
            printf("    Sensor de proximidad 1 (Adelante): %d cm \n\r",Distance1);
        }
        thread_sleep_for(1000);
    }
}

// Función para leer el sensor de proximidad 2. // ATRÁS
void Thread2_HCSR04()
{
    DigitalOut Trigger(D2);
    DigitalIn  Echo(D3);
    Timer Sonar;
    int Correccion = 0;
    Sonar.reset();
    Sonar.start();
    while(Echo == 2)
    {
        
    };
    Sonar.stop();
    Correccion = Sonar.read_us();
    printf("    Sensor de proximidad 2: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
    while(1)
    {
        Trigger = 1;
        Sonar.reset();
        thread_sleep_for(10);
        Trigger = 0;
        while(Echo == 0)
        {
            
        };
        Sonar.start();
        while(Echo == 1)
        {
            
        };
        Sonar.stop();
        Distance2 = (Sonar.read_us()-Correccion)/58.0;

        if(Distance2 < DistanceLimit && Orientacion_Atras == 1)
        {
            Detener(1000);
            printf("    Sensor de proximidad 2 (Atras): %d cm \n\r",Distance2);
        }
        thread_sleep_for(1000);
    }
}

// Función para leer el sensor de proximidad 3. // IZQUIERDA
void Thread3_HCSR04()
{
    DigitalOut Trigger(D4);
    DigitalIn  Echo(D5);
    Timer Sonar;
    int Correccion = 0;
    Sonar.reset();
    Sonar.start();
    while(Echo == 2)
    {
        
    };
    Sonar.stop();
    Correccion = Sonar.read_us();
    printf("    Sensor de proximidad 3: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
    while(1)
    {
        Trigger = 1;
        Sonar.reset();
        thread_sleep_for(10);
        Trigger = 0;
        while(Echo == 0)
        {
            
        };
        Sonar.start();
        while(Echo == 1)
        {
            
        };
        Sonar.stop();
        Distance3 = (Sonar.read_us()-Correccion)/58.0;
        if(Distance3 < DistanceLimit && Orientacion_Izquierda == 1)
        {
            Detener(1000);
            printf("    Sensor de proximidad 3 (Izquierda): %d cm \n\r",Distance3);
        }
        thread_sleep_for(1000);
    }
}

// Función para leer el sensor de proximidad 4. // DERECHA
void Thread4_HCSR04()
{
    DigitalOut Trigger(D6);
    DigitalIn  Echo(D7);
    Timer Sonar;
    int Correccion = 0;
    Sonar.reset();
    Sonar.start();
    while(Echo == 2)
    {
        
    };
    Sonar.stop();
    Correccion = Sonar.read_us();
    printf("    Sensor de proximidad 4: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
    while(1)
    {
        Trigger = 1;
        Sonar.reset();
        thread_sleep_for(10);
        Trigger = 0;
        while(Echo == 0)
        {
            
        };
        Sonar.start();
        while(Echo == 1)
        {
            
        };
        Sonar.stop();
        Distance4 = (Sonar.read_us()-Correccion)/58.0;
        if(Distance4 < DistanceLimit && Orientacion_Derecha == 1)
        {
            Detener(1000);
            printf("    Sensor de proximidad 4 (Derecha): %d cm \n\r",Distance4);
        }
        thread_sleep_for(1000);
    }
}

// Función para leer valores del joystick y ejecutar sus comandos.
void Thread5_Joystick()
{
    SparkfunAnalogJoystick JoyStick(A1,A0,PE_0);
    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.70f && X <= 0.70f && Y >= 0.90f && Y <= 1.00f)
            {
                if(Distance2 > DistanceLimit)
                {
                    Orientacion_Atras = 1;
                    printf("    Comandos del joystick: Hacia atras. \r \n");
                    printf("    Distancia medida por sensor atras: %d cm \r \n",Distance2);
                    Mover_Hacia_Atras(2000);
                }
                else
                {
                    printf("    Comandos del joystick: Obstaculo hacia atras. \r \n");
                    printf("    Distancia medida por sensor atras: %d cm \r \n",Distance2);
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
            }
            if(X >= -0.70f && X <= 0.70f && Y <= -0.90f && Y >= -1.00f)
            {   
                if(Distance1 > DistanceLimit)
                {
                    Orientacion_Adelante = 1;
                    printf("    Comandos del joystick: Hacia adelante. \r \n");
                    printf("    Distancia medida por sensor adelante: %d cm \r \n",Distance1);
                    Mover_Hacia_Adelante(2000);
                }
                else
                {
                    printf("    Comandos del joystick: Obstaculo hacia adelante. \r \n");
                    printf("    Distancia medida por sensor adelante: %d cm \r \n",Distance1);
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
            }
            if(Y >= -0.70f && Y <= 0.70f && X <= -0.90f && X >= -1.00f)
            
            {
                if(Distance4 > DistanceLimit)
                {
                    Orientacion_Derecha = 1;
                    printf("    Comandos del joystick: Hacia la derecha. \r \n");
                    printf("    Distancia medida por sensor derecha: %d cm \r \n",Distance4);
                    Mover_Hacia_Derecha(2000);
                }
                else
                {
                    printf("    Comandos del joystick: Obstaculo hacia la derecha. \r \n");
                    printf("    Distancia medida por sensor derecha: %d cm \r \n",Distance4);
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
            }
            if(Y >= -0.70f && Y  <= 0.70f && X >= 0.90f && X <= 1.00f)
            {
     
                if(Distance3 > DistanceLimit)
                {
                    Orientacion_Izquierda = 1;
                    printf("    Comandos del joystick: Hacia la izquierda. \r \n");
                    printf("    Distancia medida por sensor izquierda: %d cm \r \n",Distance3);
                    Mover_Hacia_Izquierda(2000);
                }
                else
                {
                    printf("    Comandos del joystick: Obstaculo hacia la izquierda. \r \n");
                    printf("    Distancia medida por sensor izquierda: %d cm \r \n",Distance3);
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
            }
            thread_sleep_for(5);
        }
    }
}


void Thread6_RecepcionCaracteresComandovoz()
{

    while(1)
    {   
        while(PC.readable()) { // Para saber si hay algo que recibir en el puerto serial. 
            c = PC.getc();
            printf("    Datos recibidos: %c \r\n",c);
        }
            
    }
}

void Thread7_ParadaComandoVoz()
{

    while(1)
    {
        if(c == 'x')
        {
            if(!Modo1 && !Modo2 && Modo3 && !Modo4)
            {
                printf("    Comandos de voz: Detenerse. \r \n");
                Detener(1000);
                c = ' ';
            }
        }
    }
}


// Función para leer datos del serial con caracteres de comandos de voz y ejecutar instrucciones.
void Thread8_ComandosVoz()
{
    int Tiempo_Lineal = 60000;
    int Tiempo_Lateral = 3000;
    
    while(1)
    {
        if(!Modo1 && !Modo2 && Modo3 && !Modo4)
        {
            
            if(c == 'w')
            {
                printf("    Distance1 - %d cm \r \n",Distance1);
                if(Distance1 > DistanceLimit)
                 {
                    Orientacion_Adelante = 1;
                    printf("    Comandos de voz: Hacia adelante. \r \n");
                    
                    Mover_Hacia_Adelante(Tiempo_Lineal);
                    thread_sleep_for(200);
                 }
                else
                {
                    printf("    Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
                c = ' ';
            }
            if(c == 's')
            {
                printf("    Distance2 - %d cm \r \n",Distance2);
                if(Distance2 > DistanceLimit)
                {
                    Orientacion_Atras = 1;
                    printf("    Comandos de voz: Hacia atras. \r \n");
                    
                    Mover_Hacia_Atras(Tiempo_Lineal);
                    thread_sleep_for(200);
                }
                else
                {
                    printf("    Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
                c = ' '; 
            }
            if(c == 'a')
            {
                printf("    Distance3 - %d cm \r \n",Distance3);
                if(Distance3 > DistanceLimit)
                {
                    Orientacion_Izquierda = 1;
                    printf("    Comandos de voz: Hacia la izquierda. \r \n");
                    
                    Mover_Hacia_Izquierda(Tiempo_Lateral);
                    thread_sleep_for(200);
                }
                else
                {
                    printf("    Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
                c = ' ';
            }
            if(c == 'd')
            {
                printf("    Distance4 - %d cm \r \n",Distance4);
                if(Distance4 > DistanceLimit)
                {
                    Orientacion_Derecha = 1;
                    printf("    Comandos de voz: Hacia la derecha. \r \n");
                    
                    Mover_Hacia_Derecha(Tiempo_Lateral);
                    thread_sleep_for(200);
                }
                else
                {
                    printf("    Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
                    Detener(1000);
                    //Reproducir_Buzzer_Proximidad();
                }
                c = ' ';
            }
            
        }
    }
}





// Función para seleccionar el modo de operación de la silla.
void Thread9_IndicarModo()
{
    bool EstadoModo1 = false;
    bool EstadoModo2 = false;
    bool EstadoModo3 = false;
    bool EstadoModo4 = false;

    while(true)
    {
        
        if(modo1 > 0.15f) {
            Modo1 = 1;
        } else {
            Modo1 = 0;
        }
        
        
        if(modo2 > 0.15f) {
            Modo2 = 1;
        } else {
            Modo2 = 0;
        }
        
        if(modo3 > 0.15f) {
            Modo3 = 1;
        } else {
            Modo3 = 0;
        }
        
        if(modo4 > 0.15f) {
            Modo4 = 1;
        } else {
            Modo4 = 0;
        }

        if(Modo1 && !Modo2 && !Modo3 && !Modo4 && !EstadoModo1)
        {
            printf("    Operando: Modo manual. \r \n");
            EstadoModo1 = true;
            EstadoModo2 = false;
            EstadoModo3 = false;
            EstadoModo4 = false;
        }
        if(!Modo1 && Modo2 && !Modo3 && !Modo4 && !EstadoModo2)
        {
            printf("    Operando: Modo de comandos de joystick. \r \n");
            EstadoModo1 = false;
            EstadoModo2 = true;
            EstadoModo3 = false;
            EstadoModo4 = false;
        }
        if(!Modo1 && !Modo2 && Modo3 && !Modo4 && !EstadoModo3)
        {
            printf("    Operando: Modo de comandos de voz. \r \n");
            EstadoModo1 = false;
            EstadoModo2 = false;
            EstadoModo3 = true;
            EstadoModo4 = false;
        }
        if(!Modo1 && !Modo2 && !Modo3 && Modo4 && !EstadoModo4)
        {
            printf("    Operando: Modo de rutas autonomas. \r \n");
            EstadoModo1 = false;
            EstadoModo2 = false;
            EstadoModo3 = false;
            EstadoModo4 = true;
        }
    }
}

// Proceso principal de todo el software ejecutado por el microprocesador.
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);
    
    Thread6.start(Thread6_RecepcionCaracteresComandovoz);
    thread_sleep_for(200);
    
    Thread7.start(Thread7_ParadaComandoVoz);
    thread_sleep_for(200);
    
    
    Thread9.start(Thread9_IndicarModo);
    thread_sleep_for(200);
    Thread5.start(Thread5_Joystick);
    thread_sleep_for(200);
    Thread8.start(Thread8_ComandosVoz);
    thread_sleep_for(200);
    
    
    
    //Thread8.start(Thread8_MPU6050);
    //thread_sleep_for(200);
}