![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Wheelchair Logic v3 Project (New)
Dependencies: SparkfunAnalogJoystick
main.cpp
- Committer:
- erodrz
- Date:
- 2021-06-25
- Revision:
- 2:4f5a0c64d9cd
- Parent:
- 1:17ea74f31633
- Child:
- 3:3d54fd4109c0
File content as of revision 2:4f5a0c64d9cd:
#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; Thread thread6; Thread thread7; int distance1 = 0; //distancia adelante. int distance2 = 0; //distancia atras. int distance3 = 0; // distancia izquierda. int distance4 = 0; // distancia derecha. int distanceLimit = 15; DigitalIn modo1(D8); //Modo manual DigitalIn modo2(D9); //Modo comandos joystick DigitalIn modo3(D10); //Modo comandos de voz DigitalIn modo4(D11); //Modo rutas autonomas Serial pc(USBTX, USBRX); //Para recibir los comandos de voz. void flushSerialBuffer(void) { char char1 = 0; while (pc.readable()) { char1 = pc.getc(); } return; } 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, D12); float X; float Y; while(1) { if(!modo1 && modo2 && !modo3 && !modo4) { 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("Comandos joystick: Hacia adelante \r \n"); } else { printf("Comandos joystick: Obstaculo adelante \r \n"); } thread_sleep_for(500); } if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f) { if(distance2 > distanceLimit) { printf("Comandos joystick: Hacia atras \r \n"); } else { printf("Comandos joystick: Obstaculo atras \r \n"); } thread_sleep_for(500); } if(Y >= -0.60f && Y <= 0.60f && X <= -0.90f && X >= -1.00f) { if(distance3 > distanceLimit) { printf("Comandos joystick: Hacia izquierda \r \n"); } else { printf("Comandos joystick: Obstaculo izquierda \r \n"); } thread_sleep_for(500); } if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f) { if(distance4 > distanceLimit) { printf("Comandos joystick: Hacia derecha \r \n"); } else { printf("Comandos joystick: Obstaculo derecha \r \n"); } thread_sleep_for(500); } thread_sleep_for(5); } } } void thread6_ComandosVoz() { while(1) { if(!modo1 && !modo2 && modo3 && !modo4) { flushSerialBuffer(); char c = pc.getc(); if(c == 'w') { //printf("Distance1 - %d \r \n",distance1); if(distance1 > distanceLimit) { pc.printf(" Comandos de voz: Hacia adelante \r \n"); } else { printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n"); } thread_sleep_for(1000); } if(c == 's') { //printf("Distance2 - %d \r \n",distance2); if(distance2 > distanceLimit) { pc.printf(" Comandos de voz: Hacia atras \r \n"); } else { printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n"); } thread_sleep_for(1000); } if(c == 'a') { //printf("Distance3 - %d \r \n",distance3); if(distance3 > distanceLimit) { pc.printf(" Comandos de voz: Hacia la izquierda \r \n"); } else { printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n"); } thread_sleep_for(1000); } if(c == 'd') { //printf("Distance4 - %d \r \n",distance4); if(distance4 > distanceLimit) { pc.printf(" Comandos de voz: Hacia la derecha \r \n"); } else { printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n"); } thread_sleep_for(1000); } c = ' '; thread_sleep_for(5); } } } void thread7_IndicarModo() { bool estadomodo1 = false; bool estadomodo2 = false; bool estadomodo3 = false; bool estadomodo4 = false; while (true) { if(modo1 && !modo2 && !modo3 && !modo4 && !estadomodo1) { printf("Modo manual \r \n"); estadomodo1 = true; estadomodo2 = false; estadomodo3 = false; estadomodo4 = false; } if(!modo1 && modo2 && !modo3 && !modo4 && !estadomodo2) { printf("Modo comandos joystick \r \n"); estadomodo1 = false; estadomodo2 = true; estadomodo3 = false; estadomodo4 = false; } if(!modo1 && !modo2 && modo3 && !modo4 && !estadomodo3) { printf("Modo comandos de voz \r \n"); estadomodo1 = false; estadomodo2 = false; estadomodo3 = true; estadomodo4 = false; } if(!modo1 && !modo2 && !modo3 && modo4 && !estadomodo4) { printf("Modo rutas autonomas \r \n"); estadomodo1 = false; estadomodo2 = false; estadomodo3 = false; estadomodo4 = true; } } } int main() { thread1.start(thread1_HCSR04); thread_sleep_for(200); thread2.start(thread2_HCSR04); thread_sleep_for(200); thread3.start(thread3_HCSR04); thread_sleep_for(200); thread4.start(thread4_HCSR04); thread_sleep_for(200); thread5.start(thread5_Joystick); thread_sleep_for(200); thread6.start(thread6_ComandosVoz); thread_sleep_for(200); thread7.start(thread7_IndicarModo); thread_sleep_for(200); }