Newest version of Wheelchair Logic
Dependencies: MPU6050 SparkfunAnalogJoystick
main.cpp@10:c036607dbf6e, 2021-11-05 (annotated)
- Committer:
- erodrz
- Date:
- Fri Nov 05 00:26:15 2021 +0000
- Revision:
- 10:c036607dbf6e
- Parent:
- 9:4a1c40f8b2d7
WheelchairLogicv5
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
erodrz | 4:60e3365da280 | 1 | // Librerías para abstraer funciones de MBed para manejar el microprocesador y los sensores. |
thevic16 | 0:b8adbf13199b | 2 | #include "mbed.h" |
thevic16 | 0:b8adbf13199b | 3 | #include "platform/mbed_thread.h" |
thevic16 | 0:b8adbf13199b | 4 | #include "SparkfunAnalogJoystick.h" |
erodrz | 7:ae7b2184b737 | 5 | #include "MPU6050.h" |
thevic16 | 0:b8adbf13199b | 6 | |
erodrz | 4:60e3365da280 | 7 | // Constantes de programación para procesos y funciones. |
erodrz | 4:60e3365da280 | 8 | // Valor en frecuencia de las notas reproducidas por el Buzzer que da avisos al usuario de la silla. |
erodrz | 4:60e3365da280 | 9 | #define Nota_C4 262 |
erodrz | 4:60e3365da280 | 10 | #define Nota_A4 440 |
erodrz | 4:60e3365da280 | 11 | #define Nota_E4 659 |
erodrz | 4:60e3365da280 | 12 | |
erodrz | 4:60e3365da280 | 13 | // Hilos para ejecutar tareas concurrentes. |
erodrz | 4:60e3365da280 | 14 | Thread Thread1; // Primer hilo para sensor de proximidad. |
erodrz | 4:60e3365da280 | 15 | Thread Thread2; // Segundo hilo para sensor de proximidad. |
erodrz | 4:60e3365da280 | 16 | Thread Thread3; // Tercer hilo para sensor de proximidad. |
erodrz | 4:60e3365da280 | 17 | Thread Thread4; // Cuarto hilo para sensor de proximidad. |
erodrz | 4:60e3365da280 | 18 | Thread Thread5; // Hilo para manejar el joystick. |
erodrz | 10:c036607dbf6e | 19 | Thread Thread6; // Hilo para manejar la recepcion de caracteres. |
erodrz | 10:c036607dbf6e | 20 | Thread Thread7; // Hilo para manejar la condicion de parada de los comandos de voz. |
erodrz | 10:c036607dbf6e | 21 | Thread Thread8; // Hilo para manejar los comandos de voz recibidos por la Raspberry PI. |
erodrz | 10:c036607dbf6e | 22 | Thread Thread9; // Hilo para manejar la selección de modo de la silla. |
erodrz | 10:c036607dbf6e | 23 | Thread Thread10; // Hilo para manejar la detección de caídas y movimientos bruscos. |
thevic16 | 0:b8adbf13199b | 24 | |
erodrz | 4:60e3365da280 | 25 | // Variables globales de distancias en el entorno de la silla de ruedas. |
erodrz | 4:60e3365da280 | 26 | int Distance1 = 0; // Distancia adelante de la silla. |
erodrz | 4:60e3365da280 | 27 | int Distance2 = 0; // Distancia atrás de la silla. |
erodrz | 4:60e3365da280 | 28 | int Distance3 = 0; // Distancia a la izquierda de la silla. |
erodrz | 4:60e3365da280 | 29 | int Distance4 = 0; // Distancia a la derecha de la silla. |
thevic16 | 8:418ff0ef9463 | 30 | int DistanceLimit = 100; // Distancia límite de acercamiento a un Obstaculo permitida por la silla. |
thevic16 | 0:b8adbf13199b | 31 | |
erodrz | 4:60e3365da280 | 32 | // Entradas digitales para selección de modos de la silla. |
thevic16 | 9:4a1c40f8b2d7 | 33 | DigitalIn modo1(D8); // Modo manual. |
thevic16 | 9:4a1c40f8b2d7 | 34 | DigitalIn modo2(D9); // Modo por comandos del joystick. |
thevic16 | 9:4a1c40f8b2d7 | 35 | DigitalIn modo3(D10); // Modo por comandos de voz. |
thevic16 | 9:4a1c40f8b2d7 | 36 | DigitalIn modo4(D11); // Modo por rutas autónomas |
thevic16 | 9:4a1c40f8b2d7 | 37 | |
erodrz | 10:c036607dbf6e | 38 | // Variables modos globales. |
erodrz | 10:c036607dbf6e | 39 | int Modo1 = 0; |
erodrz | 10:c036607dbf6e | 40 | int Modo2 = 0; |
erodrz | 10:c036607dbf6e | 41 | int Modo3 = 0; |
erodrz | 10:c036607dbf6e | 42 | int Modo4 = 0; |
thevic16 | 9:4a1c40f8b2d7 | 43 | |
erodrz | 10:c036607dbf6e | 44 | // Variables de orientación de movimiento de la silla. (Para saber a dónde se mueve la silla en cada momento.) |
erodrz | 10:c036607dbf6e | 45 | int Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 46 | int Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 47 | int Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 48 | int Orientacion_Derecha = 0; |
thevic16 | 0:b8adbf13199b | 49 | |
erodrz | 4:60e3365da280 | 50 | // Interfaz serial para comunicación con la Raspberry PI. |
erodrz | 4:60e3365da280 | 51 | 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. |
thevic16 | 0:b8adbf13199b | 52 | |
erodrz | 4:60e3365da280 | 53 | // Salidas digitales y PWM para controlar el driver de los motores. |
erodrz | 5:9f30f0a6dc76 | 54 | DigitalOut Direccion1(D15); // Dirección del motor 1. |
erodrz | 5:9f30f0a6dc76 | 55 | PwmOut PWM_Velocidad1(D14); // Velocidad del motor 1. |
erodrz | 5:9f30f0a6dc76 | 56 | DigitalOut Direccion2(PC_6); // Dirección del motor 2. |
erodrz | 5:9f30f0a6dc76 | 57 | PwmOut PWM_Velocidad2(PB_15); // Velocidad del motor 2. |
thevic16 | 1:17ea74f31633 | 58 | |
erodrz | 4:60e3365da280 | 59 | // Salida para manejar la señal del buzzer de alertas. |
erodrz | 5:9f30f0a6dc76 | 60 | PwmOut Buzzer(PE_14); |
thevic16 | 3:3d54fd4109c0 | 61 | |
erodrz | 7:ae7b2184b737 | 62 | // Objeto manejador del sensor acelerómetro y giroscopio. |
erodrz | 7:ae7b2184b737 | 63 | MPU6050 MPUsensor(PB_11,PB_10); |
erodrz | 7:ae7b2184b737 | 64 | |
erodrz | 10:c036607dbf6e | 65 | //Variable global para los comandos de voz recibidos desde la raspberry PI. |
erodrz | 10:c036607dbf6e | 66 | char c; |
erodrz | 10:c036607dbf6e | 67 | |
erodrz | 7:ae7b2184b737 | 68 | // Función para reproducir un sonido del buzzer cuando se detecta un movimiento brusco. |
erodrz | 7:ae7b2184b737 | 69 | void Reproducir_Buzzer_Caidas() |
erodrz | 4:60e3365da280 | 70 | { |
erodrz | 4:60e3365da280 | 71 | Timer BuzzTime; |
erodrz | 4:60e3365da280 | 72 | BuzzTime.reset(); |
erodrz | 4:60e3365da280 | 73 | BuzzTime.start(); |
erodrz | 7:ae7b2184b737 | 74 | while(BuzzTime.read_us() < 1000000) // Ejecutar sonido del buzzer por 3 segundos. |
erodrz | 4:60e3365da280 | 75 | { |
erodrz | 4:60e3365da280 | 76 | Buzzer.period(1.0/Nota_C4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo). |
erodrz | 4:60e3365da280 | 77 | Buzzer.write(0.5); |
erodrz | 4:60e3365da280 | 78 | thread_sleep_for(200); |
erodrz | 7:ae7b2184b737 | 79 | } |
erodrz | 7:ae7b2184b737 | 80 | Buzzer.write(0); |
erodrz | 7:ae7b2184b737 | 81 | BuzzTime.stop(); |
erodrz | 7:ae7b2184b737 | 82 | } |
erodrz | 7:ae7b2184b737 | 83 | |
erodrz | 7:ae7b2184b737 | 84 | // Función para detectar caídas y movimientos bruscos de la silla. |
thevic16 | 8:418ff0ef9463 | 85 | void Thread8_MPU6050() |
erodrz | 7:ae7b2184b737 | 86 | { |
erodrz | 7:ae7b2184b737 | 87 | while(1) |
erodrz | 7:ae7b2184b737 | 88 | { |
erodrz | 10:c036607dbf6e | 89 | float Acelerometro[3]; // Acelerometro[0] = X, Acelerometro[1] = Y, Acelerometro[2] = Z. |
erodrz | 10:c036607dbf6e | 90 | MPUsensor.getAccelero(Acelerometro); // Obtener lectura del sensor. |
thevic16 | 8:418ff0ef9463 | 91 | if(Acelerometro[0] >= 3) |
erodrz | 7:ae7b2184b737 | 92 | { |
thevic16 | 9:4a1c40f8b2d7 | 93 | //printf("Sensor de aceleracion: Movimiento brusco a la derecha.\n\r"); |
thevic16 | 9:4a1c40f8b2d7 | 94 | //printf("Acelerometro en x: %f \n\r",Acelerometro[0]); |
thevic16 | 9:4a1c40f8b2d7 | 95 | //printf("Acelerometro en y: %f \n\r",Acelerometro[1]); |
thevic16 | 9:4a1c40f8b2d7 | 96 | //printf("Acelerometro en z: %f \n\r",Acelerometro[2]); |
thevic16 | 9:4a1c40f8b2d7 | 97 | |
erodrz | 10:c036607dbf6e | 98 | PC.printf("Fall Event \n"); // Evento de caída detectado. |
erodrz | 10:c036607dbf6e | 99 | Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso. |
erodrz | 7:ae7b2184b737 | 100 | } |
thevic16 | 8:418ff0ef9463 | 101 | if(Acelerometro[0] <= -3) |
erodrz | 7:ae7b2184b737 | 102 | { |
thevic16 | 9:4a1c40f8b2d7 | 103 | //printf("Sensor de aceleracion: Movimiento brusco a la izquierda.\n\r"); |
thevic16 | 9:4a1c40f8b2d7 | 104 | //printf("Acelerometro en x: %f \n\r",Acelerometro[0]); |
thevic16 | 9:4a1c40f8b2d7 | 105 | //printf("Acelerometro en y: %f \n\r",Acelerometro[1]); |
thevic16 | 9:4a1c40f8b2d7 | 106 | //printf("Acelerometro en z: %f \n\r",Acelerometro[2]); |
thevic16 | 9:4a1c40f8b2d7 | 107 | |
erodrz | 10:c036607dbf6e | 108 | PC.printf("Fall Event \n"); // Evento de caída detectado. |
erodrz | 10:c036607dbf6e | 109 | Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso. |
thevic16 | 9:4a1c40f8b2d7 | 110 | |
erodrz | 7:ae7b2184b737 | 111 | } |
thevic16 | 8:418ff0ef9463 | 112 | if(Acelerometro[1] >= 1) |
erodrz | 7:ae7b2184b737 | 113 | { |
thevic16 | 9:4a1c40f8b2d7 | 114 | //printf("Sensor de aceleracion: Movimiento brusco adelante.\n\r"); |
thevic16 | 9:4a1c40f8b2d7 | 115 | //printf("Acelerometro en x: %f \n\r",Acelerometro[0]); |
thevic16 | 9:4a1c40f8b2d7 | 116 | //printf("Acelerometro en y: %f \n\r",Acelerometro[1]); |
thevic16 | 9:4a1c40f8b2d7 | 117 | //printf("Acelerometro en z: %f \n\r",Acelerometro[2]); |
thevic16 | 9:4a1c40f8b2d7 | 118 | |
erodrz | 10:c036607dbf6e | 119 | PC.printf("Fall Event \n"); // Evento de caída detectado. |
erodrz | 10:c036607dbf6e | 120 | Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso. |
thevic16 | 9:4a1c40f8b2d7 | 121 | |
erodrz | 7:ae7b2184b737 | 122 | } |
thevic16 | 8:418ff0ef9463 | 123 | if(Acelerometro[1] <= -6) |
erodrz | 7:ae7b2184b737 | 124 | { |
thevic16 | 9:4a1c40f8b2d7 | 125 | //printf("Sensor de aceleracion: Movimiento brusco atras.\n\r"); |
thevic16 | 9:4a1c40f8b2d7 | 126 | //printf("Acelerometro en x: %f \n\r",Acelerometro[0]); |
thevic16 | 9:4a1c40f8b2d7 | 127 | //printf("Acelerometro en y: %f \n\r",Acelerometro[1]); |
thevic16 | 9:4a1c40f8b2d7 | 128 | //printf("Acelerometro en z: %f \n\r",Acelerometro[2]); |
thevic16 | 9:4a1c40f8b2d7 | 129 | |
erodrz | 10:c036607dbf6e | 130 | PC.printf("Fall Event \n"); // Evento de caída detectado. |
erodrz | 10:c036607dbf6e | 131 | Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso. |
erodrz | 7:ae7b2184b737 | 132 | } |
thevic16 | 8:418ff0ef9463 | 133 | if(Acelerometro[2] <= -8) |
erodrz | 7:ae7b2184b737 | 134 | { |
thevic16 | 9:4a1c40f8b2d7 | 135 | //printf("Sensor de aceleracion: Silla girada de cabeza.\n\r"); |
thevic16 | 9:4a1c40f8b2d7 | 136 | //printf("Acelerometro en x: %f \n\r ",Acelerometro[0]); |
thevic16 | 9:4a1c40f8b2d7 | 137 | //printf("Acelerometro en y: %f \n\r",Acelerometro[1]); |
thevic16 | 9:4a1c40f8b2d7 | 138 | //printf("Acelerometro en z: %f \n\r",Acelerometro[2]); |
thevic16 | 9:4a1c40f8b2d7 | 139 | |
erodrz | 10:c036607dbf6e | 140 | PC.printf("Fall Event \n"); // Evento de caída detectado. |
erodrz | 10:c036607dbf6e | 141 | Reproducir_Buzzer_Caidas(); // Reproducir sonido de aviso. |
thevic16 | 9:4a1c40f8b2d7 | 142 | |
erodrz | 7:ae7b2184b737 | 143 | } |
erodrz | 10:c036607dbf6e | 144 | thread_sleep_for(200); // Delay. |
erodrz | 7:ae7b2184b737 | 145 | } |
erodrz | 7:ae7b2184b737 | 146 | } |
erodrz | 7:ae7b2184b737 | 147 | |
erodrz | 7:ae7b2184b737 | 148 | // Función para reproducir un sonido del buzzer cuando se detecta proximidad a un obstáculo. |
erodrz | 7:ae7b2184b737 | 149 | void Reproducir_Buzzer_Proximidad() |
erodrz | 7:ae7b2184b737 | 150 | { |
erodrz | 7:ae7b2184b737 | 151 | Timer BuzzTime; |
erodrz | 7:ae7b2184b737 | 152 | BuzzTime.reset(); |
erodrz | 7:ae7b2184b737 | 153 | BuzzTime.start(); |
erodrz | 10:c036607dbf6e | 154 | while(BuzzTime.read_us() < 1000000) // Ejecutar sonido del buzzer por 1 segundo. |
erodrz | 7:ae7b2184b737 | 155 | { |
erodrz | 4:60e3365da280 | 156 | Buzzer.period(1.0/Nota_A4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo). |
erodrz | 10:c036607dbf6e | 157 | Buzzer.write(0.5); // Duty cycle. |
erodrz | 10:c036607dbf6e | 158 | thread_sleep_for(200); // Delay. |
erodrz | 4:60e3365da280 | 159 | } |
erodrz | 10:c036607dbf6e | 160 | // Deteniendo el buzzer. |
erodrz | 5:9f30f0a6dc76 | 161 | Buzzer.write(0); |
erodrz | 4:60e3365da280 | 162 | BuzzTime.stop(); |
erodrz | 4:60e3365da280 | 163 | } |
thevic16 | 3:3d54fd4109c0 | 164 | |
erodrz | 10:c036607dbf6e | 165 | // Función para mover la silla hacia adelante. |
erodrz | 4:60e3365da280 | 166 | void Mover_Hacia_Adelante(int Tiempo) |
erodrz | 4:60e3365da280 | 167 | { |
erodrz | 4:60e3365da280 | 168 | Direccion1 = 0; // En dirección de las manecillas del reloj. |
erodrz | 4:60e3365da280 | 169 | Direccion2 = 0; // En dirección de las manecillas del reloj. |
thevic16 | 3:3d54fd4109c0 | 170 | |
thevic16 | 8:418ff0ef9463 | 171 | PWM_Velocidad1.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 172 | PWM_Velocidad1.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 173 | |
thevic16 | 8:418ff0ef9463 | 174 | PWM_Velocidad2.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 175 | PWM_Velocidad2.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 176 | |
erodrz | 4:60e3365da280 | 177 | thread_sleep_for(Tiempo); |
erodrz | 4:60e3365da280 | 178 | |
erodrz | 4:60e3365da280 | 179 | PWM_Velocidad1.write(0.0f); |
erodrz | 4:60e3365da280 | 180 | PWM_Velocidad2.write(0.0f); |
erodrz | 10:c036607dbf6e | 181 | |
erodrz | 10:c036607dbf6e | 182 | Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 183 | Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 184 | Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 185 | Orientacion_Derecha = 0; |
thevic16 | 3:3d54fd4109c0 | 186 | } |
thevic16 | 3:3d54fd4109c0 | 187 | |
erodrz | 10:c036607dbf6e | 188 | // Función para mover la silla hacia atrás. |
erodrz | 4:60e3365da280 | 189 | void Mover_Hacia_Atras(int Tiempo) |
erodrz | 4:60e3365da280 | 190 | { |
erodrz | 4:60e3365da280 | 191 | Direccion1 = 1; // En dirección contraria a las manecillas del reloj. |
erodrz | 4:60e3365da280 | 192 | Direccion2 = 1; // En dirección contraria a las manecillas del reloj. |
erodrz | 4:60e3365da280 | 193 | |
thevic16 | 8:418ff0ef9463 | 194 | PWM_Velocidad1.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 195 | PWM_Velocidad1.write(0.6f); // Duty cycle. |
thevic16 | 3:3d54fd4109c0 | 196 | |
thevic16 | 8:418ff0ef9463 | 197 | PWM_Velocidad2.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 198 | PWM_Velocidad2.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 199 | |
erodrz | 4:60e3365da280 | 200 | thread_sleep_for(Tiempo); |
erodrz | 4:60e3365da280 | 201 | |
erodrz | 4:60e3365da280 | 202 | PWM_Velocidad1.write(0.0f); |
erodrz | 4:60e3365da280 | 203 | PWM_Velocidad2.write(0.0f); |
erodrz | 10:c036607dbf6e | 204 | |
erodrz | 10:c036607dbf6e | 205 | Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 206 | Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 207 | Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 208 | Orientacion_Derecha = 0; |
thevic16 | 3:3d54fd4109c0 | 209 | } |
thevic16 | 3:3d54fd4109c0 | 210 | |
erodrz | 10:c036607dbf6e | 211 | // Función para mover la silla hacia la izquierda. |
erodrz | 4:60e3365da280 | 212 | void Mover_Hacia_Izquierda(int Tiempo) |
erodrz | 4:60e3365da280 | 213 | { |
thevic16 | 9:4a1c40f8b2d7 | 214 | Direccion1 = 0; // En dirección contraria a las manecillas del reloj. |
thevic16 | 9:4a1c40f8b2d7 | 215 | Direccion2 = 1; // En dirección de las manecillas del reloj. |
thevic16 | 3:3d54fd4109c0 | 216 | |
thevic16 | 9:4a1c40f8b2d7 | 217 | PWM_Velocidad1.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 218 | PWM_Velocidad1.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 219 | |
thevic16 | 8:418ff0ef9463 | 220 | PWM_Velocidad2.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 221 | PWM_Velocidad2.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 222 | |
erodrz | 4:60e3365da280 | 223 | thread_sleep_for(Tiempo); |
erodrz | 4:60e3365da280 | 224 | |
thevic16 | 9:4a1c40f8b2d7 | 225 | PWM_Velocidad1.write(0.0f); |
erodrz | 4:60e3365da280 | 226 | PWM_Velocidad2.write(0.0f); |
erodrz | 10:c036607dbf6e | 227 | |
erodrz | 10:c036607dbf6e | 228 | Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 229 | Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 230 | Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 231 | Orientacion_Derecha = 0; |
thevic16 | 3:3d54fd4109c0 | 232 | } |
thevic16 | 3:3d54fd4109c0 | 233 | |
erodrz | 10:c036607dbf6e | 234 | // Función para mover la silla hacia la derecha. |
erodrz | 4:60e3365da280 | 235 | void Mover_Hacia_Derecha(int Tiempo) |
erodrz | 4:60e3365da280 | 236 | { |
thevic16 | 9:4a1c40f8b2d7 | 237 | Direccion1 = 1; // En dirección de las manecillas del reloj. |
thevic16 | 9:4a1c40f8b2d7 | 238 | Direccion2 = 0; // En dirección contraria a las manecillas del reloj. |
thevic16 | 3:3d54fd4109c0 | 239 | |
thevic16 | 8:418ff0ef9463 | 240 | PWM_Velocidad1.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 241 | PWM_Velocidad1.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 242 | |
thevic16 | 9:4a1c40f8b2d7 | 243 | PWM_Velocidad2.period(0.0003f); // Declaramos el período. |
erodrz | 10:c036607dbf6e | 244 | PWM_Velocidad2.write(0.6f); // Duty cycle. |
erodrz | 4:60e3365da280 | 245 | |
erodrz | 10:c036607dbf6e | 246 | thread_sleep_for(Tiempo); |
erodrz | 4:60e3365da280 | 247 | |
erodrz | 4:60e3365da280 | 248 | PWM_Velocidad1.write(0.0f); |
thevic16 | 9:4a1c40f8b2d7 | 249 | PWM_Velocidad2.write(0.0f); |
erodrz | 10:c036607dbf6e | 250 | |
erodrz | 10:c036607dbf6e | 251 | Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 252 | Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 253 | Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 254 | Orientacion_Derecha = 0; |
erodrz | 10:c036607dbf6e | 255 | } |
erodrz | 10:c036607dbf6e | 256 | |
erodrz | 10:c036607dbf6e | 257 | // Función para detener la silla. |
erodrz | 10:c036607dbf6e | 258 | void Detener(int Tiempo) |
erodrz | 10:c036607dbf6e | 259 | { |
erodrz | 10:c036607dbf6e | 260 | PWM_Velocidad1.write(0.0f); // Duty cicle. |
erodrz | 10:c036607dbf6e | 261 | PWM_Velocidad2.write(0.0f); // Duty cicle. |
erodrz | 10:c036607dbf6e | 262 | |
erodrz | 10:c036607dbf6e | 263 | printf(" Deteniendo silla. \n\r"); |
erodrz | 10:c036607dbf6e | 264 | thread_sleep_for(Tiempo); |
erodrz | 10:c036607dbf6e | 265 | |
erodrz | 10:c036607dbf6e | 266 | Orientacion_Adelante = 0; |
erodrz | 10:c036607dbf6e | 267 | Orientacion_Atras = 0; |
erodrz | 10:c036607dbf6e | 268 | Orientacion_Izquierda = 0; |
erodrz | 10:c036607dbf6e | 269 | Orientacion_Derecha = 0; |
thevic16 | 3:3d54fd4109c0 | 270 | } |
thevic16 | 3:3d54fd4109c0 | 271 | |
erodrz | 5:9f30f0a6dc76 | 272 | // Función para leer el sensor de proximidad 1. ADELANTE |
erodrz | 4:60e3365da280 | 273 | void Thread1_HCSR04() |
thevic16 | 0:b8adbf13199b | 274 | { |
erodrz | 4:60e3365da280 | 275 | DigitalOut Trigger(D0); |
erodrz | 4:60e3365da280 | 276 | DigitalIn Echo(D1); |
erodrz | 4:60e3365da280 | 277 | Timer Sonar; |
erodrz | 4:60e3365da280 | 278 | int Correccion = 0; |
erodrz | 4:60e3365da280 | 279 | Sonar.reset(); |
erodrz | 4:60e3365da280 | 280 | Sonar.start(); |
erodrz | 4:60e3365da280 | 281 | while(Echo == 2) |
thevic16 | 1:17ea74f31633 | 282 | { |
thevic16 | 1:17ea74f31633 | 283 | |
thevic16 | 1:17ea74f31633 | 284 | }; |
erodrz | 4:60e3365da280 | 285 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 286 | Correccion = Sonar.read_us(); |
erodrz | 10:c036607dbf6e | 287 | printf(" Sensor de proximidad 1: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion); |
thevic16 | 1:17ea74f31633 | 288 | while(1) |
thevic16 | 1:17ea74f31633 | 289 | { |
erodrz | 4:60e3365da280 | 290 | Trigger = 1; |
erodrz | 4:60e3365da280 | 291 | Sonar.reset(); |
thevic16 | 8:418ff0ef9463 | 292 | thread_sleep_for(10); |
erodrz | 4:60e3365da280 | 293 | Trigger = 0; |
erodrz | 4:60e3365da280 | 294 | while(Echo == 0) |
thevic16 | 1:17ea74f31633 | 295 | { |
thevic16 | 1:17ea74f31633 | 296 | |
thevic16 | 1:17ea74f31633 | 297 | }; |
erodrz | 4:60e3365da280 | 298 | Sonar.start(); |
erodrz | 4:60e3365da280 | 299 | while(Echo == 1) |
erodrz | 4:60e3365da280 | 300 | { |
erodrz | 4:60e3365da280 | 301 | |
erodrz | 4:60e3365da280 | 302 | }; |
erodrz | 4:60e3365da280 | 303 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 304 | Distance1 = (Sonar.read_us()-Correccion)/58.0; |
erodrz | 10:c036607dbf6e | 305 | if(Distance1 < DistanceLimit && Orientacion_Adelante == 1) |
erodrz | 10:c036607dbf6e | 306 | { |
erodrz | 10:c036607dbf6e | 307 | Detener(1000); |
erodrz | 10:c036607dbf6e | 308 | printf(" Sensor de proximidad 1 (Adelante): %d cm \n\r",Distance1); |
erodrz | 10:c036607dbf6e | 309 | } |
thevic16 | 1:17ea74f31633 | 310 | thread_sleep_for(1000); |
thevic16 | 1:17ea74f31633 | 311 | } |
thevic16 | 1:17ea74f31633 | 312 | } |
thevic16 | 0:b8adbf13199b | 313 | |
erodrz | 10:c036607dbf6e | 314 | // Función para leer el sensor de proximidad 2. // ATRÁS |
erodrz | 4:60e3365da280 | 315 | void Thread2_HCSR04() |
thevic16 | 1:17ea74f31633 | 316 | { |
erodrz | 4:60e3365da280 | 317 | DigitalOut Trigger(D2); |
erodrz | 4:60e3365da280 | 318 | DigitalIn Echo(D3); |
erodrz | 4:60e3365da280 | 319 | Timer Sonar; |
erodrz | 4:60e3365da280 | 320 | int Correccion = 0; |
erodrz | 4:60e3365da280 | 321 | Sonar.reset(); |
erodrz | 4:60e3365da280 | 322 | Sonar.start(); |
erodrz | 4:60e3365da280 | 323 | while(Echo == 2) |
thevic16 | 1:17ea74f31633 | 324 | { |
thevic16 | 1:17ea74f31633 | 325 | |
thevic16 | 1:17ea74f31633 | 326 | }; |
erodrz | 4:60e3365da280 | 327 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 328 | Correccion = Sonar.read_us(); |
erodrz | 10:c036607dbf6e | 329 | printf(" Sensor de proximidad 2: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion); |
thevic16 | 1:17ea74f31633 | 330 | while(1) |
thevic16 | 1:17ea74f31633 | 331 | { |
erodrz | 4:60e3365da280 | 332 | Trigger = 1; |
erodrz | 4:60e3365da280 | 333 | Sonar.reset(); |
thevic16 | 8:418ff0ef9463 | 334 | thread_sleep_for(10); |
erodrz | 4:60e3365da280 | 335 | Trigger = 0; |
erodrz | 4:60e3365da280 | 336 | while(Echo == 0) |
thevic16 | 1:17ea74f31633 | 337 | { |
thevic16 | 1:17ea74f31633 | 338 | |
thevic16 | 1:17ea74f31633 | 339 | }; |
erodrz | 4:60e3365da280 | 340 | Sonar.start(); |
erodrz | 4:60e3365da280 | 341 | while(Echo == 1) |
thevic16 | 1:17ea74f31633 | 342 | { |
thevic16 | 1:17ea74f31633 | 343 | |
thevic16 | 1:17ea74f31633 | 344 | }; |
erodrz | 4:60e3365da280 | 345 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 346 | Distance2 = (Sonar.read_us()-Correccion)/58.0; |
erodrz | 10:c036607dbf6e | 347 | |
erodrz | 10:c036607dbf6e | 348 | if(Distance2 < DistanceLimit && Orientacion_Atras == 1) |
erodrz | 10:c036607dbf6e | 349 | { |
erodrz | 10:c036607dbf6e | 350 | Detener(1000); |
erodrz | 10:c036607dbf6e | 351 | printf(" Sensor de proximidad 2 (Atras): %d cm \n\r",Distance2); |
erodrz | 10:c036607dbf6e | 352 | } |
thevic16 | 1:17ea74f31633 | 353 | thread_sleep_for(1000); |
thevic16 | 1:17ea74f31633 | 354 | } |
thevic16 | 0:b8adbf13199b | 355 | } |
thevic16 | 0:b8adbf13199b | 356 | |
erodrz | 10:c036607dbf6e | 357 | // Función para leer el sensor de proximidad 3. // IZQUIERDA |
erodrz | 4:60e3365da280 | 358 | void Thread3_HCSR04() |
thevic16 | 0:b8adbf13199b | 359 | { |
erodrz | 4:60e3365da280 | 360 | DigitalOut Trigger(D4); |
erodrz | 4:60e3365da280 | 361 | DigitalIn Echo(D5); |
erodrz | 4:60e3365da280 | 362 | Timer Sonar; |
erodrz | 4:60e3365da280 | 363 | int Correccion = 0; |
erodrz | 4:60e3365da280 | 364 | Sonar.reset(); |
erodrz | 4:60e3365da280 | 365 | Sonar.start(); |
erodrz | 4:60e3365da280 | 366 | while(Echo == 2) |
thevic16 | 1:17ea74f31633 | 367 | { |
thevic16 | 1:17ea74f31633 | 368 | |
thevic16 | 1:17ea74f31633 | 369 | }; |
erodrz | 4:60e3365da280 | 370 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 371 | Correccion = Sonar.read_us(); |
erodrz | 10:c036607dbf6e | 372 | printf(" Sensor de proximidad 3: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion); |
thevic16 | 1:17ea74f31633 | 373 | while(1) |
thevic16 | 1:17ea74f31633 | 374 | { |
erodrz | 4:60e3365da280 | 375 | Trigger = 1; |
erodrz | 4:60e3365da280 | 376 | Sonar.reset(); |
thevic16 | 8:418ff0ef9463 | 377 | thread_sleep_for(10); |
erodrz | 4:60e3365da280 | 378 | Trigger = 0; |
erodrz | 4:60e3365da280 | 379 | while(Echo == 0) |
thevic16 | 1:17ea74f31633 | 380 | { |
thevic16 | 1:17ea74f31633 | 381 | |
thevic16 | 1:17ea74f31633 | 382 | }; |
erodrz | 4:60e3365da280 | 383 | Sonar.start(); |
erodrz | 4:60e3365da280 | 384 | while(Echo == 1) |
thevic16 | 1:17ea74f31633 | 385 | { |
thevic16 | 1:17ea74f31633 | 386 | |
thevic16 | 1:17ea74f31633 | 387 | }; |
erodrz | 4:60e3365da280 | 388 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 389 | Distance3 = (Sonar.read_us()-Correccion)/58.0; |
erodrz | 10:c036607dbf6e | 390 | if(Distance3 < DistanceLimit && Orientacion_Izquierda == 1) |
erodrz | 10:c036607dbf6e | 391 | { |
erodrz | 10:c036607dbf6e | 392 | Detener(1000); |
erodrz | 10:c036607dbf6e | 393 | printf(" Sensor de proximidad 3 (Izquierda): %d cm \n\r",Distance3); |
erodrz | 10:c036607dbf6e | 394 | } |
thevic16 | 1:17ea74f31633 | 395 | thread_sleep_for(1000); |
thevic16 | 1:17ea74f31633 | 396 | } |
thevic16 | 0:b8adbf13199b | 397 | } |
thevic16 | 1:17ea74f31633 | 398 | |
erodrz | 10:c036607dbf6e | 399 | // Función para leer el sensor de proximidad 4. // DERECHA |
erodrz | 4:60e3365da280 | 400 | void Thread4_HCSR04() |
thevic16 | 0:b8adbf13199b | 401 | { |
erodrz | 4:60e3365da280 | 402 | DigitalOut Trigger(D6); |
erodrz | 4:60e3365da280 | 403 | DigitalIn Echo(D7); |
erodrz | 4:60e3365da280 | 404 | Timer Sonar; |
erodrz | 4:60e3365da280 | 405 | int Correccion = 0; |
erodrz | 4:60e3365da280 | 406 | Sonar.reset(); |
erodrz | 4:60e3365da280 | 407 | Sonar.start(); |
erodrz | 4:60e3365da280 | 408 | while(Echo == 2) |
thevic16 | 1:17ea74f31633 | 409 | { |
thevic16 | 0:b8adbf13199b | 410 | |
thevic16 | 1:17ea74f31633 | 411 | }; |
erodrz | 4:60e3365da280 | 412 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 413 | Correccion = Sonar.read_us(); |
erodrz | 10:c036607dbf6e | 414 | printf(" Sensor de proximidad 4: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion); |
thevic16 | 1:17ea74f31633 | 415 | while(1) |
thevic16 | 1:17ea74f31633 | 416 | { |
erodrz | 4:60e3365da280 | 417 | Trigger = 1; |
erodrz | 4:60e3365da280 | 418 | Sonar.reset(); |
thevic16 | 8:418ff0ef9463 | 419 | thread_sleep_for(10); |
erodrz | 4:60e3365da280 | 420 | Trigger = 0; |
erodrz | 4:60e3365da280 | 421 | while(Echo == 0) |
thevic16 | 1:17ea74f31633 | 422 | { |
thevic16 | 1:17ea74f31633 | 423 | |
thevic16 | 1:17ea74f31633 | 424 | }; |
erodrz | 4:60e3365da280 | 425 | Sonar.start(); |
erodrz | 4:60e3365da280 | 426 | while(Echo == 1) |
thevic16 | 1:17ea74f31633 | 427 | { |
thevic16 | 1:17ea74f31633 | 428 | |
thevic16 | 1:17ea74f31633 | 429 | }; |
erodrz | 4:60e3365da280 | 430 | Sonar.stop(); |
erodrz | 4:60e3365da280 | 431 | Distance4 = (Sonar.read_us()-Correccion)/58.0; |
erodrz | 10:c036607dbf6e | 432 | if(Distance4 < DistanceLimit && Orientacion_Derecha == 1) |
erodrz | 10:c036607dbf6e | 433 | { |
erodrz | 10:c036607dbf6e | 434 | Detener(1000); |
erodrz | 10:c036607dbf6e | 435 | printf(" Sensor de proximidad 4 (Derecha): %d cm \n\r",Distance4); |
erodrz | 10:c036607dbf6e | 436 | } |
thevic16 | 1:17ea74f31633 | 437 | thread_sleep_for(1000); |
thevic16 | 1:17ea74f31633 | 438 | } |
thevic16 | 0:b8adbf13199b | 439 | } |
thevic16 | 0:b8adbf13199b | 440 | |
erodrz | 4:60e3365da280 | 441 | // Función para leer valores del joystick y ejecutar sus comandos. |
erodrz | 4:60e3365da280 | 442 | void Thread5_Joystick() |
thevic16 | 1:17ea74f31633 | 443 | { |
thevic16 | 9:4a1c40f8b2d7 | 444 | SparkfunAnalogJoystick JoyStick(A1,A0,PE_0); |
thevic16 | 0:b8adbf13199b | 445 | float X; |
thevic16 | 0:b8adbf13199b | 446 | float Y; |
thevic16 | 1:17ea74f31633 | 447 | while(1) |
thevic16 | 1:17ea74f31633 | 448 | { |
erodrz | 4:60e3365da280 | 449 | if(!Modo1 && Modo2 && !Modo3 && !Modo4) |
thevic16 | 1:17ea74f31633 | 450 | { |
thevic16 | 1:17ea74f31633 | 451 | X = JoyStick.xAxis(); |
thevic16 | 1:17ea74f31633 | 452 | Y = JoyStick.yAxis(); |
erodrz | 2:4f5a0c64d9cd | 453 | /* |
erodrz | 4:60e3365da280 | 454 | printf("X-Axis: %f\n\r",X); |
erodrz | 4:60e3365da280 | 455 | printf("Y-Axis: %f\n\r",Y); |
thevic16 | 1:17ea74f31633 | 456 | printf(" \n\r"); |
erodrz | 2:4f5a0c64d9cd | 457 | */ |
thevic16 | 9:4a1c40f8b2d7 | 458 | if(X >= -0.70f && X <= 0.70f && Y >= 0.90f && Y <= 1.00f) |
erodrz | 5:9f30f0a6dc76 | 459 | { |
erodrz | 5:9f30f0a6dc76 | 460 | if(Distance2 > DistanceLimit) |
thevic16 | 1:17ea74f31633 | 461 | { |
erodrz | 10:c036607dbf6e | 462 | Orientacion_Atras = 1; |
erodrz | 10:c036607dbf6e | 463 | printf(" Comandos del joystick: Hacia atras. \r \n"); |
erodrz | 10:c036607dbf6e | 464 | printf(" Distancia medida por sensor atras: %d cm \r \n",Distance2); |
erodrz | 10:c036607dbf6e | 465 | Mover_Hacia_Atras(2000); |
thevic16 | 1:17ea74f31633 | 466 | } |
thevic16 | 1:17ea74f31633 | 467 | else |
thevic16 | 1:17ea74f31633 | 468 | { |
erodrz | 10:c036607dbf6e | 469 | printf(" Comandos del joystick: Obstaculo hacia atras. \r \n"); |
erodrz | 10:c036607dbf6e | 470 | printf(" Distancia medida por sensor atras: %d cm \r \n",Distance2); |
erodrz | 10:c036607dbf6e | 471 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 472 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 1:17ea74f31633 | 473 | } |
thevic16 | 1:17ea74f31633 | 474 | } |
thevic16 | 9:4a1c40f8b2d7 | 475 | if(X >= -0.70f && X <= 0.70f && Y <= -0.90f && Y >= -1.00f) |
erodrz | 5:9f30f0a6dc76 | 476 | { |
erodrz | 5:9f30f0a6dc76 | 477 | if(Distance1 > DistanceLimit) |
thevic16 | 1:17ea74f31633 | 478 | { |
erodrz | 10:c036607dbf6e | 479 | Orientacion_Adelante = 1; |
erodrz | 10:c036607dbf6e | 480 | printf(" Comandos del joystick: Hacia adelante. \r \n"); |
erodrz | 10:c036607dbf6e | 481 | printf(" Distancia medida por sensor adelante: %d cm \r \n",Distance1); |
erodrz | 10:c036607dbf6e | 482 | Mover_Hacia_Adelante(2000); |
thevic16 | 1:17ea74f31633 | 483 | } |
thevic16 | 1:17ea74f31633 | 484 | else |
thevic16 | 1:17ea74f31633 | 485 | { |
erodrz | 10:c036607dbf6e | 486 | printf(" Comandos del joystick: Obstaculo hacia adelante. \r \n"); |
erodrz | 10:c036607dbf6e | 487 | printf(" Distancia medida por sensor adelante: %d cm \r \n",Distance1); |
erodrz | 10:c036607dbf6e | 488 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 489 | //Reproducir_Buzzer_Proximidad(); |
erodrz | 4:60e3365da280 | 490 | } |
thevic16 | 1:17ea74f31633 | 491 | } |
thevic16 | 9:4a1c40f8b2d7 | 492 | if(Y >= -0.70f && Y <= 0.70f && X <= -0.90f && X >= -1.00f) |
thevic16 | 9:4a1c40f8b2d7 | 493 | |
thevic16 | 1:17ea74f31633 | 494 | { |
thevic16 | 9:4a1c40f8b2d7 | 495 | if(Distance4 > DistanceLimit) |
thevic16 | 9:4a1c40f8b2d7 | 496 | { |
erodrz | 10:c036607dbf6e | 497 | Orientacion_Derecha = 1; |
erodrz | 10:c036607dbf6e | 498 | printf(" Comandos del joystick: Hacia la derecha. \r \n"); |
erodrz | 10:c036607dbf6e | 499 | printf(" Distancia medida por sensor derecha: %d cm \r \n",Distance4); |
erodrz | 10:c036607dbf6e | 500 | Mover_Hacia_Derecha(2000); |
thevic16 | 9:4a1c40f8b2d7 | 501 | } |
thevic16 | 9:4a1c40f8b2d7 | 502 | else |
thevic16 | 9:4a1c40f8b2d7 | 503 | { |
erodrz | 10:c036607dbf6e | 504 | printf(" Comandos del joystick: Obstaculo hacia la derecha. \r \n"); |
erodrz | 10:c036607dbf6e | 505 | printf(" Distancia medida por sensor derecha: %d cm \r \n",Distance4); |
erodrz | 10:c036607dbf6e | 506 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 507 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 9:4a1c40f8b2d7 | 508 | } |
thevic16 | 9:4a1c40f8b2d7 | 509 | } |
thevic16 | 9:4a1c40f8b2d7 | 510 | if(Y >= -0.70f && Y <= 0.70f && X >= 0.90f && X <= 1.00f) |
thevic16 | 9:4a1c40f8b2d7 | 511 | { |
thevic16 | 9:4a1c40f8b2d7 | 512 | |
erodrz | 4:60e3365da280 | 513 | if(Distance3 > DistanceLimit) |
thevic16 | 1:17ea74f31633 | 514 | { |
erodrz | 10:c036607dbf6e | 515 | Orientacion_Izquierda = 1; |
erodrz | 10:c036607dbf6e | 516 | printf(" Comandos del joystick: Hacia la izquierda. \r \n"); |
erodrz | 10:c036607dbf6e | 517 | printf(" Distancia medida por sensor izquierda: %d cm \r \n",Distance3); |
erodrz | 10:c036607dbf6e | 518 | Mover_Hacia_Izquierda(2000); |
thevic16 | 1:17ea74f31633 | 519 | } |
thevic16 | 1:17ea74f31633 | 520 | else |
thevic16 | 1:17ea74f31633 | 521 | { |
erodrz | 10:c036607dbf6e | 522 | printf(" Comandos del joystick: Obstaculo hacia la izquierda. \r \n"); |
erodrz | 10:c036607dbf6e | 523 | printf(" Distancia medida por sensor izquierda: %d cm \r \n",Distance3); |
erodrz | 10:c036607dbf6e | 524 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 525 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 1:17ea74f31633 | 526 | } |
thevic16 | 1:17ea74f31633 | 527 | } |
erodrz | 4:60e3365da280 | 528 | thread_sleep_for(5); |
thevic16 | 1:17ea74f31633 | 529 | } |
thevic16 | 1:17ea74f31633 | 530 | } |
thevic16 | 1:17ea74f31633 | 531 | } |
thevic16 | 1:17ea74f31633 | 532 | |
erodrz | 10:c036607dbf6e | 533 | |
erodrz | 10:c036607dbf6e | 534 | void Thread6_RecepcionCaracteresComandovoz() |
erodrz | 10:c036607dbf6e | 535 | { |
erodrz | 10:c036607dbf6e | 536 | |
erodrz | 10:c036607dbf6e | 537 | while(1) |
erodrz | 10:c036607dbf6e | 538 | { |
erodrz | 10:c036607dbf6e | 539 | while(PC.readable()) { // Para saber si hay algo que recibir en el puerto serial. |
erodrz | 10:c036607dbf6e | 540 | c = PC.getc(); |
erodrz | 10:c036607dbf6e | 541 | printf(" Datos recibidos: %c \r\n",c); |
erodrz | 10:c036607dbf6e | 542 | } |
erodrz | 10:c036607dbf6e | 543 | |
erodrz | 10:c036607dbf6e | 544 | } |
erodrz | 10:c036607dbf6e | 545 | } |
erodrz | 10:c036607dbf6e | 546 | |
erodrz | 10:c036607dbf6e | 547 | void Thread7_ParadaComandoVoz() |
erodrz | 10:c036607dbf6e | 548 | { |
erodrz | 10:c036607dbf6e | 549 | |
erodrz | 10:c036607dbf6e | 550 | while(1) |
erodrz | 10:c036607dbf6e | 551 | { |
erodrz | 10:c036607dbf6e | 552 | if(c == 'x') |
erodrz | 10:c036607dbf6e | 553 | { |
erodrz | 10:c036607dbf6e | 554 | if(!Modo1 && !Modo2 && Modo3 && !Modo4) |
erodrz | 10:c036607dbf6e | 555 | { |
erodrz | 10:c036607dbf6e | 556 | printf(" Comandos de voz: Detenerse. \r \n"); |
erodrz | 10:c036607dbf6e | 557 | Detener(1000); |
erodrz | 10:c036607dbf6e | 558 | c = ' '; |
erodrz | 10:c036607dbf6e | 559 | } |
erodrz | 10:c036607dbf6e | 560 | } |
erodrz | 10:c036607dbf6e | 561 | } |
erodrz | 10:c036607dbf6e | 562 | } |
erodrz | 10:c036607dbf6e | 563 | |
erodrz | 10:c036607dbf6e | 564 | |
erodrz | 4:60e3365da280 | 565 | // Función para leer datos del serial con caracteres de comandos de voz y ejecutar instrucciones. |
erodrz | 10:c036607dbf6e | 566 | void Thread8_ComandosVoz() |
thevic16 | 1:17ea74f31633 | 567 | { |
erodrz | 10:c036607dbf6e | 568 | int Tiempo_Lineal = 60000; |
erodrz | 10:c036607dbf6e | 569 | int Tiempo_Lateral = 3000; |
erodrz | 10:c036607dbf6e | 570 | |
thevic16 | 0:b8adbf13199b | 571 | while(1) |
thevic16 | 0:b8adbf13199b | 572 | { |
erodrz | 4:60e3365da280 | 573 | if(!Modo1 && !Modo2 && Modo3 && !Modo4) |
thevic16 | 1:17ea74f31633 | 574 | { |
thevic16 | 8:418ff0ef9463 | 575 | |
thevic16 | 9:4a1c40f8b2d7 | 576 | if(c == 'w') |
erodrz | 4:60e3365da280 | 577 | { |
erodrz | 10:c036607dbf6e | 578 | printf(" Distance1 - %d cm \r \n",Distance1); |
thevic16 | 9:4a1c40f8b2d7 | 579 | if(Distance1 > DistanceLimit) |
thevic16 | 9:4a1c40f8b2d7 | 580 | { |
erodrz | 10:c036607dbf6e | 581 | Orientacion_Adelante = 1; |
erodrz | 10:c036607dbf6e | 582 | printf(" Comandos de voz: Hacia adelante. \r \n"); |
erodrz | 10:c036607dbf6e | 583 | |
erodrz | 10:c036607dbf6e | 584 | Mover_Hacia_Adelante(Tiempo_Lineal); |
erodrz | 10:c036607dbf6e | 585 | thread_sleep_for(200); |
thevic16 | 9:4a1c40f8b2d7 | 586 | } |
thevic16 | 9:4a1c40f8b2d7 | 587 | else |
thevic16 | 1:17ea74f31633 | 588 | { |
erodrz | 10:c036607dbf6e | 589 | printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n"); |
erodrz | 10:c036607dbf6e | 590 | Detener(1000); |
erodrz | 10:c036607dbf6e | 591 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 1:17ea74f31633 | 592 | } |
erodrz | 10:c036607dbf6e | 593 | c = ' '; |
thevic16 | 9:4a1c40f8b2d7 | 594 | } |
thevic16 | 9:4a1c40f8b2d7 | 595 | if(c == 's') |
thevic16 | 9:4a1c40f8b2d7 | 596 | { |
erodrz | 10:c036607dbf6e | 597 | printf(" Distance2 - %d cm \r \n",Distance2); |
thevic16 | 9:4a1c40f8b2d7 | 598 | if(Distance2 > DistanceLimit) |
thevic16 | 9:4a1c40f8b2d7 | 599 | { |
erodrz | 10:c036607dbf6e | 600 | Orientacion_Atras = 1; |
erodrz | 10:c036607dbf6e | 601 | printf(" Comandos de voz: Hacia atras. \r \n"); |
erodrz | 10:c036607dbf6e | 602 | |
erodrz | 10:c036607dbf6e | 603 | Mover_Hacia_Atras(Tiempo_Lineal); |
erodrz | 10:c036607dbf6e | 604 | thread_sleep_for(200); |
thevic16 | 9:4a1c40f8b2d7 | 605 | } |
thevic16 | 9:4a1c40f8b2d7 | 606 | else |
thevic16 | 1:17ea74f31633 | 607 | { |
erodrz | 10:c036607dbf6e | 608 | printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n"); |
erodrz | 10:c036607dbf6e | 609 | Detener(1000); |
erodrz | 10:c036607dbf6e | 610 | //Reproducir_Buzzer_Proximidad(); |
erodrz | 4:60e3365da280 | 611 | } |
erodrz | 10:c036607dbf6e | 612 | c = ' '; |
thevic16 | 9:4a1c40f8b2d7 | 613 | } |
thevic16 | 9:4a1c40f8b2d7 | 614 | if(c == 'a') |
thevic16 | 9:4a1c40f8b2d7 | 615 | { |
erodrz | 10:c036607dbf6e | 616 | printf(" Distance3 - %d cm \r \n",Distance3); |
thevic16 | 9:4a1c40f8b2d7 | 617 | if(Distance3 > DistanceLimit) |
thevic16 | 9:4a1c40f8b2d7 | 618 | { |
erodrz | 10:c036607dbf6e | 619 | Orientacion_Izquierda = 1; |
erodrz | 10:c036607dbf6e | 620 | printf(" Comandos de voz: Hacia la izquierda. \r \n"); |
erodrz | 10:c036607dbf6e | 621 | |
erodrz | 10:c036607dbf6e | 622 | Mover_Hacia_Izquierda(Tiempo_Lateral); |
erodrz | 10:c036607dbf6e | 623 | thread_sleep_for(200); |
thevic16 | 9:4a1c40f8b2d7 | 624 | } |
thevic16 | 9:4a1c40f8b2d7 | 625 | else |
thevic16 | 1:17ea74f31633 | 626 | { |
erodrz | 10:c036607dbf6e | 627 | printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n"); |
erodrz | 10:c036607dbf6e | 628 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 629 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 1:17ea74f31633 | 630 | } |
erodrz | 10:c036607dbf6e | 631 | c = ' '; |
thevic16 | 9:4a1c40f8b2d7 | 632 | } |
thevic16 | 9:4a1c40f8b2d7 | 633 | if(c == 'd') |
thevic16 | 9:4a1c40f8b2d7 | 634 | { |
erodrz | 10:c036607dbf6e | 635 | printf(" Distance4 - %d cm \r \n",Distance4); |
thevic16 | 9:4a1c40f8b2d7 | 636 | if(Distance4 > DistanceLimit) |
erodrz | 4:60e3365da280 | 637 | { |
erodrz | 10:c036607dbf6e | 638 | Orientacion_Derecha = 1; |
erodrz | 10:c036607dbf6e | 639 | printf(" Comandos de voz: Hacia la derecha. \r \n"); |
erodrz | 10:c036607dbf6e | 640 | |
erodrz | 10:c036607dbf6e | 641 | Mover_Hacia_Derecha(Tiempo_Lateral); |
erodrz | 10:c036607dbf6e | 642 | thread_sleep_for(200); |
erodrz | 2:4f5a0c64d9cd | 643 | } |
thevic16 | 9:4a1c40f8b2d7 | 644 | else |
thevic16 | 9:4a1c40f8b2d7 | 645 | { |
erodrz | 10:c036607dbf6e | 646 | printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n"); |
erodrz | 10:c036607dbf6e | 647 | Detener(1000); |
thevic16 | 9:4a1c40f8b2d7 | 648 | //Reproducir_Buzzer_Proximidad(); |
thevic16 | 9:4a1c40f8b2d7 | 649 | } |
erodrz | 10:c036607dbf6e | 650 | c = ' '; |
erodrz | 2:4f5a0c64d9cd | 651 | } |
thevic16 | 9:4a1c40f8b2d7 | 652 | |
erodrz | 2:4f5a0c64d9cd | 653 | } |
thevic16 | 1:17ea74f31633 | 654 | } |
thevic16 | 1:17ea74f31633 | 655 | } |
thevic16 | 1:17ea74f31633 | 656 | |
erodrz | 10:c036607dbf6e | 657 | |
erodrz | 10:c036607dbf6e | 658 | |
erodrz | 10:c036607dbf6e | 659 | |
erodrz | 10:c036607dbf6e | 660 | |
erodrz | 4:60e3365da280 | 661 | // Función para seleccionar el modo de operación de la silla. |
erodrz | 10:c036607dbf6e | 662 | void Thread9_IndicarModo() |
thevic16 | 1:17ea74f31633 | 663 | { |
erodrz | 4:60e3365da280 | 664 | bool EstadoModo1 = false; |
erodrz | 4:60e3365da280 | 665 | bool EstadoModo2 = false; |
erodrz | 4:60e3365da280 | 666 | bool EstadoModo3 = false; |
erodrz | 4:60e3365da280 | 667 | bool EstadoModo4 = false; |
erodrz | 10:c036607dbf6e | 668 | |
erodrz | 4:60e3365da280 | 669 | while(true) |
thevic16 | 1:17ea74f31633 | 670 | { |
thevic16 | 9:4a1c40f8b2d7 | 671 | |
thevic16 | 9:4a1c40f8b2d7 | 672 | if(modo1 > 0.15f) { |
thevic16 | 9:4a1c40f8b2d7 | 673 | Modo1 = 1; |
thevic16 | 9:4a1c40f8b2d7 | 674 | } else { |
thevic16 | 9:4a1c40f8b2d7 | 675 | Modo1 = 0; |
thevic16 | 9:4a1c40f8b2d7 | 676 | } |
thevic16 | 9:4a1c40f8b2d7 | 677 | |
thevic16 | 9:4a1c40f8b2d7 | 678 | |
thevic16 | 9:4a1c40f8b2d7 | 679 | if(modo2 > 0.15f) { |
thevic16 | 9:4a1c40f8b2d7 | 680 | Modo2 = 1; |
thevic16 | 9:4a1c40f8b2d7 | 681 | } else { |
thevic16 | 9:4a1c40f8b2d7 | 682 | Modo2 = 0; |
thevic16 | 9:4a1c40f8b2d7 | 683 | } |
thevic16 | 9:4a1c40f8b2d7 | 684 | |
thevic16 | 9:4a1c40f8b2d7 | 685 | if(modo3 > 0.15f) { |
thevic16 | 9:4a1c40f8b2d7 | 686 | Modo3 = 1; |
thevic16 | 9:4a1c40f8b2d7 | 687 | } else { |
thevic16 | 9:4a1c40f8b2d7 | 688 | Modo3 = 0; |
thevic16 | 9:4a1c40f8b2d7 | 689 | } |
thevic16 | 9:4a1c40f8b2d7 | 690 | |
thevic16 | 9:4a1c40f8b2d7 | 691 | if(modo4 > 0.15f) { |
thevic16 | 9:4a1c40f8b2d7 | 692 | Modo4 = 1; |
thevic16 | 9:4a1c40f8b2d7 | 693 | } else { |
thevic16 | 9:4a1c40f8b2d7 | 694 | Modo4 = 0; |
thevic16 | 9:4a1c40f8b2d7 | 695 | } |
erodrz | 10:c036607dbf6e | 696 | |
erodrz | 4:60e3365da280 | 697 | if(Modo1 && !Modo2 && !Modo3 && !Modo4 && !EstadoModo1) |
thevic16 | 1:17ea74f31633 | 698 | { |
erodrz | 10:c036607dbf6e | 699 | printf(" Operando: Modo manual. \r \n"); |
erodrz | 4:60e3365da280 | 700 | EstadoModo1 = true; |
erodrz | 4:60e3365da280 | 701 | EstadoModo2 = false; |
erodrz | 4:60e3365da280 | 702 | EstadoModo3 = false; |
erodrz | 4:60e3365da280 | 703 | EstadoModo4 = false; |
thevic16 | 0:b8adbf13199b | 704 | } |
erodrz | 4:60e3365da280 | 705 | if(!Modo1 && Modo2 && !Modo3 && !Modo4 && !EstadoModo2) |
thevic16 | 1:17ea74f31633 | 706 | { |
erodrz | 10:c036607dbf6e | 707 | printf(" Operando: Modo de comandos de joystick. \r \n"); |
erodrz | 4:60e3365da280 | 708 | EstadoModo1 = false; |
erodrz | 4:60e3365da280 | 709 | EstadoModo2 = true; |
erodrz | 4:60e3365da280 | 710 | EstadoModo3 = false; |
erodrz | 4:60e3365da280 | 711 | EstadoModo4 = false; |
erodrz | 4:60e3365da280 | 712 | } |
erodrz | 4:60e3365da280 | 713 | if(!Modo1 && !Modo2 && Modo3 && !Modo4 && !EstadoModo3) |
thevic16 | 1:17ea74f31633 | 714 | { |
erodrz | 10:c036607dbf6e | 715 | printf(" Operando: Modo de comandos de voz. \r \n"); |
erodrz | 4:60e3365da280 | 716 | EstadoModo1 = false; |
erodrz | 4:60e3365da280 | 717 | EstadoModo2 = false; |
erodrz | 4:60e3365da280 | 718 | EstadoModo3 = true; |
erodrz | 4:60e3365da280 | 719 | EstadoModo4 = false; |
erodrz | 4:60e3365da280 | 720 | } |
erodrz | 4:60e3365da280 | 721 | if(!Modo1 && !Modo2 && !Modo3 && Modo4 && !EstadoModo4) |
thevic16 | 1:17ea74f31633 | 722 | { |
erodrz | 10:c036607dbf6e | 723 | printf(" Operando: Modo de rutas autonomas. \r \n"); |
erodrz | 4:60e3365da280 | 724 | EstadoModo1 = false; |
erodrz | 4:60e3365da280 | 725 | EstadoModo2 = false; |
erodrz | 4:60e3365da280 | 726 | EstadoModo3 = false; |
erodrz | 4:60e3365da280 | 727 | EstadoModo4 = true; |
erodrz | 4:60e3365da280 | 728 | } |
thevic16 | 0:b8adbf13199b | 729 | } |
thevic16 | 1:17ea74f31633 | 730 | } |
thevic16 | 0:b8adbf13199b | 731 | |
erodrz | 4:60e3365da280 | 732 | // Proceso principal de todo el software ejecutado por el microprocesador. |
thevic16 | 0:b8adbf13199b | 733 | int main() |
thevic16 | 0:b8adbf13199b | 734 | { |
erodrz | 4:60e3365da280 | 735 | Thread1.start(Thread1_HCSR04); |
erodrz | 2:4f5a0c64d9cd | 736 | thread_sleep_for(200); |
erodrz | 4:60e3365da280 | 737 | Thread2.start(Thread2_HCSR04); |
erodrz | 2:4f5a0c64d9cd | 738 | thread_sleep_for(200); |
erodrz | 4:60e3365da280 | 739 | Thread3.start(Thread3_HCSR04); |
erodrz | 2:4f5a0c64d9cd | 740 | thread_sleep_for(200); |
erodrz | 4:60e3365da280 | 741 | Thread4.start(Thread4_HCSR04); |
erodrz | 2:4f5a0c64d9cd | 742 | thread_sleep_for(200); |
erodrz | 10:c036607dbf6e | 743 | |
erodrz | 10:c036607dbf6e | 744 | Thread6.start(Thread6_RecepcionCaracteresComandovoz); |
erodrz | 10:c036607dbf6e | 745 | thread_sleep_for(200); |
erodrz | 10:c036607dbf6e | 746 | |
erodrz | 10:c036607dbf6e | 747 | Thread7.start(Thread7_ParadaComandoVoz); |
erodrz | 10:c036607dbf6e | 748 | thread_sleep_for(200); |
erodrz | 10:c036607dbf6e | 749 | |
erodrz | 10:c036607dbf6e | 750 | |
erodrz | 10:c036607dbf6e | 751 | Thread9.start(Thread9_IndicarModo); |
thevic16 | 9:4a1c40f8b2d7 | 752 | thread_sleep_for(200); |
erodrz | 4:60e3365da280 | 753 | Thread5.start(Thread5_Joystick); |
erodrz | 2:4f5a0c64d9cd | 754 | thread_sleep_for(200); |
erodrz | 10:c036607dbf6e | 755 | Thread8.start(Thread8_ComandosVoz); |
erodrz | 2:4f5a0c64d9cd | 756 | thread_sleep_for(200); |
erodrz | 10:c036607dbf6e | 757 | |
erodrz | 10:c036607dbf6e | 758 | |
erodrz | 10:c036607dbf6e | 759 | |
thevic16 | 9:4a1c40f8b2d7 | 760 | //Thread8.start(Thread8_MPU6050); |
thevic16 | 9:4a1c40f8b2d7 | 761 | //thread_sleep_for(200); |
erodrz | 4:60e3365da280 | 762 | } |