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