Wheelchair Logic v3 Project
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?
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 | |
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 | } |