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

New

Dependencies:   SparkfunAnalogJoystick

main.cpp@4:60e3365da280, 2021-07-22 (annotated)

Committer:
erodrz
Date:
Thu Jul 22 01:56:42 2021 +0000
Revision:
4:60e3365da280
Parent:
3:3d54fd4109c0
Child:
5:9f30f0a6dc76
Wheelchair Logic v3

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