Calificación:
  • 0 voto(s) - 0 Media
  • 1
  • 2
  • 3
  • 4
  • 5
Código para cabeza robot (sensor ultrasonico + servo)
#1
Buenas a tod@s:

Llevo tiempo con la idea de realizar un pequeño robot que por lo menos sea capaz de evitar obstáculos. Por eso me compré el sensor ultrasónico para poder empezar.

Bueno, el caso es que esta tarde me he puesto con el código. He cogido el que ya tenía hecho de anteriores pruebas con el sensor ultrasónico más el servomotor y lo he ido modificando a mi gusto.

Mi intención es que el servomotor se mueva de la posición 0º a 90º a 180º a 90º y a 0º otra vez. Y así de forma indefinida. En esos grados en concreto, que el sensor ultrasónico tome una medida, y una vez hecho un barrido entero de una sola pasada, es decir ya sea de derecha a izquierda o de izquierda a derecha, se comparen las 3 medidas, y que se decida ir hacia el lado donde se haya medido una mayor distancia.

Como no tengo motores lo que tengo pensado es, si quiere girar a la izquierda me enciende el led nº 1, si va recto el led nº 2 y si va a la derecha me encienda el led nº3. En el caso que el sensor detecte la misma distancia en las 3 medidas que siga recto.

Como no quiero utilizar delays para no detener el procesador he empezado con los milis por primera vez. Y me estoy perdiendo un poco. Entiendo, o creo que lo entiendo, un poco el funcionamiento. Sin embargo, ahora me ha entrado la duda, que en el primer bucle de programa, pude que funcione, pero cuando lo vuelva a repetir, los milis ya no coincidan. Entonces como lo puedo solucionar?

De momento tengo este código, hasta que no he colocado el tema de los milis es correcto, lo he probado, pero el servo no tiene tiempo a situarse en la posición que se le indica y no se mueve de sitio. Pero el programa está bien compilado.

Igual que los leds, la distancia 1 es la distancia que se mide a la izquierda la distancia 2 en el centro y la distancia 3 en la derecha.

Ahora estoy liado con lo de los milis a ver como lo soluciono. Porque quiero ponerlo para que el servo tenga tiempo de posicionarse, que el sensor de el disparo y reciba, el servo se mueva, dispare el sensor y así sucesivamente y una vez hecho los 3 movimientos con los 3 disparos se compare y decida.

Sé que aún me falta el retorno del servo, ya que tal y como está ahora una vez esté a los 180º volvería directamente a los 0º, y quiero que al volver al punto inicial también pase por los 90º haciendo una medida. O como mínimo, quiero que vuelva de la misma forma como ha ido, aunque no se pare hacer la medida.
Código:
#include <Servo.h>      //Incluimos la libreria Servo
#include <Ultrasonic.h>  //Incluimos la libreria Ultrasonico

int led1=13;
int led2=12;
int led3=11;
Servo Servo1;          //Declaramos nuestro servo
Ultrasonic ultraso (6, 7);  //Declaramos los pines del ultrasonico
int distancia1;          //Declaramos una constante
int distancia2; // Declaramos una constante 2
int distancia3; //Declaramos una constante 3
int pausa;          //Declaramos una cosntante
long previosuMillis = 0; // Almacena la ultima vez que el servo se movió
long interval = 100; // interval eespera

void setup (){
pinMode(led1,OUTPUT);
pinMode(led2,OUTPUT);
pinMode(led3,OUTPUT);
Servo1.attach(8);    //Declaramos el pin de nuestro servo
Serial.begin(9600);
}

void loop()
{
unsigned long currentMilllis = millis ();
if (currentMillis == interval){
Servo1.write(0);
distancia1=ultraso.Ranging(CM);
}
Servo1.write(90);
if (currentMillis == interval){
distancia2=ultraso.Ranging(CM);
Servo1.write(180);
}
if (currentMillis == interval){
distancia3 = ultraso.Ranging(CM);
}
if (distancia1>distancia2 && distancia1>distancia3)
{
  digitalWrite(led1,HIGH);
  digitalWrite(led2,LOW);
  digitalWrite(led3,LOW);  
}

else if (distancia2>distancia1 && distancia2>distancia3)
{
  digitalWrite(led2,HIGH);
  digitalWrite(led1,LOW);
  digitalWrite(led3,LOW);
}
if(distancia3>distancia1 && distancia3>distancia2){
  digitalWrite(led3,HIGH);
  digitalWrite(led1,LOW);
  digitalWrite(led2,LOW);
}
if (distancia1==distancia3)
{
  digitalWrite(led1,HIGH);
  digitalWrite(led2,LOW);
  digitalWrite(led3,LOW);
}
else{
  digitalWrite(led2,HIGH);
  digitalWrite(led1,LOW);
  digitalWrite(led3,LOW);
}

  
  


Serial.print("  Distancia estimada izquierda: "); //Comandos para ver en pantalla la distancia al cual está el obstaculo
Serial.print(distancia1);
Serial.println(" cm");
Serial.print("  Distancia estimada centro: "); //Comandos para ver en pantalla la distancia al cual está el obstaculo
Serial.print(distancia1);
Serial.println(" cm");
  Serial.print("  Distancia estimada derecha: "); //Comandos para ver en pantalla la distancia al cual está el obstaculo
Serial.print(distancia1);
Serial.println(" cm");

}

Para implementar lo de los milis me he basado en ese ejemplo: http://www.ardumania.es/ejercicio-2-blin...out-delay/

Muchas gracias de antemano.

Mientras tanto, continuaré indagando con ejemplos de los milis a ver si me aclaro y encuentro solución.
Citar
#2
Buenas a tod@s:

A la espera de mejorar el código, lo que he hecho es hacer lo que no quería, utilizar delays. Así, mientras esté tomando las medidas se parará completamente el robot y una vez tomadas reiniciará la marcha.

Este punto lo quiero mejorar y que no tenga que detenerse para tomar las medidas, pero bueno... como solo es una cuestión de codigo que se puede cambiar en cualquier momento, no me preocupa.
Citar


Temas similares...
Tema Autor Respuestas Vistas Último mensaje
  error al compilar el código homer32 4 207 23-09-2019, 01:29 PM
Último mensaje: homer32
  CONSULTA Codigo pluviometro balancin Dieseldj1 0 186 01-09-2019, 10:43 PM
Último mensaje: Dieseldj1
  CONSULTA Re: Arduino Mega, 1 servo ,1 nema, 3 motor DC carlos hop 8 890 26-09-2018, 01:30 PM
Último mensaje: Shellmer
  CONSULTA ROBOT DOS MOTORES CON SERVO PARA DIRECCION Miguelmetro 1 590 23-04-2018, 06:30 PM
Último mensaje: Iván
  conversión código arduino ethernet 5100 a wemos d1 mini nbaglietto 0 679 24-11-2017, 02:05 AM
Último mensaje: nbaglietto