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

New

Dependencies:   SparkfunAnalogJoystick

main.cpp@7:ae7b2184b737, 2021-08-02 (annotated)

Committer:
erodrz
Date:
Mon Aug 02 00:32:34 2021 +0000
Revision:
7:ae7b2184b737
Parent:
6:7c987ba78aa3 
v4

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