New
Dependencies: SparkfunAnalogJoystick
main.cpp
- Committer:
- thevic16
- Date:
- 2021-06-10
- Revision:
- 0:b8adbf13199b
- Child:
- 1:17ea74f31633
File content as of revision 0:b8adbf13199b:
#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); }