yes
Dependencies: SparkfunAnalogJoystick
main.cpp
- Committer:
- erodrz
- Date:
- 2021-07-22
- Revision:
- 4:60e3365da280
- Parent:
- 3:3d54fd4109c0
- Child:
- 5:9f30f0a6dc76
File content as of revision 4:60e3365da280:
// 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" // 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 los comandos de voz recibidos por la Raspberry PI. Thread Thread7; // Hilo para manejar la selección de modo de la silla. // 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 = 15; // 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 // 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(PC_6); // Dirección del motor 1. PwmOut PWM_Velocidad1(PB_15); // Velocidad del motor 1. DigitalOut Direccion2(D14); // Dirección del motor 2. PwmOut PWM_Velocidad2(D15); // Velocidad del motor 2. // Salida para manejar la señal del buzzer de alertas. PwmOut Buzzer(D3); // Función para reproducir un sonido del buzzer cuando se detecta proximidad a un obstáculo. void Reproducir_Buzzer_Proximidad(void) { Timer BuzzTime; BuzzTime.reset(); BuzzTime.start(); while(BuzzTime.read_us() < 3000000) // 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.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); thread_sleep_for(200); Buzzer.period(1.0/Nota_E4); // 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); } BuzzTime.stop(); } // Función para limpiar caracteres presentes en el buffer de la interfaz serial. void LimpiarSerialBuffer(void) { char char1 = 0; while(PC.readable()) { char1 = PC.getc(); } return; } // Función para moder 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.0001f); // Declaramos el período. PWM_Velocidad1.write(0.15f); // %25 del duty cicle. PWM_Velocidad2.period(0.0001f); // Declaramos el período. PWM_Velocidad2.write(0.15f); // %25 del duty cicle. thread_sleep_for(Tiempo); PWM_Velocidad1.write(0.0f); PWM_Velocidad2.write(0.0f); } // Función para moder 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.0001f); // Declaramos el período. PWM_Velocidad1.write(0.15f); // %25 del duty cicle. PWM_Velocidad2.period(0.0001f); // Declaramos el período. PWM_Velocidad2.write(0.15f); // %25 del duty cicle. thread_sleep_for(Tiempo); PWM_Velocidad1.write(0.0f); PWM_Velocidad2.write(0.0f); } // Función para moder la silla hacia la izquierda. void Mover_Hacia_Izquierda(int Tiempo) { Direccion1 = 1; // En dirección contraria a las manecillas del reloj. Direccion2 = 0; // En dirección de las manecillas del reloj. PWM_Velocidad1.period(0.0001f); // Declaramos el período. PWM_Velocidad1.write(0.15f); // %25 del duty cicle. PWM_Velocidad2.period(0.0001f); // Declaramos el período. PWM_Velocidad2.write(0.15f); // %25 del duty cicle. thread_sleep_for(Tiempo); PWM_Velocidad1.write(0.0f); PWM_Velocidad2.write(0.0f); } // Función para moder la silla hacia la derecha. void Mover_Hacia_Derecha(int Tiempo) { Direccion1 = 0; // En dirección de las manecillas del reloj. Direccion2 = 1; // En dirección contraria a las manecillas del reloj. PWM_Velocidad1.period(0.0001f); // Declaramos el período. PWM_Velocidad1.write(0.15f); // %25 del duty cicle. PWM_Velocidad2.period(0.0001f); // Declaramos el período. PWM_Velocidad2.write(0.15f); // %25 del duty cicle. thread_sleep_for(Tiempo); PWM_Velocidad1.write(0.0f); PWM_Velocidad2.write(0.0f); } // Función para leer el sensor de proximidad 1. 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(); wait_us(10.0); Trigger = 0; while(Echo == 0) { }; Sonar.start(); while(Echo == 1) { }; Sonar.stop(); Distance1 = (Sonar.read_us()-Correccion)/58.0; //printf("Sensor de proximidad 1: %d cm \n\r",Distance1); thread_sleep_for(1000); } } // Función para leer el sensor de proximidad 2. 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(); wait_us(10.0); Trigger = 0; while(Echo == 0) { }; Sonar.start(); while(Echo == 1) { }; Sonar.stop(); Distance2 = (Sonar.read_us()-Correccion)/58.0; //printf("Sensor de proximidad 2: %d cm \n\r",Distance2); thread_sleep_for(1000); } } // Función para leer el sensor de proximidad 3. 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(); wait_us(10.0); Trigger = 0; while(Echo == 0) { }; Sonar.start(); while(Echo == 1) { }; Sonar.stop(); Distance3 = (Sonar.read_us()-Correccion)/58.0; //printf("Sensor de proximidad 3: %d cm \n\r",Distance3); thread_sleep_for(1000); } } // Función para leer el sensor de proximidad 4. 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(); wait_us(10.0); Trigger = 0; while(Echo == 0) { }; Sonar.start(); while(Echo == 1) { }; Sonar.stop(); Distance4 = (Sonar.read_us()-Correccion)/58.0; //printf("Sensor de proximidad 4: %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,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 del joystick: Hacia adelante. \r \n"); Mover_Hacia_Adelante(1000); // Mover por un segundo. } else { printf("Comandos del joystick: Obstaculo hacia adelante. \r \n"); Reproducir_Buzzer_Proximidad(); } thread_sleep_for(500); } if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f) { if(Distance2 > DistanceLimit) { printf("Comandos del joystick: Hacia atras. \r \n"); Mover_Hacia_Atras(1000); // Mover por un segundo. } else { printf("Comandos del joystick: Obstaculo hacia atras. \r \n"); Reproducir_Buzzer_Proximidad(); } thread_sleep_for(500); } if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f) { if(Distance3 > DistanceLimit) { printf("Comandos del joystick: Hacia la izquierda. \r \n"); Mover_Hacia_Izquierda(1000); // Mover por un segundo. } else { printf("Comandos del joystick: Obstaculo hacia la izquierda. \r \n"); Reproducir_Buzzer_Proximidad(); } thread_sleep_for(500); } if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f) { if(Distance4 > DistanceLimit) { printf("Comandos del joystick: Hacia la derecha. \r \n"); Mover_Hacia_Derecha(1000); // Mover por un segundo. } else { printf("Comandos del joystick: Obstaculo hacia la derecha. \r \n"); Reproducir_Buzzer_Proximidad(); } thread_sleep_for(500); } thread_sleep_for(5); } } } // Función para leer datos del serial con caracteres de comandos de voz y ejecutar instrucciones. void Thread6_ComandosVoz() { while(1) { if(!Modo1 && !Modo2 && Modo3 && !Modo4) { LimpiarSerialBuffer(); char c = PC.getc(); thread_sleep_for(5); // Retraso necesario para que el compilador se dé cuenta del orden correcto de ejecución. int m = Modo3.read(); printf("Estado del 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"); Reproducir_Buzzer_Proximidad(); } 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"); Reproducir_Buzzer_Proximidad(); } 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"); Reproducir_Buzzer_Proximidad(); } 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"); Reproducir_Buzzer_Proximidad(); } thread_sleep_for(1000); } } c = ' '; thread_sleep_for(5); } } } // Función para seleccionar el modo de operación de la silla. void Thread7_IndicarModo() { bool EstadoModo1 = false; bool EstadoModo2 = false; bool EstadoModo3 = false; bool EstadoModo4 = false; while(true) { 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); Thread5.start(Thread5_Joystick); thread_sleep_for(200); Thread6.start(Thread6_ComandosVoz); thread_sleep_for(200); Thread7.start(Thread7_IndicarModo); thread_sleep_for(200); }