victor gomez / Mbed OS 11-logica-proximidad-joystick

yes

Dependencies:   SparkfunAnalogJoystick

main.cpp@0:b8adbf13199b, 2021-06-10 (annotated)

Committer:
thevic16
Date:
Thu Jun 10 20:35:27 2021 +0000
Revision:
0:b8adbf13199b
Child:
1:17ea74f31633
yes

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
thevic16 0:b8adbf13199b 8 Thread thread1;
thevic16 0:b8adbf13199b 9 Thread thread2;
thevic16 0:b8adbf13199b 10 Thread thread3;
thevic16 0:b8adbf13199b 11 Thread thread4;
thevic16 0:b8adbf13199b 12 Thread thread5;
thevic16 0:b8adbf13199b 13
thevic16 0:b8adbf13199b 14 int distance1 = 0;
thevic16 0:b8adbf13199b 15 int distance2 = 0;
thevic16 0:b8adbf13199b 16 int distance3 = 0;
thevic16 0:b8adbf13199b 17 int distance4 = 0;
thevic16 0:b8adbf13199b 18
thevic16 0:b8adbf13199b 19 int distanceLimit = 10;
thevic16 0:b8adbf13199b 20
thevic16 0:b8adbf13199b 21
thevic16 0:b8adbf13199b 22 void thread1_HCSR04()
thevic16 0:b8adbf13199b 23 {
thevic16 0:b8adbf13199b 24 DigitalOut trigger(D0);
thevic16 0:b8adbf13199b 25 DigitalIn echo(D1);
thevic16 0:b8adbf13199b 26 Timer sonar;
thevic16 0:b8adbf13199b 27
thevic16 0:b8adbf13199b 28 int correction = 0;
thevic16 0:b8adbf13199b 29 sonar.reset();
thevic16 0:b8adbf13199b 30 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 31 // delay used later in time correction
thevic16 0:b8adbf13199b 32 // start timer
thevic16 0:b8adbf13199b 33 sonar.start();
thevic16 0:b8adbf13199b 34 // min software polling delay to read echo pin
thevic16 0:b8adbf13199b 35 while (echo==2) {};
thevic16 0:b8adbf13199b 36
thevic16 0:b8adbf13199b 37 // stop timer
thevic16 0:b8adbf13199b 38 sonar.stop();
thevic16 0:b8adbf13199b 39 // read timer
thevic16 0:b8adbf13199b 40 correction = sonar.read_us();
thevic16 0:b8adbf13199b 41 printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 42
thevic16 0:b8adbf13199b 43 //Loop to read Sonar distance values, scale, and print
thevic16 0:b8adbf13199b 44 while(1) {
thevic16 0:b8adbf13199b 45 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 46 trigger = 1;
thevic16 0:b8adbf13199b 47
thevic16 0:b8adbf13199b 48
thevic16 0:b8adbf13199b 49 sonar.reset();
thevic16 0:b8adbf13199b 50 wait_us(10.0);
thevic16 0:b8adbf13199b 51 trigger = 0;
thevic16 0:b8adbf13199b 52
thevic16 0:b8adbf13199b 53 //wait for echo high
thevic16 0:b8adbf13199b 54 while (echo==0) {};
thevic16 0:b8adbf13199b 55
thevic16 0:b8adbf13199b 56 //echo high, so start timer
thevic16 0:b8adbf13199b 57 sonar.start();
thevic16 0:b8adbf13199b 58 //wait for echo low
thevic16 0:b8adbf13199b 59 while (echo==1) {};
thevic16 0:b8adbf13199b 60 //stop timer and read value
thevic16 0:b8adbf13199b 61 sonar.stop();
thevic16 0:b8adbf13199b 62 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 63 distance1 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 64
thevic16 0:b8adbf13199b 65 //printf("Sensor proximidad 1: %d cm \n\r",distance1);
thevic16 0:b8adbf13199b 66 //wait so that any echo(s) return before sending another ping
thevic16 0:b8adbf13199b 67 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 68 }
thevic16 0:b8adbf13199b 69 }
thevic16 0:b8adbf13199b 70
thevic16 0:b8adbf13199b 71 void thread2_HCSR04()
thevic16 0:b8adbf13199b 72 {
thevic16 0:b8adbf13199b 73 DigitalOut trigger(D2);
thevic16 0:b8adbf13199b 74 DigitalIn echo(D3);
thevic16 0:b8adbf13199b 75 Timer sonar;
thevic16 0:b8adbf13199b 76
thevic16 0:b8adbf13199b 77 int correction = 0;
thevic16 0:b8adbf13199b 78 sonar.reset();
thevic16 0:b8adbf13199b 79 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 80 // delay used later in time correction
thevic16 0:b8adbf13199b 81 // start timer
thevic16 0:b8adbf13199b 82 sonar.start();
thevic16 0:b8adbf13199b 83 // min software polling delay to read echo pin
thevic16 0:b8adbf13199b 84 while (echo==2) {};
thevic16 0:b8adbf13199b 85
thevic16 0:b8adbf13199b 86 // stop timer
thevic16 0:b8adbf13199b 87 sonar.stop();
thevic16 0:b8adbf13199b 88 // read timer
thevic16 0:b8adbf13199b 89 correction = sonar.read_us();
thevic16 0:b8adbf13199b 90 printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 91
thevic16 0:b8adbf13199b 92 //Loop to read Sonar distance values, scale, and print
thevic16 0:b8adbf13199b 93 while(1) {
thevic16 0:b8adbf13199b 94 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 95 trigger = 1;
thevic16 0:b8adbf13199b 96
thevic16 0:b8adbf13199b 97
thevic16 0:b8adbf13199b 98 sonar.reset();
thevic16 0:b8adbf13199b 99 wait_us(10.0);
thevic16 0:b8adbf13199b 100 trigger = 0;
thevic16 0:b8adbf13199b 101
thevic16 0:b8adbf13199b 102 //wait for echo high
thevic16 0:b8adbf13199b 103 while (echo==0) {};
thevic16 0:b8adbf13199b 104
thevic16 0:b8adbf13199b 105 //echo high, so start timer
thevic16 0:b8adbf13199b 106 sonar.start();
thevic16 0:b8adbf13199b 107 //wait for echo low
thevic16 0:b8adbf13199b 108 while (echo==1) {};
thevic16 0:b8adbf13199b 109 //stop timer and read value
thevic16 0:b8adbf13199b 110 sonar.stop();
thevic16 0:b8adbf13199b 111 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 112 distance2 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 113
thevic16 0:b8adbf13199b 114 //printf("Sensor proximidad 2: %d cm \n\r",distance2);
thevic16 0:b8adbf13199b 115 //wait so that any echo(s) return before sending another ping
thevic16 0:b8adbf13199b 116 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 117 }
thevic16 0:b8adbf13199b 118 }
thevic16 0:b8adbf13199b 119
thevic16 0:b8adbf13199b 120
thevic16 0:b8adbf13199b 121 void thread3_HCSR04()
thevic16 0:b8adbf13199b 122 {
thevic16 0:b8adbf13199b 123 DigitalOut trigger(D4);
thevic16 0:b8adbf13199b 124 DigitalIn echo(D5);
thevic16 0:b8adbf13199b 125 Timer sonar;
thevic16 0:b8adbf13199b 126
thevic16 0:b8adbf13199b 127 int correction = 0;
thevic16 0:b8adbf13199b 128 sonar.reset();
thevic16 0:b8adbf13199b 129 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 130 // delay used later in time correction
thevic16 0:b8adbf13199b 131 // start timer
thevic16 0:b8adbf13199b 132 sonar.start();
thevic16 0:b8adbf13199b 133 // min software polling delay to read echo pin
thevic16 0:b8adbf13199b 134 while (echo==2) {};
thevic16 0:b8adbf13199b 135
thevic16 0:b8adbf13199b 136 // stop timer
thevic16 0:b8adbf13199b 137 sonar.stop();
thevic16 0:b8adbf13199b 138 // read timer
thevic16 0:b8adbf13199b 139 correction = sonar.read_us();
thevic16 0:b8adbf13199b 140 printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 141
thevic16 0:b8adbf13199b 142 //Loop to read Sonar distance values, scale, and print
thevic16 0:b8adbf13199b 143 while(1) {
thevic16 0:b8adbf13199b 144 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 145 trigger = 1;
thevic16 0:b8adbf13199b 146
thevic16 0:b8adbf13199b 147
thevic16 0:b8adbf13199b 148 sonar.reset();
thevic16 0:b8adbf13199b 149 wait_us(10.0);
thevic16 0:b8adbf13199b 150 trigger = 0;
thevic16 0:b8adbf13199b 151
thevic16 0:b8adbf13199b 152 //wait for echo high
thevic16 0:b8adbf13199b 153 while (echo==0) {};
thevic16 0:b8adbf13199b 154
thevic16 0:b8adbf13199b 155 //echo high, so start timer
thevic16 0:b8adbf13199b 156 sonar.start();
thevic16 0:b8adbf13199b 157 //wait for echo low
thevic16 0:b8adbf13199b 158 while (echo==1) {};
thevic16 0:b8adbf13199b 159 //stop timer and read value
thevic16 0:b8adbf13199b 160 sonar.stop();
thevic16 0:b8adbf13199b 161 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 162 distance3 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 163
thevic16 0:b8adbf13199b 164 //printf("Sensor proximidad 3: %d cm \n\r",distance3);
thevic16 0:b8adbf13199b 165 //wait so that any echo(s) return before sending another ping
thevic16 0:b8adbf13199b 166 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 167 }
thevic16 0:b8adbf13199b 168 }
thevic16 0:b8adbf13199b 169
thevic16 0:b8adbf13199b 170
thevic16 0:b8adbf13199b 171 void thread4_HCSR04()
thevic16 0:b8adbf13199b 172 {
thevic16 0:b8adbf13199b 173 DigitalOut trigger(D6);
thevic16 0:b8adbf13199b 174 DigitalIn echo(D7);
thevic16 0:b8adbf13199b 175 Timer sonar;
thevic16 0:b8adbf13199b 176
thevic16 0:b8adbf13199b 177 int correction = 0;
thevic16 0:b8adbf13199b 178 sonar.reset();
thevic16 0:b8adbf13199b 179 // measure actual software polling timer delays
thevic16 0:b8adbf13199b 180 // delay used later in time correction
thevic16 0:b8adbf13199b 181 // start timer
thevic16 0:b8adbf13199b 182 sonar.start();
thevic16 0:b8adbf13199b 183 // min software polling delay to read echo pin
thevic16 0:b8adbf13199b 184 while (echo==2) {};
thevic16 0:b8adbf13199b 185
thevic16 0:b8adbf13199b 186 // stop timer
thevic16 0:b8adbf13199b 187 sonar.stop();
thevic16 0:b8adbf13199b 188 // read timer
thevic16 0:b8adbf13199b 189 correction = sonar.read_us();
thevic16 0:b8adbf13199b 190 printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction);
thevic16 0:b8adbf13199b 191
thevic16 0:b8adbf13199b 192 //Loop to read Sonar distance values, scale, and print
thevic16 0:b8adbf13199b 193 while(1) {
thevic16 0:b8adbf13199b 194 // trigger sonar to send a ping
thevic16 0:b8adbf13199b 195 trigger = 1;
thevic16 0:b8adbf13199b 196
thevic16 0:b8adbf13199b 197
thevic16 0:b8adbf13199b 198 sonar.reset();
thevic16 0:b8adbf13199b 199 wait_us(10.0);
thevic16 0:b8adbf13199b 200 trigger = 0;
thevic16 0:b8adbf13199b 201
thevic16 0:b8adbf13199b 202 //wait for echo high
thevic16 0:b8adbf13199b 203 while (echo==0) {};
thevic16 0:b8adbf13199b 204
thevic16 0:b8adbf13199b 205 //echo high, so start timer
thevic16 0:b8adbf13199b 206 sonar.start();
thevic16 0:b8adbf13199b 207 //wait for echo low
thevic16 0:b8adbf13199b 208 while (echo==1) {};
thevic16 0:b8adbf13199b 209 //stop timer and read value
thevic16 0:b8adbf13199b 210 sonar.stop();
thevic16 0:b8adbf13199b 211 //subtract software overhead timer delay and scale to cm
thevic16 0:b8adbf13199b 212 distance4 = (sonar.read_us()-correction)/58.0;
thevic16 0:b8adbf13199b 213
thevic16 0:b8adbf13199b 214 //printf("Sensor proximidad 4: %d cm \n\r",distance4);
thevic16 0:b8adbf13199b 215 //wait so that any echo(s) return before sending another ping
thevic16 0:b8adbf13199b 216 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 217 }
thevic16 0:b8adbf13199b 218 }
thevic16 0:b8adbf13199b 219
thevic16 0:b8adbf13199b 220 void thread5_Joystick(){
thevic16 0:b8adbf13199b 221 SparkfunAnalogJoystick JoyStick(A1, A0, D1);
thevic16 0:b8adbf13199b 222
thevic16 0:b8adbf13199b 223 float X;
thevic16 0:b8adbf13199b 224 float Y;
thevic16 0:b8adbf13199b 225
thevic16 0:b8adbf13199b 226 while(1)
thevic16 0:b8adbf13199b 227 {
thevic16 0:b8adbf13199b 228
thevic16 0:b8adbf13199b 229 X = JoyStick.xAxis();
thevic16 0:b8adbf13199b 230 Y = JoyStick.yAxis();
thevic16 0:b8adbf13199b 231
thevic16 0:b8adbf13199b 232 /*
thevic16 0:b8adbf13199b 233 printf("X-Axis: %f\n\r", X);
thevic16 0:b8adbf13199b 234 printf("Y-Axis: %f\n\r", Y);
thevic16 0:b8adbf13199b 235 printf(" \n\r");
thevic16 0:b8adbf13199b 236 */
thevic16 0:b8adbf13199b 237
thevic16 0:b8adbf13199b 238 if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f ){
thevic16 0:b8adbf13199b 239 if(distance1 > distanceLimit)
thevic16 0:b8adbf13199b 240 {
thevic16 0:b8adbf13199b 241 printf(" Hacia adelante \r \n");
thevic16 0:b8adbf13199b 242 }
thevic16 0:b8adbf13199b 243 else{
thevic16 0:b8adbf13199b 244 printf(" Obstaculo! No se puede ir hacia adelante. \r \n");
thevic16 0:b8adbf13199b 245 }
thevic16 0:b8adbf13199b 246
thevic16 0:b8adbf13199b 247 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 248 }
thevic16 0:b8adbf13199b 249 if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f){
thevic16 0:b8adbf13199b 250
thevic16 0:b8adbf13199b 251 if(distance2 > distanceLimit)
thevic16 0:b8adbf13199b 252 {
thevic16 0:b8adbf13199b 253 printf(" Hacia atras \r \n");
thevic16 0:b8adbf13199b 254 }
thevic16 0:b8adbf13199b 255 else{
thevic16 0:b8adbf13199b 256 printf(" Obstaculo! No se puede ir hacia atras. \r \n");
thevic16 0:b8adbf13199b 257 }
thevic16 0:b8adbf13199b 258
thevic16 0:b8adbf13199b 259 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 260 }
thevic16 0:b8adbf13199b 261 if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f){
thevic16 0:b8adbf13199b 262
thevic16 0:b8adbf13199b 263 if(distance3 > distanceLimit)
thevic16 0:b8adbf13199b 264 {
thevic16 0:b8adbf13199b 265 printf(" Hacia la izquierda \r \n");
thevic16 0:b8adbf13199b 266 }
thevic16 0:b8adbf13199b 267 else{
thevic16 0:b8adbf13199b 268 printf(" Obstaculo! No se puede ir hacia la izquierda. \r \n");
thevic16 0:b8adbf13199b 269 }
thevic16 0:b8adbf13199b 270
thevic16 0:b8adbf13199b 271 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 272 }
thevic16 0:b8adbf13199b 273 if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f){
thevic16 0:b8adbf13199b 274
thevic16 0:b8adbf13199b 275 if(distance4 > distanceLimit)
thevic16 0:b8adbf13199b 276 {
thevic16 0:b8adbf13199b 277 printf(" Hacia la derecha \r \n");
thevic16 0:b8adbf13199b 278 }
thevic16 0:b8adbf13199b 279 else{
thevic16 0:b8adbf13199b 280 printf(" Obstaculo! No se puede ir hacia la derecha. \r \n");
thevic16 0:b8adbf13199b 281 }
thevic16 0:b8adbf13199b 282
thevic16 0:b8adbf13199b 283
thevic16 0:b8adbf13199b 284 thread_sleep_for(1000);
thevic16 0:b8adbf13199b 285 }
thevic16 0:b8adbf13199b 286
thevic16 0:b8adbf13199b 287 //thread_sleep_for(1000);
thevic16 0:b8adbf13199b 288 }
thevic16 0:b8adbf13199b 289
thevic16 0:b8adbf13199b 290
thevic16 0:b8adbf13199b 291 }
thevic16 0:b8adbf13199b 292
thevic16 0:b8adbf13199b 293
thevic16 0:b8adbf13199b 294 int main()
thevic16 0:b8adbf13199b 295 {
thevic16 0:b8adbf13199b 296 thread1.start(thread1_HCSR04);
thevic16 0:b8adbf13199b 297 thread2.start(thread2_HCSR04);
thevic16 0:b8adbf13199b 298 thread3.start(thread3_HCSR04);
thevic16 0:b8adbf13199b 299 thread4.start(thread4_HCSR04);
thevic16 0:b8adbf13199b 300 thread5.start(thread5_Joystick);
thevic16 0:b8adbf13199b 301
thevic16 0:b8adbf13199b 302 }