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