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

Wheelchair Logic v3 Project (New)

Dependencies:   SparkfunAnalogJoystick

main.cpp@2:4f5a0c64d9cd, 2021-06-25 (annotated)

Committer:
erodrz
Date:
Fri Jun 25 00:34:17 2021 +0000
Revision:
2:4f5a0c64d9cd
Parent:
1:17ea74f31633
Child:
3:3d54fd4109c0
prueba;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
thevic16 0:b8adbf13199b 1 #include "mbed.h"
thevic16 0:b8adbf13199b 2 #include "platform/mbed_thread.h"
thevic16 0:b8adbf13199b 3 #include "SparkfunAnalogJoystick.h"
thevic16 0:b8adbf13199b 4
thevic16 0:b8adbf13199b 5 #define M_PI 3.14159265358979323846
thevic16 0:b8adbf13199b 6
thevic16 0:b8adbf13199b 7 Thread thread1;
thevic16 0:b8adbf13199b 8 Thread thread2;
thevic16 0:b8adbf13199b 9 Thread thread3;
thevic16 0:b8adbf13199b 10 Thread thread4;
thevic16 0:b8adbf13199b 11 Thread thread5;
thevic16 1:17ea74f31633 12 Thread thread6;
thevic16 1:17ea74f31633 13 Thread thread7;
thevic16 0:b8adbf13199b 14
thevic16 1:17ea74f31633 15 int distance1 = 0; //distancia adelante.
thevic16 1:17ea74f31633 16 int distance2 = 0; //distancia atras.
thevic16 1:17ea74f31633 17 int distance3 = 0; // distancia izquierda.
thevic16 1:17ea74f31633 18 int distance4 = 0; // distancia derecha.
thevic16 0:b8adbf13199b 19
erodrz 2:4f5a0c64d9cd 20 int distanceLimit = 15;
thevic16 0:b8adbf13199b 21
thevic16 1:17ea74f31633 22 DigitalIn modo1(D8); //Modo manual
thevic16 1:17ea74f31633 23 DigitalIn modo2(D9); //Modo comandos joystick
thevic16 1:17ea74f31633 24 DigitalIn modo3(D10); //Modo comandos de voz
thevic16 1:17ea74f31633 25 DigitalIn modo4(D11); //Modo rutas autonomas
thevic16 1:17ea74f31633 26
thevic16 1:17ea74f31633 27 Serial pc(USBTX, USBRX); //Para recibir los comandos de voz.
thevic16 0:b8adbf13199b 28
erodrz 2:4f5a0c64d9cd 29 void flushSerialBuffer(void)
erodrz 2:4f5a0c64d9cd 30 {
erodrz 2:4f5a0c64d9cd 31 char char1 = 0;
erodrz 2:4f5a0c64d9cd 32 while (pc.readable())
erodrz 2:4f5a0c64d9cd 33 {
erodrz 2:4f5a0c64d9cd 34 char1 = pc.getc();
erodrz 2:4f5a0c64d9cd 35 }
erodrz 2:4f5a0c64d9cd 36 return;
erodrz 2:4f5a0c64d9cd 37 }
erodrz 2:4f5a0c64d9cd 38
thevic16 0:b8adbf13199b 39 void thread1_HCSR04()
thevic16 0:b8adbf13199b 40 {
thevic16 0:b8adbf13199b 41 DigitalOut trigger(D0);
thevic16 0:b8adbf13199b 42 DigitalIn echo(D1);
thevic16 0:b8adbf13199b 43 Timer sonar;
thevic16 1:17ea74f31633 44 int correction = 0;
thevic16 1:17ea74f31633 45 sonar.reset();
thevic16 1:17ea74f31633 46 // measure actual software polling timer delays
thevic16 1:17ea74f31633 47 // delay used later in time correction
thevic16 1:17ea74f31633 48 // start timer
thevic16 1:17ea74f31633 49 sonar.start();
thevic16 1:17ea74f31633 50 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 51 while (echo==2)
thevic16 1:17ea74f31633 52 {
thevic16 1:17ea74f31633 53
thevic16 1:17ea74f31633 54 };
thevic16 1:17ea74f31633 55 // stop timer
thevic16 1:17ea74f31633 56 sonar.stop();
thevic16 1:17ea74f31633 57 // read timer
thevic16 1:17ea74f31633 58 correction = sonar.read_us();
thevic16 1:17ea74f31633 59 printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 1:17ea74f31633 60 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 61 while(1)
thevic16 1:17ea74f31633 62 {
thevic16 1:17ea74f31633 63 //trigger sonar to send a ping
thevic16 1:17ea74f31633 64 trigger = 1;
thevic16 1:17ea74f31633 65 sonar.reset();
thevic16 1:17ea74f31633 66 wait_us(10.0);
thevic16 1:17ea74f31633 67 trigger = 0;
thevic16 1:17ea74f31633 68 //wait for echo high
thevic16 1:17ea74f31633 69 while (echo==0)
thevic16 1:17ea74f31633 70 {
thevic16 1:17ea74f31633 71
thevic16 1:17ea74f31633 72 };
thevic16 1:17ea74f31633 73 //echo high, so start timer
thevic16 1:17ea74f31633 74 sonar.start();
thevic16 1:17ea74f31633 75 //wait for echo low
thevic16 1:17ea74f31633 76 while (echo==1) {};
thevic16 1:17ea74f31633 77 //stop timer and read value
thevic16 1:17ea74f31633 78 sonar.stop();
thevic16 1:17ea74f31633 79 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 80 distance1 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 81 //printf("Sensor proximidad 1: %d cm \n\r",distance1);
thevic16 1:17ea74f31633 82 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 83 thread_sleep_for(1000);
thevic16 1:17ea74f31633 84 }
thevic16 1:17ea74f31633 85 }
thevic16 0:b8adbf13199b 86
thevic16 1:17ea74f31633 87 void thread2_HCSR04()
thevic16 1:17ea74f31633 88 {
thevic16 1:17ea74f31633 89 DigitalOut trigger(D2);
thevic16 1:17ea74f31633 90 DigitalIn echo(D3);
thevic16 1:17ea74f31633 91 Timer sonar;
thevic16 0:b8adbf13199b 92 int correction = 0;
thevic16 0:b8adbf13199b 93 sonar.reset();
thevic16 0:b8adbf13199b 94 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 95 // delay used later in time correction
thevic16 0:b8adbf13199b 96 // start timer
thevic16 0:b8adbf13199b 97 sonar.start();
thevic16 0:b8adbf13199b 98 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 99 while (echo==2)
thevic16 1:17ea74f31633 100 {
thevic16 1:17ea74f31633 101
thevic16 1:17ea74f31633 102 };
thevic16 0:b8adbf13199b 103 // stop timer
thevic16 0:b8adbf13199b 104 sonar.stop();
thevic16 0:b8adbf13199b 105 // read timer
thevic16 0:b8adbf13199b 106 correction = sonar.read_us();
thevic16 1:17ea74f31633 107 printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 108 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 109 while(1)
thevic16 1:17ea74f31633 110 {
thevic16 1:17ea74f31633 111 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 112 trigger = 1;
thevic16 0:b8adbf13199b 113 sonar.reset();
thevic16 0:b8adbf13199b 114 wait_us(10.0);
thevic16 0:b8adbf13199b 115 trigger = 0;
thevic16 1:17ea74f31633 116 //wait for echo high
thevic16 1:17ea74f31633 117 while (echo==0)
thevic16 1:17ea74f31633 118 {
thevic16 1:17ea74f31633 119
thevic16 1:17ea74f31633 120 };
thevic16 1:17ea74f31633 121 //echo high, so start timer
thevic16 0:b8adbf13199b 122 sonar.start();
thevic16 1:17ea74f31633 123 //wait for echo low
thevic16 1:17ea74f31633 124 while (echo==1)
thevic16 1:17ea74f31633 125 {
thevic16 1:17ea74f31633 126
thevic16 1:17ea74f31633 127 };
thevic16 1:17ea74f31633 128 //stop timer and read value
thevic16 0:b8adbf13199b 129 sonar.stop();
thevic16 1:17ea74f31633 130 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 131 distance2 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 132 //printf("Sensor proximidad 2: %d cm \n\r",distance2);
thevic16 1:17ea74f31633 133 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 134 thread_sleep_for(1000);
thevic16 1:17ea74f31633 135 }
thevic16 0:b8adbf13199b 136 }
thevic16 0:b8adbf13199b 137
thevic16 1:17ea74f31633 138 void thread3_HCSR04()
thevic16 0:b8adbf13199b 139 {
thevic16 1:17ea74f31633 140 DigitalOut trigger(D4);
thevic16 1:17ea74f31633 141 DigitalIn echo(D5);
thevic16 0:b8adbf13199b 142 Timer sonar;
thevic16 0:b8adbf13199b 143 int correction = 0;
thevic16 0:b8adbf13199b 144 sonar.reset();
thevic16 0:b8adbf13199b 145 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 146 // delay used later in time correction
thevic16 0:b8adbf13199b 147 // start timer
thevic16 0:b8adbf13199b 148 sonar.start();
thevic16 0:b8adbf13199b 149 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 150 while (echo==2)
thevic16 1:17ea74f31633 151 {
thevic16 1:17ea74f31633 152
thevic16 1:17ea74f31633 153 };
thevic16 0:b8adbf13199b 154 // stop timer
thevic16 0:b8adbf13199b 155 sonar.stop();
thevic16 0:b8adbf13199b 156 // read timer
thevic16 0:b8adbf13199b 157 correction = sonar.read_us();
thevic16 1:17ea74f31633 158 printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 159 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 160 while(1)
thevic16 1:17ea74f31633 161 {
thevic16 1:17ea74f31633 162 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 163 trigger = 1;
thevic16 0:b8adbf13199b 164 sonar.reset();
thevic16 0:b8adbf13199b 165 wait_us(10.0);
thevic16 0:b8adbf13199b 166 trigger = 0;
thevic16 1:17ea74f31633 167 //wait for echo high
thevic16 1:17ea74f31633 168 while (echo==0)
thevic16 1:17ea74f31633 169 {
thevic16 1:17ea74f31633 170
thevic16 1:17ea74f31633 171 };
thevic16 1:17ea74f31633 172 //echo high, so start timer
thevic16 0:b8adbf13199b 173 sonar.start();
thevic16 1:17ea74f31633 174 //wait for echo low
thevic16 1:17ea74f31633 175 while (echo==1)
thevic16 1:17ea74f31633 176 {
thevic16 1:17ea74f31633 177
thevic16 1:17ea74f31633 178 };
thevic16 1:17ea74f31633 179 //stop timer and read value
thevic16 0:b8adbf13199b 180 sonar.stop();
thevic16 1:17ea74f31633 181 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 182 distance3 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 183 //printf("Sensor proximidad 3: %d cm \n\r",distance3);
thevic16 1:17ea74f31633 184 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 185 thread_sleep_for(1000);
thevic16 1:17ea74f31633 186 }
thevic16 0:b8adbf13199b 187 }
thevic16 1:17ea74f31633 188
thevic16 1:17ea74f31633 189 void thread4_HCSR04()
thevic16 0:b8adbf13199b 190 {
thevic16 1:17ea74f31633 191 DigitalOut trigger(D6);
thevic16 1:17ea74f31633 192 DigitalIn echo(D7);
thevic16 0:b8adbf13199b 193 Timer sonar;
thevic16 0:b8adbf13199b 194 int correction = 0;
thevic16 0:b8adbf13199b 195 sonar.reset();
thevic16 0:b8adbf13199b 196 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 197 // delay used later in time correction
thevic16 0:b8adbf13199b 198 // start timer
thevic16 0:b8adbf13199b 199 sonar.start();
thevic16 0:b8adbf13199b 200 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 201 while (echo==2)
thevic16 1:17ea74f31633 202 {
thevic16 0:b8adbf13199b 203
thevic16 1:17ea74f31633 204 };
thevic16 0:b8adbf13199b 205 // stop timer
thevic16 0:b8adbf13199b 206 sonar.stop();
thevic16 0:b8adbf13199b 207 // read timer
thevic16 0:b8adbf13199b 208 correction = sonar.read_us();
thevic16 0:b8adbf13199b 209 printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 210 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 211 while(1)
thevic16 1:17ea74f31633 212 {
thevic16 1:17ea74f31633 213 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 214 trigger = 1;
thevic16 0:b8adbf13199b 215 sonar.reset();
thevic16 0:b8adbf13199b 216 wait_us(10.0);
thevic16 0:b8adbf13199b 217 trigger = 0;
thevic16 1:17ea74f31633 218 //wait for echo high
thevic16 1:17ea74f31633 219 while (echo==0)
thevic16 1:17ea74f31633 220 {
thevic16 1:17ea74f31633 221
thevic16 1:17ea74f31633 222 };
thevic16 1:17ea74f31633 223 //echo high, so start timer
thevic16 0:b8adbf13199b 224 sonar.start();
thevic16 1:17ea74f31633 225 //wait for echo low
thevic16 1:17ea74f31633 226 while (echo==1)
thevic16 1:17ea74f31633 227 {
thevic16 1:17ea74f31633 228
thevic16 1:17ea74f31633 229 };
thevic16 1:17ea74f31633 230 //stop timer and read value
thevic16 0:b8adbf13199b 231 sonar.stop();
thevic16 1:17ea74f31633 232 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 233 distance4 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 234 //printf("Sensor proximidad 4: %d cm \n\r",distance4);
thevic16 1:17ea74f31633 235 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 236 thread_sleep_for(1000);
thevic16 1:17ea74f31633 237 }
thevic16 0:b8adbf13199b 238 }
thevic16 0:b8adbf13199b 239
thevic16 1:17ea74f31633 240 void thread5_Joystick()
thevic16 1:17ea74f31633 241 {
thevic16 1:17ea74f31633 242 SparkfunAnalogJoystick JoyStick(A1, A0, D12);
thevic16 0:b8adbf13199b 243 float X;
thevic16 0:b8adbf13199b 244 float Y;
thevic16 1:17ea74f31633 245 while(1)
thevic16 1:17ea74f31633 246 {
thevic16 1:17ea74f31633 247 if(!modo1 && modo2 && !modo3 && !modo4)
thevic16 1:17ea74f31633 248 {
thevic16 1:17ea74f31633 249 X = JoyStick.xAxis();
thevic16 1:17ea74f31633 250 Y = JoyStick.yAxis();
erodrz 2:4f5a0c64d9cd 251 /*
thevic16 1:17ea74f31633 252 printf("X-Axis: %f\n\r", X);
thevic16 1:17ea74f31633 253 printf("Y-Axis: %f\n\r", Y);
thevic16 1:17ea74f31633 254 printf(" \n\r");
erodrz 2:4f5a0c64d9cd 255 */
thevic16 1:17ea74f31633 256 if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f )
thevic16 1:17ea74f31633 257 {
thevic16 1:17ea74f31633 258 if(distance1 > distanceLimit)
thevic16 1:17ea74f31633 259 {
thevic16 1:17ea74f31633 260 printf("Comandos joystick: Hacia adelante \r \n");
thevic16 1:17ea74f31633 261 }
thevic16 1:17ea74f31633 262 else
thevic16 1:17ea74f31633 263 {
thevic16 1:17ea74f31633 264 printf("Comandos joystick: Obstaculo adelante \r \n");
thevic16 1:17ea74f31633 265 }
erodrz 2:4f5a0c64d9cd 266 thread_sleep_for(500);
thevic16 1:17ea74f31633 267 }
thevic16 1:17ea74f31633 268 if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f)
thevic16 1:17ea74f31633 269 {
thevic16 1:17ea74f31633 270 if(distance2 > distanceLimit)
thevic16 1:17ea74f31633 271 {
thevic16 1:17ea74f31633 272 printf("Comandos joystick: Hacia atras \r \n");
thevic16 1:17ea74f31633 273 }
thevic16 1:17ea74f31633 274 else
thevic16 1:17ea74f31633 275 {
thevic16 1:17ea74f31633 276 printf("Comandos joystick: Obstaculo atras \r \n");
thevic16 1:17ea74f31633 277 }
erodrz 2:4f5a0c64d9cd 278 thread_sleep_for(500);
thevic16 1:17ea74f31633 279 }
thevic16 1:17ea74f31633 280 if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f)
thevic16 1:17ea74f31633 281 {
thevic16 1:17ea74f31633 282 if(distance3 > distanceLimit)
thevic16 1:17ea74f31633 283 {
thevic16 1:17ea74f31633 284 printf("Comandos joystick: Hacia izquierda \r \n");
thevic16 1:17ea74f31633 285 }
thevic16 1:17ea74f31633 286 else
thevic16 1:17ea74f31633 287 {
thevic16 1:17ea74f31633 288 printf("Comandos joystick: Obstaculo izquierda \r \n");
thevic16 1:17ea74f31633 289 }
erodrz 2:4f5a0c64d9cd 290 thread_sleep_for(500);
thevic16 1:17ea74f31633 291 }
thevic16 1:17ea74f31633 292 if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f)
thevic16 1:17ea74f31633 293 {
thevic16 1:17ea74f31633 294 if(distance4 > distanceLimit)
thevic16 1:17ea74f31633 295 {
thevic16 1:17ea74f31633 296 printf("Comandos joystick: Hacia derecha \r \n");
thevic16 1:17ea74f31633 297 }
thevic16 1:17ea74f31633 298 else
thevic16 1:17ea74f31633 299 {
thevic16 1:17ea74f31633 300 printf("Comandos joystick: Obstaculo derecha \r \n");
thevic16 1:17ea74f31633 301 }
erodrz 2:4f5a0c64d9cd 302 thread_sleep_for(500);
thevic16 1:17ea74f31633 303 }
thevic16 1:17ea74f31633 304 thread_sleep_for(5);
thevic16 1:17ea74f31633 305 }
thevic16 1:17ea74f31633 306 }
thevic16 1:17ea74f31633 307 }
thevic16 1:17ea74f31633 308
thevic16 1:17ea74f31633 309 void thread6_ComandosVoz()
thevic16 1:17ea74f31633 310 {
thevic16 0:b8adbf13199b 311 while(1)
thevic16 0:b8adbf13199b 312 {
thevic16 1:17ea74f31633 313 if(!modo1 && !modo2 && modo3 && !modo4)
thevic16 1:17ea74f31633 314 {
erodrz 2:4f5a0c64d9cd 315 flushSerialBuffer();
erodrz 2:4f5a0c64d9cd 316 char c = pc.getc();
thevic16 1:17ea74f31633 317 if(c == 'w')
thevic16 1:17ea74f31633 318 {
erodrz 2:4f5a0c64d9cd 319 //printf("Distance1 - %d \r \n",distance1);
erodrz 2:4f5a0c64d9cd 320 if(distance1 > distanceLimit)
thevic16 1:17ea74f31633 321 {
thevic16 1:17ea74f31633 322 pc.printf(" Comandos de voz: Hacia adelante \r \n");
thevic16 1:17ea74f31633 323 }
thevic16 1:17ea74f31633 324 else
thevic16 1:17ea74f31633 325 {
thevic16 1:17ea74f31633 326 printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
erodrz 2:4f5a0c64d9cd 327 }
erodrz 2:4f5a0c64d9cd 328 thread_sleep_for(1000);
erodrz 2:4f5a0c64d9cd 329 }
erodrz 2:4f5a0c64d9cd 330 if(c == 's')
thevic16 0:b8adbf13199b 331 {
erodrz 2:4f5a0c64d9cd 332 //printf("Distance2 - %d \r \n",distance2);
thevic16 1:17ea74f31633 333 if(distance2 > distanceLimit)
thevic16 1:17ea74f31633 334 {
erodrz 2:4f5a0c64d9cd 335 pc.printf(" Comandos de voz: Hacia atras \r \n");
thevic16 1:17ea74f31633 336 }
thevic16 1:17ea74f31633 337 else
thevic16 1:17ea74f31633 338 {
thevic16 1:17ea74f31633 339 printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
thevic16 1:17ea74f31633 340 }
erodrz 2:4f5a0c64d9cd 341 thread_sleep_for(1000);
thevic16 1:17ea74f31633 342 }
erodrz 2:4f5a0c64d9cd 343 if(c == 'a')
thevic16 1:17ea74f31633 344 {
erodrz 2:4f5a0c64d9cd 345 //printf("Distance3 - %d \r \n",distance3);
thevic16 1:17ea74f31633 346 if(distance3 > distanceLimit)
thevic16 1:17ea74f31633 347 {
erodrz 2:4f5a0c64d9cd 348 pc.printf(" Comandos de voz: Hacia la izquierda \r \n");
thevic16 1:17ea74f31633 349 }
thevic16 1:17ea74f31633 350 else
thevic16 1:17ea74f31633 351 {
thevic16 1:17ea74f31633 352 printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
thevic16 1:17ea74f31633 353 }
erodrz 2:4f5a0c64d9cd 354 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 355 }
erodrz 2:4f5a0c64d9cd 356 if(c == 'd')
thevic16 1:17ea74f31633 357 {
erodrz 2:4f5a0c64d9cd 358 //printf("Distance4 - %d \r \n",distance4);
thevic16 1:17ea74f31633 359 if(distance4 > distanceLimit)
thevic16 1:17ea74f31633 360 {
erodrz 2:4f5a0c64d9cd 361 pc.printf(" Comandos de voz: Hacia la derecha \r \n");
thevic16 1:17ea74f31633 362 }
thevic16 1:17ea74f31633 363 else
thevic16 1:17ea74f31633 364 {
thevic16 1:17ea74f31633 365 printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
erodrz 2:4f5a0c64d9cd 366 }
erodrz 2:4f5a0c64d9cd 367 thread_sleep_for(1000);
erodrz 2:4f5a0c64d9cd 368 }
erodrz 2:4f5a0c64d9cd 369 c = ' ';
erodrz 2:4f5a0c64d9cd 370 thread_sleep_for(5);
erodrz 2:4f5a0c64d9cd 371 }
thevic16 1:17ea74f31633 372 }
thevic16 1:17ea74f31633 373 }
thevic16 1:17ea74f31633 374
thevic16 1:17ea74f31633 375 void thread7_IndicarModo()
thevic16 1:17ea74f31633 376 {
thevic16 1:17ea74f31633 377 bool estadomodo1 = false;
thevic16 1:17ea74f31633 378 bool estadomodo2 = false;
thevic16 1:17ea74f31633 379 bool estadomodo3 = false;
thevic16 1:17ea74f31633 380 bool estadomodo4 = false;
thevic16 1:17ea74f31633 381 while (true)
thevic16 1:17ea74f31633 382 {
thevic16 1:17ea74f31633 383 if(modo1 && !modo2 && !modo3 && !modo4 && !estadomodo1)
thevic16 1:17ea74f31633 384 {
thevic16 1:17ea74f31633 385 printf("Modo manual \r \n");
thevic16 1:17ea74f31633 386 estadomodo1 = true;
thevic16 1:17ea74f31633 387 estadomodo2 = false;
thevic16 1:17ea74f31633 388 estadomodo3 = false;
thevic16 1:17ea74f31633 389 estadomodo4 = false;
thevic16 0:b8adbf13199b 390 }
thevic16 1:17ea74f31633 391 if(!modo1 && modo2 && !modo3 && !modo4 && !estadomodo2)
thevic16 1:17ea74f31633 392 {
thevic16 1:17ea74f31633 393 printf("Modo comandos joystick \r \n");
thevic16 1:17ea74f31633 394 estadomodo1 = false;
thevic16 1:17ea74f31633 395 estadomodo2 = true;
thevic16 1:17ea74f31633 396 estadomodo3 = false;
thevic16 1:17ea74f31633 397 estadomodo4 = false;
thevic16 1:17ea74f31633 398 }
thevic16 1:17ea74f31633 399 if(!modo1 && !modo2 && modo3 && !modo4 && !estadomodo3)
thevic16 1:17ea74f31633 400 {
thevic16 1:17ea74f31633 401 printf("Modo comandos de voz \r \n");
thevic16 1:17ea74f31633 402 estadomodo1 = false;
thevic16 1:17ea74f31633 403 estadomodo2 = false;
thevic16 1:17ea74f31633 404 estadomodo3 = true;
thevic16 1:17ea74f31633 405 estadomodo4 = false;
thevic16 1:17ea74f31633 406 }
thevic16 1:17ea74f31633 407 if(!modo1 && !modo2 && !modo3 && modo4 && !estadomodo4)
thevic16 1:17ea74f31633 408 {
thevic16 1:17ea74f31633 409 printf("Modo rutas autonomas \r \n");
thevic16 1:17ea74f31633 410 estadomodo1 = false;
thevic16 1:17ea74f31633 411 estadomodo2 = false;
thevic16 1:17ea74f31633 412 estadomodo3 = false;
thevic16 1:17ea74f31633 413 estadomodo4 = true;
thevic16 1:17ea74f31633 414 }
thevic16 0:b8adbf13199b 415 }
thevic16 1:17ea74f31633 416 }
thevic16 0:b8adbf13199b 417
thevic16 0:b8adbf13199b 418 int main()
thevic16 0:b8adbf13199b 419 {
thevic16 0:b8adbf13199b 420 thread1.start(thread1_HCSR04);
erodrz 2:4f5a0c64d9cd 421 thread_sleep_for(200);
thevic16 0:b8adbf13199b 422 thread2.start(thread2_HCSR04);
erodrz 2:4f5a0c64d9cd 423 thread_sleep_for(200);
thevic16 0:b8adbf13199b 424 thread3.start(thread3_HCSR04);
erodrz 2:4f5a0c64d9cd 425 thread_sleep_for(200);
thevic16 0:b8adbf13199b 426 thread4.start(thread4_HCSR04);
erodrz 2:4f5a0c64d9cd 427 thread_sleep_for(200);
thevic16 0:b8adbf13199b 428 thread5.start(thread5_Joystick);
erodrz 2:4f5a0c64d9cd 429 thread_sleep_for(200);
thevic16 1:17ea74f31633 430 thread6.start(thread6_ComandosVoz);
erodrz 2:4f5a0c64d9cd 431 thread_sleep_for(200);
thevic16 1:17ea74f31633 432 thread7.start(thread7_IndicarModo);
erodrz 2:4f5a0c64d9cd 433 thread_sleep_for(200);
thevic16 0:b8adbf13199b 434 }