![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |