Proyecto ITT
Dependencies: SparkfunAnalogJoystick
Diff: main.cpp
- Revision:
- 0:b8adbf13199b
- Child:
- 1:17ea74f31633
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jun 10 20:35:27 2021 +0000 @@ -0,0 +1,302 @@ +#include "mbed.h" +#include "platform/mbed_thread.h" +#include "SparkfunAnalogJoystick.h" + +#define M_PI 3.14159265358979323846 + + +Thread thread1; +Thread thread2; +Thread thread3; +Thread thread4; +Thread thread5; + +int distance1 = 0; +int distance2 = 0; +int distance3 = 0; +int distance4 = 0; + +int distanceLimit = 10; + + +void thread1_HCSR04() +{ + DigitalOut trigger(D0); + DigitalIn echo(D1); + Timer sonar; + + int correction = 0; + sonar.reset(); + // measure actual software polling timer delays + // delay used later in time correction + // start timer + sonar.start(); + // min software polling delay to read echo pin + while (echo==2) {}; + + // stop timer + sonar.stop(); + // read timer + correction = sonar.read_us(); + printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction); + + //Loop to read Sonar distance values, scale, and print + while(1) { + // trigger sonar to send a ping + trigger = 1; + + + sonar.reset(); + wait_us(10.0); + trigger = 0; + + //wait for echo high + while (echo==0) {}; + + //echo high, so start timer + sonar.start(); + //wait for echo low + while (echo==1) {}; + //stop timer and read value + sonar.stop(); + //subtract software overhead timer delay and scale to cm + distance1 = (sonar.read_us()-correction)/58.0; + + //printf("Sensor proximidad 1: %d cm \n\r",distance1); + //wait so that any echo(s) return before sending another ping + thread_sleep_for(1000); + } +} + +void thread2_HCSR04() +{ + DigitalOut trigger(D2); + DigitalIn echo(D3); + Timer sonar; + + int correction = 0; + sonar.reset(); + // measure actual software polling timer delays + // delay used later in time correction + // start timer + sonar.start(); + // min software polling delay to read echo pin + while (echo==2) {}; + + // stop timer + sonar.stop(); + // read timer + correction = sonar.read_us(); + printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction); + + //Loop to read Sonar distance values, scale, and print + while(1) { + // trigger sonar to send a ping + trigger = 1; + + + sonar.reset(); + wait_us(10.0); + trigger = 0; + + //wait for echo high + while (echo==0) {}; + + //echo high, so start timer + sonar.start(); + //wait for echo low + while (echo==1) {}; + //stop timer and read value + sonar.stop(); + //subtract software overhead timer delay and scale to cm + distance2 = (sonar.read_us()-correction)/58.0; + + //printf("Sensor proximidad 2: %d cm \n\r",distance2); + //wait so that any echo(s) return before sending another ping + thread_sleep_for(1000); + } +} + + +void thread3_HCSR04() +{ + DigitalOut trigger(D4); + DigitalIn echo(D5); + Timer sonar; + + int correction = 0; + sonar.reset(); + // measure actual software polling timer delays + // delay used later in time correction + // start timer + sonar.start(); + // min software polling delay to read echo pin + while (echo==2) {}; + + // stop timer + sonar.stop(); + // read timer + correction = sonar.read_us(); + printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction); + + //Loop to read Sonar distance values, scale, and print + while(1) { + // trigger sonar to send a ping + trigger = 1; + + + sonar.reset(); + wait_us(10.0); + trigger = 0; + + //wait for echo high + while (echo==0) {}; + + //echo high, so start timer + sonar.start(); + //wait for echo low + while (echo==1) {}; + //stop timer and read value + sonar.stop(); + //subtract software overhead timer delay and scale to cm + distance3 = (sonar.read_us()-correction)/58.0; + + //printf("Sensor proximidad 3: %d cm \n\r",distance3); + //wait so that any echo(s) return before sending another ping + thread_sleep_for(1000); + } +} + + +void thread4_HCSR04() +{ + DigitalOut trigger(D6); + DigitalIn echo(D7); + Timer sonar; + + int correction = 0; + sonar.reset(); + // measure actual software polling timer delays + // delay used later in time correction + // start timer + sonar.start(); + // min software polling delay to read echo pin + while (echo==2) {}; + + // stop timer + sonar.stop(); + // read timer + correction = sonar.read_us(); + printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction); + + //Loop to read Sonar distance values, scale, and print + while(1) { + // trigger sonar to send a ping + trigger = 1; + + + sonar.reset(); + wait_us(10.0); + trigger = 0; + + //wait for echo high + while (echo==0) {}; + + //echo high, so start timer + sonar.start(); + //wait for echo low + while (echo==1) {}; + //stop timer and read value + sonar.stop(); + //subtract software overhead timer delay and scale to cm + distance4 = (sonar.read_us()-correction)/58.0; + + //printf("Sensor proximidad 4: %d cm \n\r",distance4); + //wait so that any echo(s) return before sending another ping + thread_sleep_for(1000); + } +} + +void thread5_Joystick(){ + SparkfunAnalogJoystick JoyStick(A1, A0, D1); + + float X; + float Y; + + while(1) + { + + X = JoyStick.xAxis(); + Y = JoyStick.yAxis(); + + /* + printf("X-Axis: %f\n\r", X); + printf("Y-Axis: %f\n\r", Y); + printf(" \n\r"); + */ + + if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f ){ + if(distance1 > distanceLimit) + { + printf(" Hacia adelante \r \n"); + } + else{ + printf(" Obstaculo! No se puede ir hacia adelante. \r \n"); + } + + thread_sleep_for(1000); + } + if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f){ + + if(distance2 > distanceLimit) + { + printf(" Hacia atras \r \n"); + } + else{ + printf(" Obstaculo! No se puede ir hacia atras. \r \n"); + } + + thread_sleep_for(1000); + } + if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f){ + + if(distance3 > distanceLimit) + { + printf(" Hacia la izquierda \r \n"); + } + else{ + printf(" Obstaculo! No se puede ir hacia la izquierda. \r \n"); + } + + thread_sleep_for(1000); + } + if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f){ + + if(distance4 > distanceLimit) + { + printf(" Hacia la derecha \r \n"); + } + else{ + printf(" Obstaculo! No se puede ir hacia la derecha. \r \n"); + } + + + thread_sleep_for(1000); + } + + //thread_sleep_for(1000); + } + + + } + + +int main() +{ + thread1.start(thread1_HCSR04); + thread2.start(thread2_HCSR04); + thread3.start(thread3_HCSR04); + thread4.start(thread4_HCSR04); + thread5.start(thread5_Joystick); + +} \ No newline at end of file