Esdras Nicolás Rodríguez Lantigua / Mbed OS WheelchairLogicv3New

Wheelchair Logic v3 Project (New)

Dependencies:   SparkfunAnalogJoystick

main.cpp@5:9f30f0a6dc76, 2021-07-23 (annotated)

Committer:
erodrz
Date:
Fri Jul 23 03:05:12 2021 +0000
Revision:
5:9f30f0a6dc76
Parent:
4:60e3365da280 
New

Who changed what in which revision?

UserRevisionLine numberNew 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"
thevic16 0:b8adbf13199b 5
erodrz 4:60e3365da280 6 // Constantes de programación para procesos y funciones.
erodrz 4:60e3365da280 7 // Valor en frecuencia de las notas reproducidas por el Buzzer que da avisos al usuario de la silla.
erodrz 4:60e3365da280 8 #define Nota_C4 262
erodrz 4:60e3365da280 9 #define Nota_A4 440
erodrz 4:60e3365da280 10 #define Nota_E4 659
erodrz 4:60e3365da280 11
erodrz 4:60e3365da280 12 // Hilos para ejecutar tareas concurrentes.
erodrz 4:60e3365da280 13 Thread Thread1; // Primer hilo para sensor de proximidad.
erodrz 4:60e3365da280 14 Thread Thread2; // Segundo hilo para sensor de proximidad.
erodrz 4:60e3365da280 15 Thread Thread3; // Tercer hilo para sensor de proximidad.
erodrz 4:60e3365da280 16 Thread Thread4; // Cuarto hilo para sensor de proximidad.
erodrz 4:60e3365da280 17 Thread Thread5; // Hilo para manejar el joystick.
erodrz 4:60e3365da280 18 Thread Thread6; // Hilo para manejar los comandos de voz recibidos por la Raspberry PI.
erodrz 4:60e3365da280 19 Thread Thread7; // Hilo para manejar la selección de modo de la silla.
thevic16 0:b8adbf13199b 20
erodrz 4:60e3365da280 21 // Variables globales de distancias en el entorno de la silla de ruedas.
erodrz 4:60e3365da280 22 int Distance1 = 0; // Distancia adelante de la silla.
erodrz 4:60e3365da280 23 int Distance2 = 0; // Distancia atrás de la silla.
erodrz 4:60e3365da280 24 int Distance3 = 0; // Distancia a la izquierda de la silla.
erodrz 4:60e3365da280 25 int Distance4 = 0; // Distancia a la derecha de la silla.
erodrz 5:9f30f0a6dc76 26 int DistanceLimit = 50; // Distancia límite de acercamiento a un Obstaculo permitida por la silla.
thevic16 0:b8adbf13199b 27
erodrz 4:60e3365da280 28 // Entradas digitales para selección de modos de la silla.
erodrz 4:60e3365da280 29 DigitalIn Modo1(D8); // Modo manual.
erodrz 4:60e3365da280 30 DigitalIn Modo2(D9); // Modo por comandos del joystick.
erodrz 4:60e3365da280 31 DigitalIn Modo3(D10); // Modo por comandos de voz.
erodrz 4:60e3365da280 32 DigitalIn Modo4(D11); // Modo por rutas autónomas
thevic16 0:b8adbf13199b 33
erodrz 4:60e3365da280 34 // Interfaz serial para comunicación con la Raspberry PI.
erodrz 4:60e3365da280 35 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 36
erodrz 4:60e3365da280 37 // Salidas digitales y PWM para controlar el driver de los motores.
erodrz 5:9f30f0a6dc76 38 DigitalOut Direccion1(D15); // Dirección del motor 1.
erodrz 5:9f30f0a6dc76 39 PwmOut PWM_Velocidad1(D14); // Velocidad del motor 1.
erodrz 5:9f30f0a6dc76 40 DigitalOut Direccion2(PC_6); // Dirección del motor 2.
erodrz 5:9f30f0a6dc76 41 PwmOut PWM_Velocidad2(PB_15); // Velocidad del motor 2.
thevic16 1:17ea74f31633 42
erodrz 4:60e3365da280 43 // Salida para manejar la señal del buzzer de alertas.
erodrz 5:9f30f0a6dc76 44 PwmOut Buzzer(PE_14);
thevic16 3:3d54fd4109c0 45
erodrz 4:60e3365da280 46 // Función para reproducir un sonido del buzzer cuando se detecta proximidad a un obstáculo.
erodrz 4:60e3365da280 47 void Reproducir_Buzzer_Proximidad(void)
erodrz 4:60e3365da280 48 {
erodrz 4:60e3365da280 49 Timer BuzzTime;
erodrz 4:60e3365da280 50 BuzzTime.reset();
erodrz 4:60e3365da280 51 BuzzTime.start();
erodrz 4:60e3365da280 52 while(BuzzTime.read_us() < 3000000) // Ejecutar sonido del buzzer por 3 segundos.
erodrz 4:60e3365da280 53 {
erodrz 4:60e3365da280 54 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 55 Buzzer.write(0.5);
erodrz 4:60e3365da280 56 thread_sleep_for(200);
erodrz 4:60e3365da280 57
erodrz 4:60e3365da280 58 Buzzer.period(1.0/Nota_A4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo).
erodrz 4:60e3365da280 59 Buzzer.write(0.5);
erodrz 4:60e3365da280 60 thread_sleep_for(200);
erodrz 4:60e3365da280 61
erodrz 4:60e3365da280 62 Buzzer.period(1.0/Nota_E4); // Configurando el período, que es equivalente a frecuencia (veces que se reproducirá el tono por segundo).
erodrz 4:60e3365da280 63 Buzzer.write(0.5);
erodrz 4:60e3365da280 64 thread_sleep_for(200);
erodrz 4:60e3365da280 65 }
erodrz 5:9f30f0a6dc76 66 Buzzer.write(0);
erodrz 4:60e3365da280 67 BuzzTime.stop();
erodrz 4:60e3365da280 68 }
thevic16 3:3d54fd4109c0 69
erodrz 4:60e3365da280 70 // Función para limpiar caracteres presentes en el buffer de la interfaz serial.
erodrz 4:60e3365da280 71 void LimpiarSerialBuffer(void)
erodrz 2:4f5a0c64d9cd 72 {
erodrz 2:4f5a0c64d9cd 73 char char1 = 0;
erodrz 4:60e3365da280 74 while(PC.readable())
erodrz 2:4f5a0c64d9cd 75 {
erodrz 4:60e3365da280 76 char1 = PC.getc();
erodrz 2:4f5a0c64d9cd 77 }
erodrz 2:4f5a0c64d9cd 78 return;
erodrz 2:4f5a0c64d9cd 79 }
erodrz 2:4f5a0c64d9cd 80
erodrz 4:60e3365da280 81 // Función para moder la silla hacia adelante.
erodrz 4:60e3365da280 82 void Mover_Hacia_Adelante(int Tiempo)
erodrz 4:60e3365da280 83 {
erodrz 4:60e3365da280 84 Direccion1 = 0; // En dirección de las manecillas del reloj.
erodrz 4:60e3365da280 85 Direccion2 = 0; // En dirección de las manecillas del reloj.
thevic16 3:3d54fd4109c0 86
erodrz 4:60e3365da280 87 PWM_Velocidad1.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 88 PWM_Velocidad1.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 89
erodrz 4:60e3365da280 90 PWM_Velocidad2.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 91 PWM_Velocidad2.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 92
erodrz 4:60e3365da280 93 thread_sleep_for(Tiempo);
erodrz 4:60e3365da280 94
erodrz 4:60e3365da280 95 PWM_Velocidad1.write(0.0f);
erodrz 4:60e3365da280 96 PWM_Velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 97 }
thevic16 3:3d54fd4109c0 98
erodrz 4:60e3365da280 99 // Función para moder la silla hacia atrás.
erodrz 4:60e3365da280 100 void Mover_Hacia_Atras(int Tiempo)
erodrz 4:60e3365da280 101 {
erodrz 4:60e3365da280 102 Direccion1 = 1; // En dirección contraria a las manecillas del reloj.
erodrz 4:60e3365da280 103 Direccion2 = 1; // En dirección contraria a las manecillas del reloj.
erodrz 4:60e3365da280 104
erodrz 4:60e3365da280 105 PWM_Velocidad1.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 106 PWM_Velocidad1.write(0.15f); // %25 del duty cicle.
thevic16 3:3d54fd4109c0 107
erodrz 4:60e3365da280 108 PWM_Velocidad2.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 109 PWM_Velocidad2.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 110
erodrz 4:60e3365da280 111 thread_sleep_for(Tiempo);
erodrz 4:60e3365da280 112
erodrz 4:60e3365da280 113 PWM_Velocidad1.write(0.0f);
erodrz 4:60e3365da280 114 PWM_Velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 115 }
thevic16 3:3d54fd4109c0 116
erodrz 4:60e3365da280 117 // Función para moder la silla hacia la izquierda.
erodrz 4:60e3365da280 118 void Mover_Hacia_Izquierda(int Tiempo)
erodrz 4:60e3365da280 119 {
erodrz 4:60e3365da280 120 Direccion1 = 1; // En dirección contraria a las manecillas del reloj.
erodrz 4:60e3365da280 121 Direccion2 = 0; // En dirección de las manecillas del reloj.
thevic16 3:3d54fd4109c0 122
erodrz 4:60e3365da280 123 PWM_Velocidad1.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 124 PWM_Velocidad1.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 125
erodrz 4:60e3365da280 126 PWM_Velocidad2.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 127 PWM_Velocidad2.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 128
erodrz 4:60e3365da280 129 thread_sleep_for(Tiempo);
erodrz 4:60e3365da280 130
erodrz 4:60e3365da280 131 PWM_Velocidad1.write(0.0f);
erodrz 4:60e3365da280 132 PWM_Velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 133 }
thevic16 3:3d54fd4109c0 134
erodrz 4:60e3365da280 135 // Función para moder la silla hacia la derecha.
erodrz 4:60e3365da280 136 void Mover_Hacia_Derecha(int Tiempo)
erodrz 4:60e3365da280 137 {
erodrz 4:60e3365da280 138 Direccion1 = 0; // En dirección de las manecillas del reloj.
erodrz 4:60e3365da280 139 Direccion2 = 1; // En dirección contraria a las manecillas del reloj.
thevic16 3:3d54fd4109c0 140
erodrz 4:60e3365da280 141 PWM_Velocidad1.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 142 PWM_Velocidad1.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 143
erodrz 4:60e3365da280 144 PWM_Velocidad2.period(0.0001f); // Declaramos el período.
erodrz 4:60e3365da280 145 PWM_Velocidad2.write(0.15f); // %25 del duty cicle.
erodrz 4:60e3365da280 146
erodrz 4:60e3365da280 147 thread_sleep_for(Tiempo);
erodrz 4:60e3365da280 148
erodrz 4:60e3365da280 149 PWM_Velocidad1.write(0.0f);
erodrz 4:60e3365da280 150 PWM_Velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 151 }
thevic16 3:3d54fd4109c0 152
erodrz 5:9f30f0a6dc76 153 // Función para leer el sensor de proximidad 1. ADELANTE
erodrz 4:60e3365da280 154 void Thread1_HCSR04()
thevic16 0:b8adbf13199b 155 {
erodrz 4:60e3365da280 156 DigitalOut Trigger(D0);
erodrz 4:60e3365da280 157 DigitalIn Echo(D1);
erodrz 4:60e3365da280 158 Timer Sonar;
erodrz 4:60e3365da280 159 int Correccion = 0;
erodrz 4:60e3365da280 160 Sonar.reset();
erodrz 4:60e3365da280 161 Sonar.start();
erodrz 4:60e3365da280 162 while(Echo == 2)
thevic16 1:17ea74f31633 163 {
thevic16 1:17ea74f31633 164
thevic16 1:17ea74f31633 165 };
erodrz 4:60e3365da280 166 Sonar.stop();
erodrz 4:60e3365da280 167 Correccion = Sonar.read_us();
erodrz 4:60e3365da280 168 printf("Sensor de proximidad 1: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
thevic16 1:17ea74f31633 169 while(1)
thevic16 1:17ea74f31633 170 {
erodrz 4:60e3365da280 171 Trigger = 1;
erodrz 4:60e3365da280 172 Sonar.reset();
thevic16 1:17ea74f31633 173 wait_us(10.0);
erodrz 4:60e3365da280 174 Trigger = 0;
erodrz 4:60e3365da280 175 while(Echo == 0)
thevic16 1:17ea74f31633 176 {
thevic16 1:17ea74f31633 177
thevic16 1:17ea74f31633 178 };
erodrz 4:60e3365da280 179 Sonar.start();
erodrz 4:60e3365da280 180 while(Echo == 1)
erodrz 4:60e3365da280 181 {
erodrz 4:60e3365da280 182
erodrz 4:60e3365da280 183 };
erodrz 4:60e3365da280 184 Sonar.stop();
erodrz 4:60e3365da280 185 Distance1 = (Sonar.read_us()-Correccion)/58.0;
erodrz 4:60e3365da280 186 //printf("Sensor de proximidad 1: %d cm \n\r",Distance1);
thevic16 1:17ea74f31633 187 thread_sleep_for(1000);
thevic16 1:17ea74f31633 188 }
thevic16 1:17ea74f31633 189 }
thevic16 0:b8adbf13199b 190
erodrz 5:9f30f0a6dc76 191 // Función para leer el sensor de proximidad 2. //ATRAS
erodrz 4:60e3365da280 192 void Thread2_HCSR04()
thevic16 1:17ea74f31633 193 {
erodrz 4:60e3365da280 194 DigitalOut Trigger(D2);
erodrz 4:60e3365da280 195 DigitalIn Echo(D3);
erodrz 4:60e3365da280 196 Timer Sonar;
erodrz 4:60e3365da280 197 int Correccion = 0;
erodrz 4:60e3365da280 198 Sonar.reset();
erodrz 4:60e3365da280 199 Sonar.start();
erodrz 4:60e3365da280 200 while(Echo == 2)
thevic16 1:17ea74f31633 201 {
thevic16 1:17ea74f31633 202
thevic16 1:17ea74f31633 203 };
erodrz 4:60e3365da280 204 Sonar.stop();
erodrz 4:60e3365da280 205 Correccion = Sonar.read_us();
erodrz 4:60e3365da280 206 printf("Sensor de proximidad 2: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
thevic16 1:17ea74f31633 207 while(1)
thevic16 1:17ea74f31633 208 {
erodrz 4:60e3365da280 209 Trigger = 1;
erodrz 4:60e3365da280 210 Sonar.reset();
thevic16 0:b8adbf13199b 211 wait_us(10.0);
erodrz 4:60e3365da280 212 Trigger = 0;
erodrz 4:60e3365da280 213 while(Echo == 0)
thevic16 1:17ea74f31633 214 {
thevic16 1:17ea74f31633 215
thevic16 1:17ea74f31633 216 };
erodrz 4:60e3365da280 217 Sonar.start();
erodrz 4:60e3365da280 218 while(Echo == 1)
thevic16 1:17ea74f31633 219 {
thevic16 1:17ea74f31633 220
thevic16 1:17ea74f31633 221 };
erodrz 4:60e3365da280 222 Sonar.stop();
erodrz 4:60e3365da280 223 Distance2 = (Sonar.read_us()-Correccion)/58.0;
erodrz 4:60e3365da280 224 //printf("Sensor de proximidad 2: %d cm \n\r",Distance2);
thevic16 1:17ea74f31633 225 thread_sleep_for(1000);
thevic16 1:17ea74f31633 226 }
thevic16 0:b8adbf13199b 227 }
thevic16 0:b8adbf13199b 228
erodrz 5:9f30f0a6dc76 229 // Función para leer el sensor de proximidad 3. //IZQUIERDA
erodrz 4:60e3365da280 230 void Thread3_HCSR04()
thevic16 0:b8adbf13199b 231 {
erodrz 4:60e3365da280 232 DigitalOut Trigger(D4);
erodrz 4:60e3365da280 233 DigitalIn Echo(D5);
erodrz 4:60e3365da280 234 Timer Sonar;
erodrz 4:60e3365da280 235 int Correccion = 0;
erodrz 4:60e3365da280 236 Sonar.reset();
erodrz 4:60e3365da280 237 Sonar.start();
erodrz 4:60e3365da280 238 while(Echo == 2)
thevic16 1:17ea74f31633 239 {
thevic16 1:17ea74f31633 240
thevic16 1:17ea74f31633 241 };
erodrz 4:60e3365da280 242 Sonar.stop();
erodrz 4:60e3365da280 243 Correccion = Sonar.read_us();
erodrz 4:60e3365da280 244 printf("Sensor de proximidad 3: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
thevic16 1:17ea74f31633 245 while(1)
thevic16 1:17ea74f31633 246 {
erodrz 4:60e3365da280 247 Trigger = 1;
erodrz 4:60e3365da280 248 Sonar.reset();
thevic16 0:b8adbf13199b 249 wait_us(10.0);
erodrz 4:60e3365da280 250 Trigger = 0;
erodrz 4:60e3365da280 251 while(Echo == 0)
thevic16 1:17ea74f31633 252 {
thevic16 1:17ea74f31633 253
thevic16 1:17ea74f31633 254 };
erodrz 4:60e3365da280 255 Sonar.start();
erodrz 4:60e3365da280 256 while(Echo == 1)
thevic16 1:17ea74f31633 257 {
thevic16 1:17ea74f31633 258
thevic16 1:17ea74f31633 259 };
erodrz 4:60e3365da280 260 Sonar.stop();
erodrz 4:60e3365da280 261 Distance3 = (Sonar.read_us()-Correccion)/58.0;
erodrz 4:60e3365da280 262 //printf("Sensor de proximidad 3: %d cm \n\r",Distance3);
thevic16 1:17ea74f31633 263 thread_sleep_for(1000);
thevic16 1:17ea74f31633 264 }
thevic16 0:b8adbf13199b 265 }
thevic16 1:17ea74f31633 266
erodrz 5:9f30f0a6dc76 267 // Función para leer el sensor de proximidad 4. //DERECHA
erodrz 4:60e3365da280 268 void Thread4_HCSR04()
thevic16 0:b8adbf13199b 269 {
erodrz 4:60e3365da280 270 DigitalOut Trigger(D6);
erodrz 4:60e3365da280 271 DigitalIn Echo(D7);
erodrz 4:60e3365da280 272 Timer Sonar;
erodrz 4:60e3365da280 273 int Correccion = 0;
erodrz 4:60e3365da280 274 Sonar.reset();
erodrz 4:60e3365da280 275 Sonar.start();
erodrz 4:60e3365da280 276 while(Echo == 2)
thevic16 1:17ea74f31633 277 {
thevic16 0:b8adbf13199b 278
thevic16 1:17ea74f31633 279 };
erodrz 4:60e3365da280 280 Sonar.stop();
erodrz 4:60e3365da280 281 Correccion = Sonar.read_us();
erodrz 4:60e3365da280 282 printf("Sensor de proximidad 4: El retardo aproximado del temporizador de sobrecarga del software es %d uS\n\r",Correccion);
thevic16 1:17ea74f31633 283 while(1)
thevic16 1:17ea74f31633 284 {
erodrz 4:60e3365da280 285 Trigger = 1;
erodrz 4:60e3365da280 286 Sonar.reset();
thevic16 0:b8adbf13199b 287 wait_us(10.0);
erodrz 4:60e3365da280 288 Trigger = 0;
erodrz 4:60e3365da280 289 while(Echo == 0)
thevic16 1:17ea74f31633 290 {
thevic16 1:17ea74f31633 291
thevic16 1:17ea74f31633 292 };
erodrz 4:60e3365da280 293 Sonar.start();
erodrz 4:60e3365da280 294 while(Echo == 1)
thevic16 1:17ea74f31633 295 {
thevic16 1:17ea74f31633 296
thevic16 1:17ea74f31633 297 };
erodrz 4:60e3365da280 298 Sonar.stop();
erodrz 4:60e3365da280 299 Distance4 = (Sonar.read_us()-Correccion)/58.0;
erodrz 4:60e3365da280 300 //printf("Sensor de proximidad 4: %d cm \n\r",Distance4);
thevic16 1:17ea74f31633 301 thread_sleep_for(1000);
thevic16 1:17ea74f31633 302 }
thevic16 0:b8adbf13199b 303 }
thevic16 0:b8adbf13199b 304
erodrz 4:60e3365da280 305 // Función para leer valores del joystick y ejecutar sus comandos.
erodrz 4:60e3365da280 306 void Thread5_Joystick()
thevic16 1:17ea74f31633 307 {
erodrz 5:9f30f0a6dc76 308 SparkfunAnalogJoystick JoyStick(A1,A0,PE_0);
thevic16 0:b8adbf13199b 309 float X;
thevic16 0:b8adbf13199b 310 float Y;
thevic16 1:17ea74f31633 311 while(1)
thevic16 1:17ea74f31633 312 {
erodrz 4:60e3365da280 313 if(!Modo1 && Modo2 && !Modo3 && !Modo4)
thevic16 1:17ea74f31633 314 {
thevic16 1:17ea74f31633 315 X = JoyStick.xAxis();
thevic16 1:17ea74f31633 316 Y = JoyStick.yAxis();
erodrz 2:4f5a0c64d9cd 317 /*
erodrz 4:60e3365da280 318 printf("X-Axis: %f\n\r",X);
erodrz 4:60e3365da280 319 printf("Y-Axis: %f\n\r",Y);
thevic16 1:17ea74f31633 320 printf(" \n\r");
erodrz 2:4f5a0c64d9cd 321 */
erodrz 4:60e3365da280 322 if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f)
erodrz 5:9f30f0a6dc76 323 {
erodrz 5:9f30f0a6dc76 324 if(Distance2 > DistanceLimit)
thevic16 1:17ea74f31633 325 {
erodrz 5:9f30f0a6dc76 326 printf("Comandos del joystick: Hacia atras. \r \n");
erodrz 5:9f30f0a6dc76 327 Mover_Hacia_Atras(3000);
thevic16 1:17ea74f31633 328 }
thevic16 1:17ea74f31633 329 else
thevic16 1:17ea74f31633 330 {
erodrz 5:9f30f0a6dc76 331 printf("Comandos del joystick: Obstaculo hacia atras. \r \n");
erodrz 4:60e3365da280 332 Reproducir_Buzzer_Proximidad();
thevic16 1:17ea74f31633 333 }
erodrz 4:60e3365da280 334 thread_sleep_for(500);
thevic16 1:17ea74f31633 335 }
erodrz 4:60e3365da280 336 if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f)
erodrz 5:9f30f0a6dc76 337 {
erodrz 5:9f30f0a6dc76 338 if(Distance1 > DistanceLimit)
thevic16 1:17ea74f31633 339 {
erodrz 5:9f30f0a6dc76 340 printf("Comandos del joystick: Hacia adelante. \r \n");
erodrz 5:9f30f0a6dc76 341 Mover_Hacia_Adelante(3000);
thevic16 1:17ea74f31633 342 }
thevic16 1:17ea74f31633 343 else
thevic16 1:17ea74f31633 344 {
erodrz 5:9f30f0a6dc76 345 printf("Comandos del joystick: Obstaculo hacia adelante. \r \n");
erodrz 4:60e3365da280 346 Reproducir_Buzzer_Proximidad();
erodrz 4:60e3365da280 347 }
erodrz 4:60e3365da280 348 thread_sleep_for(500);
thevic16 1:17ea74f31633 349 }
thevic16 1:17ea74f31633 350 if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f)
thevic16 1:17ea74f31633 351 {
erodrz 4:60e3365da280 352 if(Distance3 > DistanceLimit)
thevic16 1:17ea74f31633 353 {
erodrz 4:60e3365da280 354 printf("Comandos del joystick: Hacia la izquierda. \r \n");
erodrz 5:9f30f0a6dc76 355 Mover_Hacia_Izquierda(3000);
thevic16 1:17ea74f31633 356 }
thevic16 1:17ea74f31633 357 else
thevic16 1:17ea74f31633 358 {
erodrz 4:60e3365da280 359 printf("Comandos del joystick: Obstaculo hacia la izquierda. \r \n");
erodrz 4:60e3365da280 360 Reproducir_Buzzer_Proximidad();
thevic16 1:17ea74f31633 361 }
erodrz 4:60e3365da280 362 thread_sleep_for(500);
thevic16 1:17ea74f31633 363 }
erodrz 4:60e3365da280 364 if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f)
thevic16 1:17ea74f31633 365 {
erodrz 4:60e3365da280 366 if(Distance4 > DistanceLimit)
thevic16 1:17ea74f31633 367 {
erodrz 4:60e3365da280 368 printf("Comandos del joystick: Hacia la derecha. \r \n");
erodrz 5:9f30f0a6dc76 369 Mover_Hacia_Derecha(3000);
thevic16 1:17ea74f31633 370 }
thevic16 1:17ea74f31633 371 else
thevic16 1:17ea74f31633 372 {
erodrz 4:60e3365da280 373 printf("Comandos del joystick: Obstaculo hacia la derecha. \r \n");
erodrz 4:60e3365da280 374 Reproducir_Buzzer_Proximidad();
thevic16 1:17ea74f31633 375 }
erodrz 2:4f5a0c64d9cd 376 thread_sleep_for(500);
thevic16 1:17ea74f31633 377 }
erodrz 4:60e3365da280 378 thread_sleep_for(5);
thevic16 1:17ea74f31633 379 }
thevic16 1:17ea74f31633 380 }
thevic16 1:17ea74f31633 381 }
thevic16 1:17ea74f31633 382
erodrz 4:60e3365da280 383 // Función para leer datos del serial con caracteres de comandos de voz y ejecutar instrucciones.
erodrz 4:60e3365da280 384 void Thread6_ComandosVoz()
thevic16 1:17ea74f31633 385 {
thevic16 0:b8adbf13199b 386 while(1)
thevic16 0:b8adbf13199b 387 {
erodrz 4:60e3365da280 388 if(!Modo1 && !Modo2 && Modo3 && !Modo4)
thevic16 1:17ea74f31633 389 {
erodrz 4:60e3365da280 390 LimpiarSerialBuffer();
erodrz 4:60e3365da280 391 char c = PC.getc();
erodrz 4:60e3365da280 392 thread_sleep_for(5); // Retraso necesario para que el compilador se dé cuenta del orden correcto de ejecución.
erodrz 4:60e3365da280 393 int m = Modo3.read();
erodrz 4:60e3365da280 394 printf("Estado del modo 3 (Comandos de Voz): %d \r \n",m);
erodrz 4:60e3365da280 395 if(m == 1)
erodrz 4:60e3365da280 396 {
thevic16 3:3d54fd4109c0 397 if(c == 'w')
thevic16 1:17ea74f31633 398 {
erodrz 4:60e3365da280 399 //printf("Distance1 - %d \r \n",Distance1);
erodrz 4:60e3365da280 400 if(Distance1 > DistanceLimit)
thevic16 3:3d54fd4109c0 401 {
erodrz 4:60e3365da280 402 PC.printf("Comandos de voz: Hacia adelante. \r \n");
erodrz 4:60e3365da280 403 Mover_Hacia_Adelante(3000);
thevic16 3:3d54fd4109c0 404 }
thevic16 3:3d54fd4109c0 405 else
thevic16 3:3d54fd4109c0 406 {
erodrz 4:60e3365da280 407 printf("Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
erodrz 4:60e3365da280 408 Reproducir_Buzzer_Proximidad();
thevic16 3:3d54fd4109c0 409 }
thevic16 3:3d54fd4109c0 410 thread_sleep_for(1000);
thevic16 1:17ea74f31633 411 }
thevic16 3:3d54fd4109c0 412 if(c == 's')
thevic16 1:17ea74f31633 413 {
erodrz 4:60e3365da280 414 //printf("Distance2 - %d \r \n",Distance2);
erodrz 4:60e3365da280 415 if(Distance2 > DistanceLimit)
thevic16 3:3d54fd4109c0 416 {
erodrz 4:60e3365da280 417 PC.printf("Comandos de voz: Hacia atras. \r \n");
erodrz 4:60e3365da280 418 Mover_Hacia_Atras(3000);
thevic16 3:3d54fd4109c0 419 }
thevic16 3:3d54fd4109c0 420 else
thevic16 3:3d54fd4109c0 421 {
erodrz 4:60e3365da280 422 printf("Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
erodrz 4:60e3365da280 423 Reproducir_Buzzer_Proximidad();
thevic16 3:3d54fd4109c0 424 }
thevic16 3:3d54fd4109c0 425 thread_sleep_for(1000);
erodrz 4:60e3365da280 426 }
thevic16 3:3d54fd4109c0 427 if(c == 'a')
thevic16 1:17ea74f31633 428 {
erodrz 4:60e3365da280 429 //printf("Distance3 - %d \r \n",Distance3);
erodrz 4:60e3365da280 430 if(Distance3 > DistanceLimit)
thevic16 3:3d54fd4109c0 431 {
erodrz 4:60e3365da280 432 PC.printf("Comandos de voz: Hacia la izquierda. \r \n");
erodrz 4:60e3365da280 433 Mover_Hacia_Izquierda(3000);
thevic16 3:3d54fd4109c0 434 }
thevic16 3:3d54fd4109c0 435 else
thevic16 3:3d54fd4109c0 436 {
erodrz 4:60e3365da280 437 printf("Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
erodrz 4:60e3365da280 438 Reproducir_Buzzer_Proximidad();
thevic16 3:3d54fd4109c0 439 }
thevic16 3:3d54fd4109c0 440 thread_sleep_for(1000);
thevic16 1:17ea74f31633 441 }
thevic16 3:3d54fd4109c0 442 if(c == 'd')
erodrz 4:60e3365da280 443 {
erodrz 4:60e3365da280 444 //printf("Distance4 - %d \r \n",Distance4);
erodrz 4:60e3365da280 445 if(Distance4 > DistanceLimit)
thevic16 3:3d54fd4109c0 446 {
erodrz 4:60e3365da280 447 PC.printf("Comandos de voz: Hacia la derecha. \r \n");
erodrz 4:60e3365da280 448 Mover_Hacia_Derecha(3000);
erodrz 4:60e3365da280 449 }
thevic16 3:3d54fd4109c0 450 else
thevic16 3:3d54fd4109c0 451 {
erodrz 4:60e3365da280 452 printf("Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
erodrz 4:60e3365da280 453 Reproducir_Buzzer_Proximidad();
thevic16 3:3d54fd4109c0 454 }
thevic16 3:3d54fd4109c0 455 thread_sleep_for(1000);
erodrz 2:4f5a0c64d9cd 456 }
erodrz 2:4f5a0c64d9cd 457 }
erodrz 2:4f5a0c64d9cd 458 c = ' ';
erodrz 4:60e3365da280 459 thread_sleep_for(5);
erodrz 2:4f5a0c64d9cd 460 }
thevic16 1:17ea74f31633 461 }
thevic16 1:17ea74f31633 462 }
thevic16 1:17ea74f31633 463
erodrz 4:60e3365da280 464 // Función para seleccionar el modo de operación de la silla.
erodrz 4:60e3365da280 465 void Thread7_IndicarModo()
thevic16 1:17ea74f31633 466 {
erodrz 4:60e3365da280 467 bool EstadoModo1 = false;
erodrz 4:60e3365da280 468 bool EstadoModo2 = false;
erodrz 4:60e3365da280 469 bool EstadoModo3 = false;
erodrz 4:60e3365da280 470 bool EstadoModo4 = false;
erodrz 4:60e3365da280 471 while(true)
thevic16 1:17ea74f31633 472 {
erodrz 4:60e3365da280 473 if(Modo1 && !Modo2 && !Modo3 && !Modo4 && !EstadoModo1)
thevic16 1:17ea74f31633 474 {
erodrz 4:60e3365da280 475 printf("Operando: Modo manual. \r \n");
erodrz 4:60e3365da280 476 EstadoModo1 = true;
erodrz 4:60e3365da280 477 EstadoModo2 = false;
erodrz 4:60e3365da280 478 EstadoModo3 = false;
erodrz 4:60e3365da280 479 EstadoModo4 = false;
thevic16 0:b8adbf13199b 480 }
erodrz 4:60e3365da280 481 if(!Modo1 && Modo2 && !Modo3 && !Modo4 && !EstadoModo2)
thevic16 1:17ea74f31633 482 {
erodrz 4:60e3365da280 483 printf("Operando: Modo de comandos de joystick. \r \n");
erodrz 4:60e3365da280 484 EstadoModo1 = false;
erodrz 4:60e3365da280 485 EstadoModo2 = true;
erodrz 4:60e3365da280 486 EstadoModo3 = false;
erodrz 4:60e3365da280 487 EstadoModo4 = false;
erodrz 4:60e3365da280 488 }
erodrz 4:60e3365da280 489 if(!Modo1 && !Modo2 && Modo3 && !Modo4 && !EstadoModo3)
thevic16 1:17ea74f31633 490 {
erodrz 4:60e3365da280 491 printf("Operando: Modo de comandos de voz. \r \n");
erodrz 4:60e3365da280 492 EstadoModo1 = false;
erodrz 4:60e3365da280 493 EstadoModo2 = false;
erodrz 4:60e3365da280 494 EstadoModo3 = true;
erodrz 4:60e3365da280 495 EstadoModo4 = false;
erodrz 4:60e3365da280 496 }
erodrz 4:60e3365da280 497 if(!Modo1 && !Modo2 && !Modo3 && Modo4 && !EstadoModo4)
thevic16 1:17ea74f31633 498 {
erodrz 4:60e3365da280 499 printf("Operando: Modo de rutas autonomas. \r \n");
erodrz 4:60e3365da280 500 EstadoModo1 = false;
erodrz 4:60e3365da280 501 EstadoModo2 = false;
erodrz 4:60e3365da280 502 EstadoModo3 = false;
erodrz 4:60e3365da280 503 EstadoModo4 = true;
erodrz 4:60e3365da280 504 }
thevic16 0:b8adbf13199b 505 }
thevic16 1:17ea74f31633 506 }
thevic16 0:b8adbf13199b 507
erodrz 4:60e3365da280 508 // Proceso principal de todo el software ejecutado por el microprocesador.
thevic16 0:b8adbf13199b 509 int main()
thevic16 0:b8adbf13199b 510 {
erodrz 4:60e3365da280 511 Thread1.start(Thread1_HCSR04);
erodrz 2:4f5a0c64d9cd 512 thread_sleep_for(200);
erodrz 4:60e3365da280 513 Thread2.start(Thread2_HCSR04);
erodrz 2:4f5a0c64d9cd 514 thread_sleep_for(200);
erodrz 4:60e3365da280 515 Thread3.start(Thread3_HCSR04);
erodrz 2:4f5a0c64d9cd 516 thread_sleep_for(200);
erodrz 4:60e3365da280 517 Thread4.start(Thread4_HCSR04);
erodrz 2:4f5a0c64d9cd 518 thread_sleep_for(200);
erodrz 4:60e3365da280 519 Thread5.start(Thread5_Joystick);
erodrz 2:4f5a0c64d9cd 520 thread_sleep_for(200);
erodrz 4:60e3365da280 521 Thread6.start(Thread6_ComandosVoz);
erodrz 2:4f5a0c64d9cd 522 thread_sleep_for(200);
erodrz 4:60e3365da280 523 Thread7.start(Thread7_IndicarModo);
erodrz 2:4f5a0c64d9cd 524 thread_sleep_for(200);
erodrz 4:60e3365da280 525 }