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