![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Wheelchair Logic v3 Project (New)
Dependencies: SparkfunAnalogJoystick
Diff: main.cpp
- Revision:
- 1:17ea74f31633
- Parent:
- 0:b8adbf13199b
- Child:
- 2:4f5a0c64d9cd
--- a/main.cpp Thu Jun 10 20:35:27 2021 +0000 +++ b/main.cpp Thu Jun 24 19:50:59 2021 +0000 @@ -4,27 +4,81 @@ #define M_PI 3.14159265358979323846 - Thread thread1; Thread thread2; Thread thread3; Thread thread4; Thread thread5; +Thread thread6; +Thread thread7; -int distance1 = 0; -int distance2 = 0; -int distance3 = 0; -int distance4 = 0; +int distance1 = 0; //distancia adelante. +int distance2 = 0; //distancia atras. +int distance3 = 0; // distancia izquierda. +int distance4 = 0; // distancia derecha. int distanceLimit = 10; +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 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 @@ -32,48 +86,50 @@ // start timer sonar.start(); // min software polling delay to read echo pin - while (echo==2) {}; - + 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); - + 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 + 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 + //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 + //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); - } + //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 thread2_HCSR04() +void thread3_HCSR04() { - DigitalOut trigger(D2); - DigitalIn echo(D3); + DigitalOut trigger(D4); + DigitalIn echo(D5); Timer sonar; - int correction = 0; sonar.reset(); // measure actual software polling timer delays @@ -81,49 +137,50 @@ // start timer sonar.start(); // min software polling delay to read echo pin - while (echo==2) {}; - + 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); - + 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 + 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 + //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 + //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); - } + //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 thread3_HCSR04() + +void thread4_HCSR04() { - DigitalOut trigger(D4); - DigitalIn echo(D5); + DigitalOut trigger(D6); + DigitalIn echo(D7); Timer sonar; - int correction = 0; sonar.reset(); // measure actual software polling timer delays @@ -131,166 +188,216 @@ // 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) {}; + while (echo==2) + { - //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 + 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 + //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 + //wait for echo low + while (echo==1) + { + + }; + //stop timer and read value sonar.stop(); - //subtract software overhead timer delay and scale to cm + //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); - } + //wait so that any echo(s) return before sending another ping + thread_sleep_for(1000); + } } -void thread5_Joystick(){ - SparkfunAnalogJoystick JoyStick(A1, A0, D1); - +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(1000); + } + 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(1000); + } + 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(1000); + } + 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(1000); + } + thread_sleep_for(5); + } + } +} + +void thread6_ComandosVoz() +{ + char c = pc.getc(); 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) + if(!modo1 && !modo2 && modo3 && !modo4) + { + if(c == 'w') + { + if(distance1 > distanceLimit ) + { + pc.printf(" Comandos de voz: Hacia adelante \r \n"); + thread_sleep_for(500); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n"); + } + } + if(c == 'd') { - printf(" Hacia adelante \r \n"); + if(distance2 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia la derecha \r \n"); + thread_sleep_for(500); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n"); + } + } + if(c == 's') + { + if(distance3 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia atrás \r \n"); + thread_sleep_for(500); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \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(c == 'a') + { + if(distance4 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia la izquierda \r \n"); + thread_sleep_for(500); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n"); + } + } + } + } +} + +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(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); + 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); @@ -298,5 +405,6 @@ thread3.start(thread3_HCSR04); thread4.start(thread4_HCSR04); thread5.start(thread5_Joystick); - + thread6.start(thread6_ComandosVoz); + thread7.start(thread7_IndicarModo); } \ No newline at end of file