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