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

Newest version of Wheelchair Logic

Dependencies:   MPU6050 SparkfunAnalogJoystick

main.cpp@8:418ff0ef9463, 2021-10-01 (annotated)

Committer:
thevic16
Date:
Fri Oct 01 12:53:13 2021 +0000
Revision:
8:418ff0ef9463
Parent:
7:ae7b2184b737
Child:
9:4a1c40f8b2d7
- Version 4

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