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

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?

UserRevisionLine numberNew contents of line
erodrz 4:60e3365da280 1 // Librerías para abstraer funciones de MBed para manejar el microprocesador y los sensores.
thevic16 0:b8adbf13199b 2 #include "mbed.h"
thevic16 0:b8adbf13199b 3 #include "platform/mbed_thread.h"
thevic16 0:b8adbf13199b 4 #include "SparkfunAnalogJoystick.h"
erodrz 7:ae7b2184b737 5 #include "MPU6050.h"
thevic16 0:b8adbf13199b 6
erodrz 4:60e3365da280 7 // Constantes de programación para procesos y funciones.
erodrz 4:60e3365da280 8 // Valor en frecuencia de las notas reproducidas por el Buzzer que da avisos al usuario de la silla.
erodrz 4:60e3365da280 9 #define Nota_C4 262
erodrz 4:60e3365da280 10 #define Nota_A4 440
erodrz 4:60e3365da280 11 #define Nota_E4 659
erodrz 4:60e3365da280 12
erodrz 4:60e3365da280 13 // Hilos para ejecutar tareas concurrentes.
erodrz 4:60e3365da280 14 Thread Thread1; // Primer hilo para sensor de proximidad.
erodrz 4:60e3365da280 15 Thread Thread2; // Segundo hilo para sensor de proximidad.
erodrz 4:60e3365da280 16 Thread Thread3; // Tercer hilo para sensor de proximidad.
erodrz 4:60e3365da280 17 Thread Thread4; // Cuarto hilo para sensor de proximidad.
erodrz 4:60e3365da280 18 Thread Thread5; // Hilo para manejar el joystick.
erodrz 4:60e3365da280 19 Thread Thread6; // Hilo para manejar los comandos de voz recibidos por la Raspberry PI.
erodrz 4:60e3365da280 20 Thread Thread7; // Hilo para manejar la selección de modo de la silla.
erodrz 7:ae7b2184b737 21 Thread Thread8; // Hilo para manejar la detección de caídas y movimientos bruscos.
thevic16 0:b8adbf13199b 22
erodrz 4:60e3365da280 23 // Variables globales de distancias en el entorno de la silla de ruedas.
erodrz 4:60e3365da280 24 int Distance1 = 0; // Distancia adelante de la silla.
erodrz 4:60e3365da280 25 int Distance2 = 0; // Distancia atrás de la silla.
erodrz 4:60e3365da280 26 int Distance3 = 0; // Distancia a la izquierda de la silla.
erodrz 4:60e3365da280 27 int Distance4 = 0; // Distancia a la derecha de la silla.
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 }