Proyecto ITT
Dependencies: SparkfunAnalogJoystick
Diff: main.cpp
- Revision:
- 3:3d54fd4109c0
- Parent:
- 2:4f5a0c64d9cd
--- a/main.cpp Fri Jun 25 00:34:17 2021 +0000 +++ b/main.cpp Wed Jul 21 19:16:11 2021 +0000 @@ -26,6 +26,15 @@ Serial pc(USBTX, USBRX); //Para recibir los comandos de voz. + +//motor driver +DigitalOut direccion1(PC_6); +PwmOut PWM_velocidad1(PB_15); + +DigitalOut direccion2(D14); +PwmOut PWM_velocidad2(D15); + + void flushSerialBuffer(void) { char char1 = 0; @@ -36,6 +45,83 @@ return; } + +void mover_hacia_adelante(int tiempo){ + direccion1 = 0; // En direccion de las manecillas del reloj. + direccion2 = 0; // En direccion de las manecillas del reloj. + + + PWM_velocidad1.period(0.0001f); //Declaramos el periodo + PWM_velocidad1.write(0.15f); //%25 + + PWM_velocidad2.period(0.0001f); //Declaramos el periodo + PWM_velocidad2.write(0.15f); //25% + + + thread_sleep_for(tiempo); + + PWM_velocidad1.write(0.0f); + PWM_velocidad2.write(0.0f); +} + +void mover_hacia_atras(int tiempo){ + direccion1 = 1; // En direccion de las manecillas del reloj. + direccion2 = 1; // En direccion de las manecillas del reloj. + + + PWM_velocidad1.period(0.0001f); //Declaramos el periodo + PWM_velocidad1.write(0.15f); //%25 + + PWM_velocidad2.period(0.0001f); //Declaramos el periodo + PWM_velocidad2.write(0.15f); //25% + + + thread_sleep_for(tiempo); + + PWM_velocidad1.write(0.0f); + PWM_velocidad2.write(0.0f); +} + + +void mover_hacia_izquierda(int tiempo){ + direccion1 = 1; // En direccion de las manecillas del reloj. + direccion2 = 0; // En direccion de las manecillas del reloj. + + + PWM_velocidad1.period(0.0001f); //Declaramos el periodo + PWM_velocidad1.write(0.15f); //%25 + + PWM_velocidad2.period(0.0001f); //Declaramos el periodo + PWM_velocidad2.write(0.15f); //25% + + + thread_sleep_for(tiempo); + + PWM_velocidad1.write(0.0f); + PWM_velocidad2.write(0.0f); +} + + +void mover_hacia_derecha(int tiempo){ + direccion1 = 0; // En direccion de las manecillas del reloj. + direccion2 = 1; // En direccion de las manecillas del reloj. + + + PWM_velocidad1.period(0.0001f); //Declaramos el periodo + PWM_velocidad1.write(0.15f); //%25 + + PWM_velocidad2.period(0.0001f); //Declaramos el periodo + PWM_velocidad2.write(0.15f); //25% + + + thread_sleep_for(tiempo); + + PWM_velocidad1.write(0.0f); + PWM_velocidad2.write(0.0f); +} + + + void thread1_HCSR04() { DigitalOut trigger(D0); @@ -258,6 +344,7 @@ if(distance1 > distanceLimit) { printf("Comandos joystick: Hacia adelante \r \n"); + mover_hacia_adelante(1000); // un segundo } else { @@ -270,6 +357,7 @@ if(distance2 > distanceLimit) { printf("Comandos joystick: Hacia atras \r \n"); + mover_hacia_atras(1000); // un segundo } else { @@ -282,6 +370,7 @@ if(distance3 > distanceLimit) { printf("Comandos joystick: Hacia izquierda \r \n"); + mover_hacia_izquierda(1000); //un segundo } else { @@ -294,6 +383,7 @@ if(distance4 > distanceLimit) { printf("Comandos joystick: Hacia derecha \r \n"); + mover_hacia_derecha(1000); // segundo } else { @@ -314,60 +404,72 @@ { 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 + + thread_sleep_for(5); //timer necesario para que el compilar se de cuenta el orden correcto de ejecucion + + int m = modo3.read(); + printf("estado modo 3 (comandos de voz): %d \r \n",m); + + if(m == 1){ + if(c == 'w') { - 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"); + //printf("Distance1 - %d \r \n",distance1); + if(distance1 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia adelante \r \n"); + mover_hacia_adelante(3000); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n"); + } + thread_sleep_for(1000); } - thread_sleep_for(1000); - } - if(c == 'a') - { - //printf("Distance3 - %d \r \n",distance3); - if(distance3 > distanceLimit) + if(c == 's') { - pc.printf(" Comandos de voz: Hacia la izquierda \r \n"); - } - else + //printf("Distance2 - %d \r \n",distance2); + if(distance2 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia atras \r \n"); + mover_hacia_atras(3000); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n"); + } + thread_sleep_for(1000); + } + if(c == 'a') { - printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n"); + //printf("Distance3 - %d \r \n",distance3); + if(distance3 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia la izquierda \r \n"); + mover_hacia_izquierda(3000); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n"); + } + thread_sleep_for(1000); } - 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"); + if(c == 'd') + { + //printf("Distance4 - %d \r \n",distance4); + if(distance4 > distanceLimit) + { + pc.printf(" Comandos de voz: Hacia la derecha \r \n"); + mover_hacia_derecha(3000); + } + else + { + printf(" Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n"); + } + thread_sleep_for(1000); } - thread_sleep_for(1000); } c = ' '; - thread_sleep_for(5); + thread_sleep_for(5); } } }