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

Newest version of Wheelchair Logic

Dependencies:   MPU6050 SparkfunAnalogJoystick

main.cpp@10:c036607dbf6e, 2021-11-05 (annotated)

Committer:
erodrz
Date:
Fri Nov 05 00:26:15 2021 +0000
Revision:
10:c036607dbf6e
Parent:
9:4a1c40f8b2d7 
WheelchairLogicv5

Who changed what in which revision?

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