victor gomez
yes
Dependencies: SparkfunAnalogJoystick
Revision 1:17ea74f31633, committed 2021-06-24
- Comitter:
- thevic16
- Date:
- Thu Jun 24 19:50:59 2021 +0000
- Parent:
- 0:b8adbf13199b
- Commit message:
- Prueba2;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r b8adbf13199b -r 17ea74f31633 main.cpp
--- 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