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

Wheelchair Logic v3 Project (New)

Dependencies:   SparkfunAnalogJoystick

main.cpp@3:3d54fd4109c0, 2021-07-21 (annotated)

Committer:
thevic16
Date:
Wed Jul 21 19:16:11 2021 +0000
Revision:
3:3d54fd4109c0
Parent:
2:4f5a0c64d9cd
Child:
4:60e3365da280
Logica del proyecto

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
thevic16 3:3d54fd4109c0 29
thevic16 3:3d54fd4109c0 30 //motor driver
thevic16 3:3d54fd4109c0 31 DigitalOut direccion1(PC_6);
thevic16 3:3d54fd4109c0 32 PwmOut PWM_velocidad1(PB_15);
thevic16 3:3d54fd4109c0 33
thevic16 3:3d54fd4109c0 34 DigitalOut direccion2(D14);
thevic16 3:3d54fd4109c0 35 PwmOut PWM_velocidad2(D15);
thevic16 3:3d54fd4109c0 36
thevic16 3:3d54fd4109c0 37
erodrz 2:4f5a0c64d9cd 38 void flushSerialBuffer(void)
erodrz 2:4f5a0c64d9cd 39 {
erodrz 2:4f5a0c64d9cd 40 char char1 = 0;
erodrz 2:4f5a0c64d9cd 41 while (pc.readable())
erodrz 2:4f5a0c64d9cd 42 {
erodrz 2:4f5a0c64d9cd 43 char1 = pc.getc();
erodrz 2:4f5a0c64d9cd 44 }
erodrz 2:4f5a0c64d9cd 45 return;
erodrz 2:4f5a0c64d9cd 46 }
erodrz 2:4f5a0c64d9cd 47
thevic16 3:3d54fd4109c0 48
thevic16 3:3d54fd4109c0 49 void mover_hacia_adelante(int tiempo){
thevic16 3:3d54fd4109c0 50 direccion1 = 0; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 51 direccion2 = 0; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 52
thevic16 3:3d54fd4109c0 53
thevic16 3:3d54fd4109c0 54 PWM_velocidad1.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 55 PWM_velocidad1.write(0.15f); //%25
thevic16 3:3d54fd4109c0 56
thevic16 3:3d54fd4109c0 57 PWM_velocidad2.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 58 PWM_velocidad2.write(0.15f); //25%
thevic16 3:3d54fd4109c0 59
thevic16 3:3d54fd4109c0 60
thevic16 3:3d54fd4109c0 61 thread_sleep_for(tiempo);
thevic16 3:3d54fd4109c0 62
thevic16 3:3d54fd4109c0 63 PWM_velocidad1.write(0.0f);
thevic16 3:3d54fd4109c0 64 PWM_velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 65 }
thevic16 3:3d54fd4109c0 66
thevic16 3:3d54fd4109c0 67 void mover_hacia_atras(int tiempo){
thevic16 3:3d54fd4109c0 68 direccion1 = 1; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 69 direccion2 = 1; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 70
thevic16 3:3d54fd4109c0 71
thevic16 3:3d54fd4109c0 72 PWM_velocidad1.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 73 PWM_velocidad1.write(0.15f); //%25
thevic16 3:3d54fd4109c0 74
thevic16 3:3d54fd4109c0 75 PWM_velocidad2.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 76 PWM_velocidad2.write(0.15f); //25%
thevic16 3:3d54fd4109c0 77
thevic16 3:3d54fd4109c0 78
thevic16 3:3d54fd4109c0 79 thread_sleep_for(tiempo);
thevic16 3:3d54fd4109c0 80
thevic16 3:3d54fd4109c0 81 PWM_velocidad1.write(0.0f);
thevic16 3:3d54fd4109c0 82 PWM_velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 83 }
thevic16 3:3d54fd4109c0 84
thevic16 3:3d54fd4109c0 85
thevic16 3:3d54fd4109c0 86 void mover_hacia_izquierda(int tiempo){
thevic16 3:3d54fd4109c0 87 direccion1 = 1; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 88 direccion2 = 0; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 89
thevic16 3:3d54fd4109c0 90
thevic16 3:3d54fd4109c0 91 PWM_velocidad1.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 92 PWM_velocidad1.write(0.15f); //%25
thevic16 3:3d54fd4109c0 93
thevic16 3:3d54fd4109c0 94 PWM_velocidad2.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 95 PWM_velocidad2.write(0.15f); //25%
thevic16 3:3d54fd4109c0 96
thevic16 3:3d54fd4109c0 97
thevic16 3:3d54fd4109c0 98 thread_sleep_for(tiempo);
thevic16 3:3d54fd4109c0 99
thevic16 3:3d54fd4109c0 100 PWM_velocidad1.write(0.0f);
thevic16 3:3d54fd4109c0 101 PWM_velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 102 }
thevic16 3:3d54fd4109c0 103
thevic16 3:3d54fd4109c0 104
thevic16 3:3d54fd4109c0 105 void mover_hacia_derecha(int tiempo){
thevic16 3:3d54fd4109c0 106 direccion1 = 0; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 107 direccion2 = 1; // En direccion de las manecillas del reloj.
thevic16 3:3d54fd4109c0 108
thevic16 3:3d54fd4109c0 109
thevic16 3:3d54fd4109c0 110 PWM_velocidad1.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 111 PWM_velocidad1.write(0.15f); //%25
thevic16 3:3d54fd4109c0 112
thevic16 3:3d54fd4109c0 113 PWM_velocidad2.period(0.0001f); //Declaramos el periodo
thevic16 3:3d54fd4109c0 114 PWM_velocidad2.write(0.15f); //25%
thevic16 3:3d54fd4109c0 115
thevic16 3:3d54fd4109c0 116
thevic16 3:3d54fd4109c0 117 thread_sleep_for(tiempo);
thevic16 3:3d54fd4109c0 118
thevic16 3:3d54fd4109c0 119 PWM_velocidad1.write(0.0f);
thevic16 3:3d54fd4109c0 120 PWM_velocidad2.write(0.0f);
thevic16 3:3d54fd4109c0 121 }
thevic16 3:3d54fd4109c0 122
thevic16 3:3d54fd4109c0 123
thevic16 3:3d54fd4109c0 124
thevic16 0:b8adbf13199b 125 void thread1_HCSR04()
thevic16 0:b8adbf13199b 126 {
thevic16 0:b8adbf13199b 127 DigitalOut trigger(D0);
thevic16 0:b8adbf13199b 128 DigitalIn echo(D1);
thevic16 0:b8adbf13199b 129 Timer sonar;
thevic16 1:17ea74f31633 130 int correction = 0;
thevic16 1:17ea74f31633 131 sonar.reset();
thevic16 1:17ea74f31633 132 // measure actual software polling timer delays
thevic16 1:17ea74f31633 133 // delay used later in time correction
thevic16 1:17ea74f31633 134 // start timer
thevic16 1:17ea74f31633 135 sonar.start();
thevic16 1:17ea74f31633 136 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 137 while (echo==2)
thevic16 1:17ea74f31633 138 {
thevic16 1:17ea74f31633 139
thevic16 1:17ea74f31633 140 };
thevic16 1:17ea74f31633 141 // stop timer
thevic16 1:17ea74f31633 142 sonar.stop();
thevic16 1:17ea74f31633 143 // read timer
thevic16 1:17ea74f31633 144 correction = sonar.read_us();
thevic16 1:17ea74f31633 145 printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 1:17ea74f31633 146 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 147 while(1)
thevic16 1:17ea74f31633 148 {
thevic16 1:17ea74f31633 149 //trigger sonar to send a ping
thevic16 1:17ea74f31633 150 trigger = 1;
thevic16 1:17ea74f31633 151 sonar.reset();
thevic16 1:17ea74f31633 152 wait_us(10.0);
thevic16 1:17ea74f31633 153 trigger = 0;
thevic16 1:17ea74f31633 154 //wait for echo high
thevic16 1:17ea74f31633 155 while (echo==0)
thevic16 1:17ea74f31633 156 {
thevic16 1:17ea74f31633 157
thevic16 1:17ea74f31633 158 };
thevic16 1:17ea74f31633 159 //echo high, so start timer
thevic16 1:17ea74f31633 160 sonar.start();
thevic16 1:17ea74f31633 161 //wait for echo low
thevic16 1:17ea74f31633 162 while (echo==1) {};
thevic16 1:17ea74f31633 163 //stop timer and read value
thevic16 1:17ea74f31633 164 sonar.stop();
thevic16 1:17ea74f31633 165 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 166 distance1 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 167 //printf("Sensor proximidad 1: %d cm \n\r",distance1);
thevic16 1:17ea74f31633 168 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 169 thread_sleep_for(1000);
thevic16 1:17ea74f31633 170 }
thevic16 1:17ea74f31633 171 }
thevic16 0:b8adbf13199b 172
thevic16 1:17ea74f31633 173 void thread2_HCSR04()
thevic16 1:17ea74f31633 174 {
thevic16 1:17ea74f31633 175 DigitalOut trigger(D2);
thevic16 1:17ea74f31633 176 DigitalIn echo(D3);
thevic16 1:17ea74f31633 177 Timer sonar;
thevic16 0:b8adbf13199b 178 int correction = 0;
thevic16 0:b8adbf13199b 179 sonar.reset();
thevic16 0:b8adbf13199b 180 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 181 // delay used later in time correction
thevic16 0:b8adbf13199b 182 // start timer
thevic16 0:b8adbf13199b 183 sonar.start();
thevic16 0:b8adbf13199b 184 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 185 while (echo==2)
thevic16 1:17ea74f31633 186 {
thevic16 1:17ea74f31633 187
thevic16 1:17ea74f31633 188 };
thevic16 0:b8adbf13199b 189 // stop timer
thevic16 0:b8adbf13199b 190 sonar.stop();
thevic16 0:b8adbf13199b 191 // read timer
thevic16 0:b8adbf13199b 192 correction = sonar.read_us();
thevic16 1:17ea74f31633 193 printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 194 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 195 while(1)
thevic16 1:17ea74f31633 196 {
thevic16 1:17ea74f31633 197 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 198 trigger = 1;
thevic16 0:b8adbf13199b 199 sonar.reset();
thevic16 0:b8adbf13199b 200 wait_us(10.0);
thevic16 0:b8adbf13199b 201 trigger = 0;
thevic16 1:17ea74f31633 202 //wait for echo high
thevic16 1:17ea74f31633 203 while (echo==0)
thevic16 1:17ea74f31633 204 {
thevic16 1:17ea74f31633 205
thevic16 1:17ea74f31633 206 };
thevic16 1:17ea74f31633 207 //echo high, so start timer
thevic16 0:b8adbf13199b 208 sonar.start();
thevic16 1:17ea74f31633 209 //wait for echo low
thevic16 1:17ea74f31633 210 while (echo==1)
thevic16 1:17ea74f31633 211 {
thevic16 1:17ea74f31633 212
thevic16 1:17ea74f31633 213 };
thevic16 1:17ea74f31633 214 //stop timer and read value
thevic16 0:b8adbf13199b 215 sonar.stop();
thevic16 1:17ea74f31633 216 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 217 distance2 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 218 //printf("Sensor proximidad 2: %d cm \n\r",distance2);
thevic16 1:17ea74f31633 219 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 220 thread_sleep_for(1000);
thevic16 1:17ea74f31633 221 }
thevic16 0:b8adbf13199b 222 }
thevic16 0:b8adbf13199b 223
thevic16 1:17ea74f31633 224 void thread3_HCSR04()
thevic16 0:b8adbf13199b 225 {
thevic16 1:17ea74f31633 226 DigitalOut trigger(D4);
thevic16 1:17ea74f31633 227 DigitalIn echo(D5);
thevic16 0:b8adbf13199b 228 Timer sonar;
thevic16 0:b8adbf13199b 229 int correction = 0;
thevic16 0:b8adbf13199b 230 sonar.reset();
thevic16 0:b8adbf13199b 231 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 232 // delay used later in time correction
thevic16 0:b8adbf13199b 233 // start timer
thevic16 0:b8adbf13199b 234 sonar.start();
thevic16 0:b8adbf13199b 235 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 236 while (echo==2)
thevic16 1:17ea74f31633 237 {
thevic16 1:17ea74f31633 238
thevic16 1:17ea74f31633 239 };
thevic16 0:b8adbf13199b 240 // stop timer
thevic16 0:b8adbf13199b 241 sonar.stop();
thevic16 0:b8adbf13199b 242 // read timer
thevic16 0:b8adbf13199b 243 correction = sonar.read_us();
thevic16 1:17ea74f31633 244 printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 245 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 246 while(1)
thevic16 1:17ea74f31633 247 {
thevic16 1:17ea74f31633 248 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 249 trigger = 1;
thevic16 0:b8adbf13199b 250 sonar.reset();
thevic16 0:b8adbf13199b 251 wait_us(10.0);
thevic16 0:b8adbf13199b 252 trigger = 0;
thevic16 1:17ea74f31633 253 //wait for echo high
thevic16 1:17ea74f31633 254 while (echo==0)
thevic16 1:17ea74f31633 255 {
thevic16 1:17ea74f31633 256
thevic16 1:17ea74f31633 257 };
thevic16 1:17ea74f31633 258 //echo high, so start timer
thevic16 0:b8adbf13199b 259 sonar.start();
thevic16 1:17ea74f31633 260 //wait for echo low
thevic16 1:17ea74f31633 261 while (echo==1)
thevic16 1:17ea74f31633 262 {
thevic16 1:17ea74f31633 263
thevic16 1:17ea74f31633 264 };
thevic16 1:17ea74f31633 265 //stop timer and read value
thevic16 0:b8adbf13199b 266 sonar.stop();
thevic16 1:17ea74f31633 267 //subtract software overhead timer delay and scale to cm
thevic16 1:17ea74f31633 268 distance3 = (sonar.read_us()-correction)/58.0;
thevic16 1:17ea74f31633 269 //printf("Sensor proximidad 3: %d cm \n\r",distance3);
thevic16 1:17ea74f31633 270 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 271 thread_sleep_for(1000);
thevic16 1:17ea74f31633 272 }
thevic16 0:b8adbf13199b 273 }
thevic16 1:17ea74f31633 274
thevic16 1:17ea74f31633 275 void thread4_HCSR04()
thevic16 0:b8adbf13199b 276 {
thevic16 1:17ea74f31633 277 DigitalOut trigger(D6);
thevic16 1:17ea74f31633 278 DigitalIn echo(D7);
thevic16 0:b8adbf13199b 279 Timer sonar;
thevic16 0:b8adbf13199b 280 int correction = 0;
thevic16 0:b8adbf13199b 281 sonar.reset();
thevic16 0:b8adbf13199b 282 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 283 // delay used later in time correction
thevic16 0:b8adbf13199b 284 // start timer
thevic16 0:b8adbf13199b 285 sonar.start();
thevic16 0:b8adbf13199b 286 // min software polling delay to read echo pin
thevic16 1:17ea74f31633 287 while (echo==2)
thevic16 1:17ea74f31633 288 {
thevic16 0:b8adbf13199b 289
thevic16 1:17ea74f31633 290 };
thevic16 0:b8adbf13199b 291 // stop timer
thevic16 0:b8adbf13199b 292 sonar.stop();
thevic16 0:b8adbf13199b 293 // read timer
thevic16 0:b8adbf13199b 294 correction = sonar.read_us();
thevic16 0:b8adbf13199b 295 printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 296 //Loop to read Sonar distance values, scale, and print
thevic16 1:17ea74f31633 297 while(1)
thevic16 1:17ea74f31633 298 {
thevic16 1:17ea74f31633 299 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 300 trigger = 1;
thevic16 0:b8adbf13199b 301 sonar.reset();
thevic16 0:b8adbf13199b 302 wait_us(10.0);
thevic16 0:b8adbf13199b 303 trigger = 0;
thevic16 1:17ea74f31633 304 //wait for echo high
thevic16 1:17ea74f31633 305 while (echo==0)
thevic16 1:17ea74f31633 306 {
thevic16 1:17ea74f31633 307
thevic16 1:17ea74f31633 308 };
thevic16 1:17ea74f31633 309 //echo high, so start timer
thevic16 0:b8adbf13199b 310 sonar.start();
thevic16 1:17ea74f31633 311 //wait for echo low
thevic16 1:17ea74f31633 312 while (echo==1)
thevic16 1:17ea74f31633 313 {
thevic16 1:17ea74f31633 314
thevic16 1:17ea74f31633 315 };
thevic16 1:17ea74f31633 316 //stop timer and read value
thevic16 0:b8adbf13199b 317 sonar.stop();
thevic16 1:17ea74f31633 318 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 319 distance4 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 320 //printf("Sensor proximidad 4: %d cm \n\r",distance4);
thevic16 1:17ea74f31633 321 //wait so that any echo(s) return before sending another ping
thevic16 1:17ea74f31633 322 thread_sleep_for(1000);
thevic16 1:17ea74f31633 323 }
thevic16 0:b8adbf13199b 324 }
thevic16 0:b8adbf13199b 325
thevic16 1:17ea74f31633 326 void thread5_Joystick()
thevic16 1:17ea74f31633 327 {
thevic16 1:17ea74f31633 328 SparkfunAnalogJoystick JoyStick(A1, A0, D12);
thevic16 0:b8adbf13199b 329 float X;
thevic16 0:b8adbf13199b 330 float Y;
thevic16 1:17ea74f31633 331 while(1)
thevic16 1:17ea74f31633 332 {
thevic16 1:17ea74f31633 333 if(!modo1 && modo2 && !modo3 && !modo4)
thevic16 1:17ea74f31633 334 {
thevic16 1:17ea74f31633 335 X = JoyStick.xAxis();
thevic16 1:17ea74f31633 336 Y = JoyStick.yAxis();
erodrz 2:4f5a0c64d9cd 337 /*
thevic16 1:17ea74f31633 338 printf("X-Axis: %f\n\r", X);
thevic16 1:17ea74f31633 339 printf("Y-Axis: %f\n\r", Y);
thevic16 1:17ea74f31633 340 printf(" \n\r");
erodrz 2:4f5a0c64d9cd 341 */
thevic16 1:17ea74f31633 342 if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f )
thevic16 1:17ea74f31633 343 {
thevic16 1:17ea74f31633 344 if(distance1 > distanceLimit)
thevic16 1:17ea74f31633 345 {
thevic16 1:17ea74f31633 346 printf("Comandos joystick: Hacia adelante \r \n");
thevic16 3:3d54fd4109c0 347 mover_hacia_adelante(1000); // un segundo
thevic16 1:17ea74f31633 348 }
thevic16 1:17ea74f31633 349 else
thevic16 1:17ea74f31633 350 {
thevic16 1:17ea74f31633 351 printf("Comandos joystick: Obstaculo adelante \r \n");
thevic16 1:17ea74f31633 352 }
erodrz 2:4f5a0c64d9cd 353 thread_sleep_for(500);
thevic16 1:17ea74f31633 354 }
thevic16 1:17ea74f31633 355 if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f)
thevic16 1:17ea74f31633 356 {
thevic16 1:17ea74f31633 357 if(distance2 > distanceLimit)
thevic16 1:17ea74f31633 358 {
thevic16 1:17ea74f31633 359 printf("Comandos joystick: Hacia atras \r \n");
thevic16 3:3d54fd4109c0 360 mover_hacia_atras(1000); // un segundo
thevic16 1:17ea74f31633 361 }
thevic16 1:17ea74f31633 362 else
thevic16 1:17ea74f31633 363 {
thevic16 1:17ea74f31633 364 printf("Comandos joystick: Obstaculo atras \r \n");
thevic16 1:17ea74f31633 365 }
erodrz 2:4f5a0c64d9cd 366 thread_sleep_for(500);
thevic16 1:17ea74f31633 367 }
thevic16 1:17ea74f31633 368 if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f)
thevic16 1:17ea74f31633 369 {
thevic16 1:17ea74f31633 370 if(distance3 > distanceLimit)
thevic16 1:17ea74f31633 371 {
thevic16 1:17ea74f31633 372 printf("Comandos joystick: Hacia izquierda \r \n");
thevic16 3:3d54fd4109c0 373 mover_hacia_izquierda(1000); //un segundo
thevic16 1:17ea74f31633 374 }
thevic16 1:17ea74f31633 375 else
thevic16 1:17ea74f31633 376 {
thevic16 1:17ea74f31633 377 printf("Comandos joystick: Obstaculo izquierda \r \n");
thevic16 1:17ea74f31633 378 }
erodrz 2:4f5a0c64d9cd 379 thread_sleep_for(500);
thevic16 1:17ea74f31633 380 }
thevic16 1:17ea74f31633 381 if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f)
thevic16 1:17ea74f31633 382 {
thevic16 1:17ea74f31633 383 if(distance4 > distanceLimit)
thevic16 1:17ea74f31633 384 {
thevic16 1:17ea74f31633 385 printf("Comandos joystick: Hacia derecha \r \n");
thevic16 3:3d54fd4109c0 386 mover_hacia_derecha(1000); // segundo
thevic16 1:17ea74f31633 387 }
thevic16 1:17ea74f31633 388 else
thevic16 1:17ea74f31633 389 {
thevic16 1:17ea74f31633 390 printf("Comandos joystick: Obstaculo derecha \r \n");
thevic16 1:17ea74f31633 391 }
erodrz 2:4f5a0c64d9cd 392 thread_sleep_for(500);
thevic16 1:17ea74f31633 393 }
thevic16 1:17ea74f31633 394 thread_sleep_for(5);
thevic16 1:17ea74f31633 395 }
thevic16 1:17ea74f31633 396 }
thevic16 1:17ea74f31633 397 }
thevic16 1:17ea74f31633 398
thevic16 1:17ea74f31633 399 void thread6_ComandosVoz()
thevic16 1:17ea74f31633 400 {
thevic16 0:b8adbf13199b 401 while(1)
thevic16 0:b8adbf13199b 402 {
thevic16 1:17ea74f31633 403 if(!modo1 && !modo2 && modo3 && !modo4)
thevic16 1:17ea74f31633 404 {
erodrz 2:4f5a0c64d9cd 405 flushSerialBuffer();
erodrz 2:4f5a0c64d9cd 406 char c = pc.getc();
thevic16 3:3d54fd4109c0 407
thevic16 3:3d54fd4109c0 408 thread_sleep_for(5); //timer necesario para que el compilar se de cuenta el orden correcto de ejecucion
thevic16 3:3d54fd4109c0 409
thevic16 3:3d54fd4109c0 410 int m = modo3.read();
thevic16 3:3d54fd4109c0 411 printf("estado modo 3 (comandos de voz): %d \r \n",m);
thevic16 3:3d54fd4109c0 412
thevic16 3:3d54fd4109c0 413 if(m == 1){
thevic16 3:3d54fd4109c0 414 if(c == 'w')
thevic16 1:17ea74f31633 415 {
thevic16 3:3d54fd4109c0 416 //printf("Distance1 - %d \r \n",distance1);
thevic16 3:3d54fd4109c0 417 if(distance1 > distanceLimit)
thevic16 3:3d54fd4109c0 418 {
thevic16 3:3d54fd4109c0 419 pc.printf(" Comandos de voz: Hacia adelante \r \n");
thevic16 3:3d54fd4109c0 420 mover_hacia_adelante(3000);
thevic16 3:3d54fd4109c0 421 }
thevic16 3:3d54fd4109c0 422 else
thevic16 3:3d54fd4109c0 423 {
thevic16 3:3d54fd4109c0 424 printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
thevic16 3:3d54fd4109c0 425 }
thevic16 3:3d54fd4109c0 426 thread_sleep_for(1000);
thevic16 1:17ea74f31633 427 }
thevic16 3:3d54fd4109c0 428 if(c == 's')
thevic16 1:17ea74f31633 429 {
thevic16 3:3d54fd4109c0 430 //printf("Distance2 - %d \r \n",distance2);
thevic16 3:3d54fd4109c0 431 if(distance2 > distanceLimit)
thevic16 3:3d54fd4109c0 432 {
thevic16 3:3d54fd4109c0 433 pc.printf(" Comandos de voz: Hacia atras \r \n");
thevic16 3:3d54fd4109c0 434 mover_hacia_atras(3000);
thevic16 3:3d54fd4109c0 435 }
thevic16 3:3d54fd4109c0 436 else
thevic16 3:3d54fd4109c0 437 {
thevic16 3:3d54fd4109c0 438 printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
thevic16 3:3d54fd4109c0 439 }
thevic16 3:3d54fd4109c0 440 thread_sleep_for(1000);
thevic16 3:3d54fd4109c0 441 }
thevic16 3:3d54fd4109c0 442 if(c == 'a')
thevic16 1:17ea74f31633 443 {
thevic16 3:3d54fd4109c0 444 //printf("Distance3 - %d \r \n",distance3);
thevic16 3:3d54fd4109c0 445 if(distance3 > distanceLimit)
thevic16 3:3d54fd4109c0 446 {
thevic16 3:3d54fd4109c0 447 pc.printf(" Comandos de voz: Hacia la izquierda \r \n");
thevic16 3:3d54fd4109c0 448 mover_hacia_izquierda(3000);
thevic16 3:3d54fd4109c0 449 }
thevic16 3:3d54fd4109c0 450 else
thevic16 3:3d54fd4109c0 451 {
thevic16 3:3d54fd4109c0 452 printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
thevic16 3:3d54fd4109c0 453 }
thevic16 3:3d54fd4109c0 454 thread_sleep_for(1000);
thevic16 1:17ea74f31633 455 }
thevic16 3:3d54fd4109c0 456 if(c == 'd')
thevic16 3:3d54fd4109c0 457 {
thevic16 3:3d54fd4109c0 458 //printf("Distance4 - %d \r \n",distance4);
thevic16 3:3d54fd4109c0 459 if(distance4 > distanceLimit)
thevic16 3:3d54fd4109c0 460 {
thevic16 3:3d54fd4109c0 461 pc.printf(" Comandos de voz: Hacia la derecha \r \n");
thevic16 3:3d54fd4109c0 462 mover_hacia_derecha(3000);
thevic16 3:3d54fd4109c0 463 }
thevic16 3:3d54fd4109c0 464 else
thevic16 3:3d54fd4109c0 465 {
thevic16 3:3d54fd4109c0 466 printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
thevic16 3:3d54fd4109c0 467 }
thevic16 3:3d54fd4109c0 468 thread_sleep_for(1000);
erodrz 2:4f5a0c64d9cd 469 }
erodrz 2:4f5a0c64d9cd 470 }
erodrz 2:4f5a0c64d9cd 471 c = ' ';
thevic16 3:3d54fd4109c0 472 thread_sleep_for(5);
erodrz 2:4f5a0c64d9cd 473 }
thevic16 1:17ea74f31633 474 }
thevic16 1:17ea74f31633 475 }
thevic16 1:17ea74f31633 476
thevic16 1:17ea74f31633 477 void thread7_IndicarModo()
thevic16 1:17ea74f31633 478 {
thevic16 1:17ea74f31633 479 bool estadomodo1 = false;
thevic16 1:17ea74f31633 480 bool estadomodo2 = false;
thevic16 1:17ea74f31633 481 bool estadomodo3 = false;
thevic16 1:17ea74f31633 482 bool estadomodo4 = false;
thevic16 1:17ea74f31633 483 while (true)
thevic16 1:17ea74f31633 484 {
thevic16 1:17ea74f31633 485 if(modo1 && !modo2 && !modo3 && !modo4 && !estadomodo1)
thevic16 1:17ea74f31633 486 {
thevic16 1:17ea74f31633 487 printf("Modo manual \r \n");
thevic16 1:17ea74f31633 488 estadomodo1 = true;
thevic16 1:17ea74f31633 489 estadomodo2 = false;
thevic16 1:17ea74f31633 490 estadomodo3 = false;
thevic16 1:17ea74f31633 491 estadomodo4 = false;
thevic16 0:b8adbf13199b 492 }
thevic16 1:17ea74f31633 493 if(!modo1 && modo2 && !modo3 && !modo4 && !estadomodo2)
thevic16 1:17ea74f31633 494 {
thevic16 1:17ea74f31633 495 printf("Modo comandos joystick \r \n");
thevic16 1:17ea74f31633 496 estadomodo1 = false;
thevic16 1:17ea74f31633 497 estadomodo2 = true;
thevic16 1:17ea74f31633 498 estadomodo3 = false;
thevic16 1:17ea74f31633 499 estadomodo4 = false;
thevic16 1:17ea74f31633 500 }
thevic16 1:17ea74f31633 501 if(!modo1 && !modo2 && modo3 && !modo4 && !estadomodo3)
thevic16 1:17ea74f31633 502 {
thevic16 1:17ea74f31633 503 printf("Modo comandos de voz \r \n");
thevic16 1:17ea74f31633 504 estadomodo1 = false;
thevic16 1:17ea74f31633 505 estadomodo2 = false;
thevic16 1:17ea74f31633 506 estadomodo3 = true;
thevic16 1:17ea74f31633 507 estadomodo4 = false;
thevic16 1:17ea74f31633 508 }
thevic16 1:17ea74f31633 509 if(!modo1 && !modo2 && !modo3 && modo4 && !estadomodo4)
thevic16 1:17ea74f31633 510 {
thevic16 1:17ea74f31633 511 printf("Modo rutas autonomas \r \n");
thevic16 1:17ea74f31633 512 estadomodo1 = false;
thevic16 1:17ea74f31633 513 estadomodo2 = false;
thevic16 1:17ea74f31633 514 estadomodo3 = false;
thevic16 1:17ea74f31633 515 estadomodo4 = true;
thevic16 1:17ea74f31633 516 }
thevic16 0:b8adbf13199b 517 }
thevic16 1:17ea74f31633 518 }
thevic16 0:b8adbf13199b 519
thevic16 0:b8adbf13199b 520 int main()
thevic16 0:b8adbf13199b 521 {
thevic16 0:b8adbf13199b 522 thread1.start(thread1_HCSR04);
erodrz 2:4f5a0c64d9cd 523 thread_sleep_for(200);
thevic16 0:b8adbf13199b 524 thread2.start(thread2_HCSR04);
erodrz 2:4f5a0c64d9cd 525 thread_sleep_for(200);
thevic16 0:b8adbf13199b 526 thread3.start(thread3_HCSR04);
erodrz 2:4f5a0c64d9cd 527 thread_sleep_for(200);
thevic16 0:b8adbf13199b 528 thread4.start(thread4_HCSR04);
erodrz 2:4f5a0c64d9cd 529 thread_sleep_for(200);
thevic16 0:b8adbf13199b 530 thread5.start(thread5_Joystick);
erodrz 2:4f5a0c64d9cd 531 thread_sleep_for(200);
thevic16 1:17ea74f31633 532 thread6.start(thread6_ComandosVoz);
erodrz 2:4f5a0c64d9cd 533 thread_sleep_for(200);
thevic16 1:17ea74f31633 534 thread7.start(thread7_IndicarModo);
erodrz 2:4f5a0c64d9cd 535 thread_sleep_for(200);
thevic16 0:b8adbf13199b 536 }