Lectura de sonar HC-SR04

[liskarra0]

Buenas,

Estoy haciendo un proyecto que consta de un rover 4x4 que a través de la Pixy2 identifica colores de diferentes objetos y los lleva cada uno a un determinado lugar. Ademas, le he puesto el sonar HC-SR04 para detectar objetos y detenerse si encuentra alguno.

El caso es que el sonar daba lecturas correctas y todo funcionaba de maravilla hasta que metí la Pixy2 para determinar que rutina elegir (para ir a un sitio o a otro).

La subrutina del sonar es la misma de antes y ahora no funciona.

Os adjunto la subrutina del sonar y como utilizo la Pixy2.

HC-SR04

Code: [Select]

long SONAR( int angelua) // SONARRAREN AZPIPROGRAMA
{  

           servoMotor.attach(10);
           long duracion, distancia ;
           
           servoMotor.write(angelua);
           delay(50);
           digitalWrite(52,HIGH);
           digitalWrite(trigPin, LOW);        // Nos aseguramos de que el trigger está desactivado
           delayMicroseconds(2);              // Para asegurarnos de que el trigger esta LOW
           digitalWrite(trigPin, HIGH);       // Activamos el pulso de salida
           delayMicroseconds(10);             // Esperamos 10µs. El pulso sigue active este tiempo
           digitalWrite(trigPin, LOW);        // Cortamos el pulso y a esperar el echo
           duracion = pulseIn(echoPin, HIGH) ;
           distancia = duracion / 2 / 29.1  ;
           //Serial.println(String(distancia) + " cm.") ;

           return distancia;
}


PIXY2:

Code: [Select]

void loop()
{
 Serial.println("ITXAROTEN");
 pixy.ccc.getBlocks(); // Kamerako datuak irakurri
 if(pixy.ccc.numBlocks) // Zerbait detektatu duen jakiteko
 {
   
   if(pixy.ccc.blocks[0].m_signature==1)
   {
     Abat(); // A bat lekura joatea
   }
   else if(pixy.ccc.blocks[0].m_signature==2)
   {
     Abi();   // A bi lekura joatea
   }
   else if(pixy.ccc.blocks[0].m_signature==3)
   {
     digitalWrite(30,HIGH);
   }
   
 }
}


Espero que podáis ayudarme.

Muchas gracias

[liskarra0]

La lectura erronea de la que hablo es que siempre da 0.
El ardunio que estoy utilizando es arduino Mega.

[surbyte]

Hola @Liskarra0, bienvenido al foro Arduino en Español.

Te invito a que leas las Normas del foro y luego edites tu post inicial

Si observas, he modificado tu segundo post porque repites lo que se lee en el primero. No lo hagas, eso se considera una falta menor pero falta al fin a las normas.
Manten el hilo limpio de comentarios repetidos.

Solo usa quote o cita para resaltar un párrafo de quien te responda, pero no TODA su respuesta.
Simple no?

Bueno, las cosas no funcionan porque se han agregado tiempos y éstos alternan los comportamientos que conocías.

Igualmente colocas los dos codigos por separado pero no se ve el código de como esta ahora el conjunto.
Usa etiquetas.

[victorjam]

El moderador te ha regañado ya por no leer las NORMAS, y vas y posteas el código de la misma manera...

[liskarra0]

Hola surbyte, gracias por contestarme.

No entiendo a que te refieres con añadir tiempos. Como ya comente el programa funcionaba perfectamente cuando en el loop escribía yo que rutina utilizar. Sin embargo al añadir la pixy2 para la elección de la rutina las mediciones del sonar eran erroneas.

Adjunto el código entero para que puedas entender mejor el programa.

Muchas gracias de antemano.

Code: [Select]



#include <AFMotor.h>
#include <Servo.h>
#include <Pixy2.h>

// SONARRAREN PINAK
    #define trigPin 50 // Arduino pin tied to trigger pin on the ultrasonic sensor.   
    #define echoPin 48    // Arduino pin tied to echo pin on the ultrasonic sensor.
    #define led 46
   
// PI-ren ezarpena
    #define PI 3.1415926535897932384626433832795
     
int encoderPos=0;
AF_DCMotor aurezk(4,MOTOR34_64KHZ); // Rueda delantera izquierda
AF_DCMotor auresk(3,MOTOR34_64KHZ); // Rueda delantera derecha
AF_DCMotor atzesk(1,MOTOR12_64KHZ); // Rueda trasera derecha
AF_DCMotor atzezk(2,MOTOR12_64KHZ); // Rueda trasera izquierda
Servo servoMotor;
Pixy2 pixy;



void setup()
{
          Serial.begin(9600);
          pinMode(trigPin, OUTPUT);
          pinMode(echoPin, INPUT);
          pinMode(led, OUTPUT);
          pinMode(52,OUTPUT);
          attachInterrupt(2, interrupcion, FALLING);
          pixy.init(); // Inicializacion de la camara Pixy2
}

void loop()
{
 Serial.println("ITXAROTEN");
 pixy.ccc.getBlocks(); // Recoger datos de la camara
 if(pixy.ccc.numBlocks) // Para comprobar si ha detectado algo
 {
   
   if(pixy.ccc.blocks[0].m_signature==1) // Color uno detectado
   {
     Abat(); // Ir a A1
   }
   else if(pixy.ccc.blocks[0].m_signature==2) // Color dos detectado
   {
     Abi();   // Ir a A2
   }
   else if(pixy.ccc.blocks[0].m_signature==3) // Color. 3 detectado
   {
     digitalWrite(30,HIGH);
   }
   
 }
}



void interrupcion()
{
 encoderPos++;
}




void JATORRItikAP()
{
 
int angelua; //angulo del servo
int aldaketa; // variable para cambiar el angulo
double minimoa; //distancia mínima para colision

// establecer velocidades
atzezk.setSpeed(100);
atzesk.setSpeed(100);
aurezk.setSpeed(100);
auresk.setSpeed(100);

long distancia;
distancia=0;
encoderPos=0;
angelua=90;
aldaketa=-5;
delay(1000);
do
{
//moverse
atzezk.run(FORWARD); // Hacia delante
atzesk.run(FORWARD);
auresk.run(FORWARD);
aurezk.run(FORWARD);
//Serial.println(encoderPos);
distancia=SONAR(angelua);
Serial.print("Neurtuta: ");
Serial.print(distancia);
minimoa=kalminimoa(angelua);
Serial.print(" DMIN: ");
Serial.println(minimoa);

 if(distancia<minimoa)
     {
       atzezk.run(RELEASE);
       atzesk.run(RELEASE);
       auresk.run(RELEASE);
       aurezk.run(RELEASE);
       do
       {
         distancia=SONAR(angelua);
         Serial.print("ARRISKUA!");
         Serial.print(" DMIN: ");
         Serial.print(minimoa);
         Serial.print(" REALA: ");
         Serial.println(distancia);
       }while(distancia<minimoa);
       atzezk.run(FORWARD); // hacia delante
       atzesk.run(FORWARD);
       auresk.run(FORWARD);
       aurezk.run(FORWARD);
     }
 angelua=angelua+aldaketa; //actualizar variable del angulo

 
 if(angelua==155)
 {
   aldaketa=-5;
   angelua=145;
   
 }
 if(angelua==40)
 {
   aldaketa=5;
   angelua=40;
 }

}while(encoderPos<=405);

//Parar
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);
}


void APtikA1()
{
int angelua;
int aldaketa;
long distancia;
double minimoa;
minimoa=0;
distancia=0;
angelua=90;
aldaketa=-5;
encoderPos=0;
// Abiadurak ezarri
atzezk.setSpeed(100);
atzesk.setSpeed(100);
aurezk.setSpeed(100);
auresk.setSpeed(100);
delay(1000);
do
{
//Mugitzen hasi
atzezk.run(FORWARD);
atzesk.run(FORWARD);
auresk.run(FORWARD);
aurezk.run(FORWARD);
distancia=SONAR(angelua);
minimoa=kalminimoa(angelua);

if(distancia<minimoa)
 {
   atzezk.run(RELEASE);
   atzesk.run(RELEASE);
   auresk.run(RELEASE);
   aurezk.run(RELEASE);
   do
   {
    distancia=SONAR(angelua);
   }while(distancia<minimoa);
   atzezk.run(FORWARD); // Aurrerantza
   atzesk.run(FORWARD);
   auresk.run(FORWARD);
   aurezk.run(FORWARD);
 }
 angelua=angelua+aldaketa;
}while(encoderPos<=405);

//Gelditu
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);
}

void APtikA2()
{
int angelua;
int aldaketa;
long distancia;
double minimoa;
minimoa=0;
distancia=0;
angelua=90;
aldaketa=-5;
encoderPos=0;
// Abiadurak ezarri
atzezk.setSpeed(100);
atzesk.setSpeed(100);
aurezk.setSpeed(100);
auresk.setSpeed(100);
delay(1000);
do
{
//Mugitzen hasi
atzezk.run(FORWARD); // Aurrerantza
atzesk.run(FORWARD);
auresk.run(FORWARD);
aurezk.run(FORWARD);
distancia=SONAR(angelua);
minimoa=kalminimoa(angelua);

if(distancia<minimoa)
 {
   atzezk.run(RELEASE);
   atzesk.run(RELEASE);
   auresk.run(RELEASE);
   aurezk.run(RELEASE);
   do
   {
    distancia=SONAR(angelua);
   }while(distancia<minimoa);
   atzezk.run(FORWARD); // Aurrerantza
   atzesk.run(FORWARD);
   auresk.run(FORWARD);
   aurezk.run(FORWARD);
 }
 angelua=angelua+aldaketa;
}while(encoderPos<=560);

//Gelditu
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);
}


void Abat()
{
 JATORRItikAP();
 BIRAKETA();
 APtikA1();
 delay(5000);
 KONTRABIRAKETA();
 APtikA1();
 BIRAKETA2();
 JATORRItikAP();
 KONTRABIRAKETA();
}

void Abi()
{
 JATORRItikAP();

}


void KONTRABIRAKETA() // Para girar 360° el Rover
{

 //Abiadurak ezarri
atzezk.setSpeed(120);
atzesk.setSpeed(120);
auresk.setSpeed(120);
aurezk.setSpeed(120);
encoderPos=0;


delay(1000);
do{
//Mugitzen hasi
atzezk.run(FORWARD);
aurezk.run(FORWARD);
atzesk.run(BACKWARD);
auresk.run(BACKWARD);
Serial.print(encoderPos);
}while(encoderPos<=135);


//Gelditu
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);
}


void BIRAKETA2() // Para girar 90 grados el. Rover
{

//Abiadurak ezarri
atzezk.setSpeed(120);
atzesk.setSpeed(120);
auresk.setSpeed(120);
aurezk.setSpeed(120);
encoderPos=0;
delay(1000);
do{

//Mugitzen hasi
atzezk.run(FORWARD);
aurezk.run(FORWARD);
atzesk.run(BACKWARD);
auresk.run(BACKWARD);
Serial.print(encoderPos);
}while(encoderPos<=60);

//Gelditu
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);

}

void BIRAKETA() // Para girar 90°. Rover
{

//Abiadurak ezarri
atzezk.setSpeed(120);
atzesk.setSpeed(120);
auresk.setSpeed(120);
aurezk.setSpeed(120);
encoderPos=0;
delay(1000);
do{
//Mugitzen hasi
atzezk.run(BACKWARD);
aurezk.run(BACKWARD);
atzesk.run(FORWARD);
auresk.run(FORWARD);
Serial.print(encoderPos);
}while(encoderPos<=60);

//Gelditu
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);

}


long SONAR( int angelua) // Subrutina del sonar
{ 

           servoMotor.attach(10);
           long duracion, distancia ;
           
           servoMotor.write(angelua);
           delay(50);
           digitalWrite(52,HIGH);
           digitalWrite(trigPin, LOW);        // Nos aseguramos de que el trigger está desactivado
           delayMicroseconds(2);              // Para asegurarnos de que el trigger esta LOW
           digitalWrite(trigPin, HIGH);       // Activamos el pulso de salida
           delayMicroseconds(10);             // Esperamos 10µs. El pulso sigue active este tiempo
           digitalWrite(trigPin, LOW);        // Cortamos el pulso y a esperar el echo
           duracion = pulseIn(echoPin, HIGH) ;
           distancia = duracion / 2 / 29.1  ;
           //Serial.println(String(distancia) + " cm.") ;

           return distancia;
}


float kalminimoa(int angelua) // subrutina para calcular distancia minima de colision

{
float radianetan;
float minimo;

radianetan=0;
minimo=0;


 if(angelua<90)
{
radianetan=angelua*(2*PI/360); // angulos a radianes
minimo=(7.5/cos(radianetan))+0.5; // Distancia minima kalkuloa + 0.5 margen de seguridad
}
 if(angelua>90)
{
radianetan=angelua*(2*PI/360);
minimo=(-7.5/cos(radianetan))+0.5;

}
if (angelua==90)
{
 minimo=25; // 90º cos()-a error,
}

// limitar la distancia mínima
if (minimo>25)
{
 minimo=25;
}

 return minimo;
}


[surbyte]

En un código donde usas control de motores y dispositivos que deben captar "algo" para enviar una orden hay que hacer las cosas con mucha prolijidad aunque vaya a un sitio de internet y copie un código ese código no me asegura que está todo lo depurado que se requiere.
Estoy escribiendo sin haberlo leído, cosa que haré ahora.

Me pongo a ver el programa y encuentro cosas primero en un idioma que no entiendo aunque traduje 2 o 3 sentencias no vale la pena el esfuerzo. Hazlo tu.

Bueno, evidentemente el código tiene muchos delay() o sea tiempos perdidos, y tiene loops como ese

Code: [Select]

do {} while(encoderPos<=405);

y me quedo corto...
me encuentro que las indicaciones del Pixy conducen a cosas como esta

Code: [Select]

void Abat()
{
 JATORRItikAP();
 BIRAKETA();
 APtikA1();
 delay(5000); // Itxaroteko, gero kamara bidez objeturik ez dagoenenan gertatuko da.
 KONTRABIRAKETA();
 APtikA1();
 BIRAKETA2();
 JATORRItikAP();
 KONTRABIRAKETA();
}

donde no solo cada uno tiene loop con delays de 1000 mseg sino que en el medio tienes otro delay() de 5 segundos. Todo estos son los tiempos a los que me refería.

Si no lo sabías delay() es una instrucción que DESTRUYE un programa como el tuyo donde esperas la acción de otros sensores Pixy, ultrasónico, etc.

Delay() solo sirve cuando la tarea es secuencial al punto que no importa si me detengo o no a hacer algo.

[liskarra0]

Entiendo lo que dices, pero el programa entra sin ningún problema a la subrutina del sonar donde hace el calculo. Ahi no hay delays, mas que los de la salida del Trigger. Una vez ahi dentro los demás delay tienen repercusión?

En vez de usar un delay() recomendarias el uso de millis()??

Muchas gracias

Unai

[ElEspanol]

Pon el código en "etiquetas de código", y los comentarios en español o inglés.

¿Que es un pixy2?

[liskarra0]

Pon el código en "etiquetas de código", y los comentarios en español o inglés.

¿Que es un pixy2?

Ya he puesto el código con los comentarios en castellano, no me habia dado cuenta lo siento.

Pixy2 es una cámara que te permite obtener información de manera sencilla mediante vision artificial.

[ElEspanol]

Me he mirado por encima lo que es pixy2 cam.

Sin haber analizado la programación del pixy, diría que podría ser un conflicto con los timer que usa el pulseIn().

[liskarra0]

Tiene sentido, porque el dato recogido de esa función es 0. Y no deberia. Como se puede contar el tiempo de otra forma?

[surbyte]

timers de pulsein? Que yo sepa usa millis() entre flancos o estados de un pin o peor ciclos de reloj de instrucciones determinadas.

Este es el pulsein del IDE 1.0 ha pasado mucha agua bajo el puente pero todos los comentarios indican que trabaja mas o menos igual, no usa timers ni millis() como yo suponía

Code: [Select]

/*
  wiring_pulse.c - pulseIn() function
  Part of Arduino - http://www.arduino.cc/

  Copyright (c) 2005-2006 David A. Mellis

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with this library; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA

  $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $
*/

#include "wiring_private.h"
#include "pins_arduino.h"

/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
* or LOW, the type of pulse to measure.  Works on pulses from 2-3 microseconds
* to 3 minutes in length, but must be called at least a few dozen microseconds
* before the start of the pulse. */
unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout)
{
 // cache the port and bit of the pin in order to speed up the
 // pulse width measuring loop and achieve finer resolution.  calling
 // digitalRead() instead yields much coarser resolution.
 uint8_t bit = digitalPinToBitMask(pin);
 uint8_t port = digitalPinToPort(pin);
 uint8_t stateMask = (state ? bit : 0);
 unsigned long width = 0; // keep initialization out of time critical area
 
 // convert the timeout from microseconds to a number of times through
 // the initial loop; it takes 16 clock cycles per iteration.
 unsigned long numloops = 0;
 unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;
 
 // wait for any previous pulse to end
 while ((*portInputRegister(port) & bit) == stateMask)
 if (numloops++ == maxloops)
 return 0;
 
 // wait for the pulse to start
 while ((*portInputRegister(port) & bit) != stateMask)
 if (numloops++ == maxloops)
 return 0;
 
 // wait for the pulse to stop
 while ((*portInputRegister(port) & bit) == stateMask) {
 if (numloops++ == maxloops)
 return 0;
 width++;
 }

 // convert the reading to microseconds. The loop has been determined
 // to be 20 clock cycles long and have about 16 clocks between the edge
 // and the start of the loop. There will be some error introduced by
 // the interrupt handlers.
 return clockCyclesToMicroseconds(width * 21 + 16);
}

O sea es algo poco preciso.
Usas micros() y tendras mejor precisión ni hablar con Timers.

[liskarra0]

Entonces me recomiendas quitar todos los delay() del codigo como digiste antes verdad?

Con el tema del pulsein? Recomiendas quitarlo y poner otra opcion?

Muchas gracias

[surbyte]

Tienes para mi mucho trabajo
Empieza con pixy que ya esta y no tienes mucha opción.
Okay pixy llama a 1 de 3 rutinas

Yo comencé con tu código y luego vi que es para un buen rato y lo dejé

esto hice

Code: [Select]


void loop() {
  Serial.println("ITXAROTEN");
  pixy.ccc.getBlocks();     //Lee los datos de la cámara
 
  if (pixy.ccc.numBlocks) { // Averigua si se ha detectado algo
      while(pixy.ccc.blocks[0].m_signature) {
          case 1: Abat();  // Ir a un lugar
                  break;
          case 2: Abi();   Ir a dos lugares
                  break;
          case 3: digitalWrite(30,HIGH);
                  break; 
      }
  }
}

Ahora hay que trabajar con Abat() y Abi()

Haciendolos mas veloces.. cómo, usando millis() y creando máquina de estados.

[liskarra0]

Tu crees que haciendo Abat y Abi mas veloces la medicion del sonar ya no sera nula?

Porque antes con el mismo programa pero eligiendo el sitio al que ir a mano, sin la cámara, funcionaba bien, teniendo en cuenta todos los delays.

Yo creia que era mas la relación entre la Pixy2 y el sonar por eso pensaba que el tema del pulsein tendría sentido. Ya que la duración recibida del echo es 0.