Lectura de sonar HC-SR04

ElEspanol: Posicionar el servo antes de medir distancia esta mal hecho.

Primero el delay(50) a lo mejor no es suficiente para llegar a la posición correcta. Y el delay por si esta mal. Nunca uses delay, solo existen pocas excepciones. Tu caso es ninguna de ellas.

Hay una librería que se llama mobatools.h, esta muchísimo mejor que la original servos.h En esta puedes enterarte si el servo aún se mueve. Y hacer la medición de distancia cuando ya no se mueve el servo.

Okey, gracias. Intentare probar esto que me dices. Me parece un punto muy importante a tener en cuenta.

Igualmente no tiene nada que ver el uso de Servos con que tu ultrasonico no reporte la lectura.

Buenas,

He probado a usar la libreria NewPing para la lectura del sonar, y al igual que pasaba antes, sin camara todo va genial, pero con ella la lectura del sonar es 0.

He depurado el codigo como me lo habeis recomendado. A ver si asi podeis ayudarme a saber que pasa.

#include 
#include 
#include 
#include 


// 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); // Aurreko ezkerreko gurpila
AF_DCMotor auresk(3,MOTOR34_64KHZ); // Aurreko eskumako gurpila
AF_DCMotor atzesk(1,MOTOR12_64KHZ); // Atzeko ezkerreko gurpila
AF_DCMotor atzezk(2,MOTOR12_64KHZ); // Atzeko eskumako gurpila
Servo servoMotor;
NewPing sonar(trigPin,echoPin,500);
Pixy2 pixy; // Pixy objetuaren deklarazioa



void setup() 
{
           Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.
           pinMode(trigPin, OUTPUT);
           pinMode(echoPin, INPUT); 
           pinMode(led, OUTPUT);
           pinMode(52,OUTPUT);
           digitalWrite(52,HIGH);
           attachInterrupt(2, interrupcion, FALLING);
           servoMotor.attach(10);      
           pixy.init(); // Kamera hasierarazi  
}

void loop() 
{
  Serial.println("ITXAROTEN");
  pixy.ccc.getBlocks(); //Leer datos de la cámara
  if(pixy.ccc.numBlocks) // Chequear si detecta algún objeto
  {
     
    if(pixy.ccc.blocks[0].m_signature==1)
    {
      Abat(); // Ir a A1
    }
    else if(pixy.ccc.blocks[0].m_signature==2)
    {
      Abi();   // Ir a A2
    }
    else if(pixy.ccc.blocks[0].m_signature==3)
    {
      digitalWrite(30,HIGH);
    }
    
  }
}


 void interrupcion() // Interrupcion del encoder para saber cuanta distancia anda
 {
  encoderPos++;
 }


void JATORRItikAP() // Subrutina para ir a A'
{
  
int angelua; // angulo
int aldaketa; // cambio
double minimoa; // distancia minima
long distancia; // distancia medida

abiadurak(100); // Poner velocidades
distancia=0;
encoderPos=0;
angelua=90;
aldaketa=-5;

itxaron(1000); // Esperar 1000 milisegundos

do
{
aurrerantza(); // Ir hacia delante

distancia=SONAR(angelua); // Medir distancia mediante sonar
minimoa=kalminimoa(angelua); // Calcular distancia minima para colision

Serial.print("Angelua: ");
Serial.print(angelua);
Serial.print(" Distantzia minimoa: ");
Serial.print(minimoa);
Serial.print(" Distantzia erreala: ");
Serial.println(distancia);

  if(distancia90)
{
radianetan=angelua*(2*PI/360); 
minimo=(-7.5/cos(radianetan))+1; 

}
if (angelua==90)
{
  minimo=25; // Cos de 90º es 0 por eso se establece a mano
}

// Si la distancia minima da muy alta se limita a 25
if (minimo>25)
{
  minimo=25;
}

  return minimo; //Distantzia minimoa itzuli.
}


void abiadurak(int v) //Subrutina para establecer velocidades
{
  atzezk.setSpeed(v); 
  atzesk.setSpeed(v);
  aurezk.setSpeed(v);
  auresk.setSpeed(v);
}

void gelditu() //Subrutina para parar
{
  atzezk.run(RELEASE);
  atzesk.run(RELEASE);
  auresk.run(RELEASE);
  aurezk.run(RELEASE);
}

void aurrerantza() //Subrutina para avanzar
{
  atzezk.run(FORWARD); // Aurrerantza
  atzesk.run(FORWARD);
  auresk.run(FORWARD);
  aurezk.run(FORWARD);
}

void itxaron(int t) // Subrutina para esperar, simular un delay()
{
  unsigned long A=0;
  unsigned long B=0;
  B=millis();
  do{
    A=millis();
  }while(A

Gracias, porque eso quiere decir que todo el trabajo que me tomé rehaciendo tu código y hasta traduciendolo no sirvió de nada ni para que lo leas.

Vaya que desilusión.

Perdona pero si he leído tu código, y analizado cada cosa que has hecho ahí. Pero una vez analizado tu código he intentado hacerlo por mi mismo también y entender cada cosa que ponía.

Si te fijas son prácticamente identicos excepto la función de newping que yo utilizo una que directamente da la medida en cm. También he probado la que tu utilizas pero también da error.

Ademas le he añadido una función que hace de delay() implementada por millis() para poder esperar sin bloquear el procesador.

Pero en todo momento he dejado de valorar tu trabajo y esfuerzo. Y lo he leído y apreciado mucho cada cosa que has hecho y te lo agradezco.

Una vez dicho esto, se os ocurre que puede pasar? Que es lo que esta interfiriendo en las mediciones del sonar si no es el pulsein?

Solo para agregar algo de matemática: en arduino.h está PI 3.1415926535897932384626433832795

Por otro lado, yo hago lescturas del HC-SR04 con esto y va muy bien: (Es mi código para probarlo cuando lo sueldo, conecto o desconecto en varios proyectos)

#define trig  0
#define echo  1
float distancia;
int tiempo;
int lectura;
void setup() {
  pinMode (trig, OUTPUT);
  pinMode(echo, INPUT);
}
void loop() {
  digitalWrite(trig, HIGH);
  delay(1);
  digitalWrite(trig, LOW);
  tiempo = pulseIn(echo, HIGH);
  distancia = tiempo / 58.2;
  Serial.println(distancia);
}

Intenta probar primero el sensor y no todo al mismo tiempo

Ya lo ha hecho @robertoxyz20, y lo dice en varias oportunidades que le funciona bien pero cuando agrega la cámara Pixy deja de funcionar.

Como ha mencionado surbyte, esa era mi principal forma de programar el sonar. Y la medicion del sonar era correcta hasta que incorporaba la Pixy2.

Mañana voy a intentar poner el pulsein a traves de interrupciónes de cambio de pin a ver si asi consigo obtener una medida del sonar.

Mañana voy a intentar poner el pulsein a traves de interrupciónes de cambio de pin a ver si asi consigo obtener una medida del sonar.

No hagas eso porque es simplemente una barbaridad. Ya de por si pulsein te hemos dicho que es una mala rutina y lo hemos fundamentado.

Usar pulsein en una interrupción no se puede. Como desconoces la idea de para qué sirve una interrupción encuentras algo en Internet y quieres probarlo. No es el camino.

Creo haber encontrado tu problema. No se porque funciona sin la cámara pero con la cámara se detiene. Tal vez sea un problema de tiempos como te dije al comienzo.

MIra esto Usas long distancia en cada rutina y siempre lo pones a 0. Pues quita ese 0 y deja de redefinir a cada momento distancia en cada rutina porque lo tienes definido como global.

Además para mi distancia debería ser la misma porque tienes un solo sensor, solo lo actualizas. Entonces definelo como global (antes del setup), quita todos los distancia = 0 y espera a sus consultas cuando pregunta al HC-SR04

Creo que me he explicado mal, mi intención no era meter el pulseIn dentro de una interrupción, era intentar recrear el funcionamiento con una interrupción de cambio de estado del pin.

Es decir con la interrupción del cambio de estado del pin medir el tiempo entre los cambios de subida y bajada y asi calcular la duración.

Se perfectamente lo que es una interrupción y para que sirve, y todos los codigos y ideas que cojo por Internet como dices, los analizo e intento enterdelos y adaptarlos a mi programa.

Mañana probare esto ultimo que me has dicho de la variable global y de no igualarla a 0. Gracias

He estado viendo que Pixy2 dispone de un cable entre el módulo y el Arduino. Ese cable se conecta al SPI, supongo que por defecto esta usando SPI o sea los pines 10, 11, 12 y 13. El pin 10 lo tienes conectado al servo si mal recuerdo. Cambialo a un pin que no sea estos que te he mencionado y debería funcionar todo. No es que no funcione el sonar o HC-SR04 sino que tu en el SONAR introduces el servo y ahi se produce el conflicto con la cámara. Tampoco falla el Sonar, sino que es el servo.

Se entiende.. pensé que era la variable pero ahora en tu modificación de mi código a medias que has usado (digo a medias porque no esta toda la traducción, ni la simplificación del loop) se ve claramente lo que digo.

Asi que cambia ese servoMotor.attach(10); por otro pin distinto a los que usa el SPI 10 a 13.

Buenos dias,

El cable de la pixy lo estoy conectando a a los pines ICSP del arduino Mega, el que esta al lado del boton reset. Ese es el SPI al que tu te refiere?

|500x329

Por otro lado, el servo lo tengo conectado a uno de los pines de servo del Adafruit motorShield, que en este caso, va conectado al pin 10. El otro conector va conectado al pin 9 asique probare ahi.

He probado a cambiar el pin del servo al 9 y tampoco ha dado resultado. El movimiento del servo lo hace bien, lo unico que el sonar devuelve la lectura 0.

Surbyte se refiere a un Uno, en tu Mega es diferente. No se ha dado cuenta que es un Mega2560

Visto que ninguna de las soluciones que han salido aquí han surgido efecto. He pensado en mirar el código de las librerías de Pixy2 a ver si ahí encontraba algo llamativo. He encontrado lo que creo que es el código de la función getblocks que utilizo en para adquirir la información desde la Pixy2.

template <class LinkType> int8_t Pixy2CCC<LinkType>::getBlocks(bool wait, uint8_t sigmap, uint8_t maxBlocks)
{
  blocks = NULL;
  numBlocks = 0;
  
  while(1)
  {
    // fill in request data
    m_pixy->m_bufPayload[0] = sigmap;
    m_pixy->m_bufPayload[1] = maxBlocks;
    m_pixy->m_length = 2;
    m_pixy->m_type = CCC_REQUEST_BLOCKS;
  
    // send request
    m_pixy->sendPacket();
    if (m_pixy->recvPacket()==0)
    {
      if (m_pixy->m_type==CCC_RESPONSE_BLOCKS)
      {
        blocks = (Block *)m_pixy->m_buf;
        numBlocks = m_pixy->m_length/sizeof(Block);
        return numBlocks;
      }
	  // deal with busy and program changing states from Pixy (we'll wait)
      else if (m_pixy->m_type==PIXY_TYPE_RESPONSE_ERROR)
      {
        if ((int8_t)m_pixy->m_buf[0]==PIXY_RESULT_BUSY)
        {
          if(!wait)
            return PIXY_RESULT_BUSY; // new data not available yet
		}
	    else if ((int8_t)m_pixy->m_buf[0]!=PIXY_RESULT_PROG_CHANGING)
          return m_pixy->m_buf[0];
      }
    }
    else
      return PIXY_RESULT_ERROR;  // some kind of bitstream error
  
    // If we're waiting for frame data, don't thrash Pixy with requests.
    // We can give up half a millisecond of latency (worst case)	
    delayMicroseconds(500);
  }
}

Todo esto estaba dentro una de las librerías de pixy2. En la Pixy2CCC concretamente. El código completo de esta es el siguiente:

//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//
// This file is for defining the Block struct and the Pixy template class version 2.
// (TPixy2).  TPixy takes a communication link as a template parameter so that 
// all communication modes (SPI, I2C and UART) can share the same code.  
//

#ifndef _PIXY2CCC_H
#define _PIXY2CCC_H

#define CCC_MAX_SIGNATURE                   7

#define CCC_RESPONSE_BLOCKS                 0x21
#define CCC_REQUEST_BLOCKS                  0x20

// Defines for sigmap:
// You can bitwise "or" these together to make a custom sigmap.
// For example if you're only interested in receiving blocks
// with signatures 1 and 5, you could use a sigmap of 
// PIXY_SIG1 | PIXY_SIG5
#define CCC_SIG1                     1 
#define CCC_SIG2                     2
#define CCC_SIG3                     4
#define CCC_SIG4                     8
#define CCC_SIG5                     16
#define CCC_SIG6                     32
#define CCC_SIG7                     64
#define CCC_COLOR_CODES              128

#define CCC_SIG_ALL                  0xff // all bits or'ed together

struct Block 
{
  // print block structure!
  void print()
  {
    int i, j;
    char buf[128], sig[6], d;
    bool flag;  
    if (m_signature>CCC_MAX_SIGNATURE) // color code! (CC)
    {
      // convert signature number to an octal string
      for (i=12, j=0, flag=false; i>=0; i-=3)
      {
        d = (m_signature>>i)&0x07;
        if (d>0 && !flag)
          flag = true;
        if (flag)
          sig[j++] = d + '0';
      }
      sig[j] = '\0';  
      sprintf(buf, "CC block sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle: %d index: %d age: %d", sig, m_signature, m_x, m_y, m_width, m_height, m_angle, m_index, m_age);
    }     
    else // regular block.  Note, angle is always zero, so no need to print
      sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d index: %d age: %d", m_signature, m_x, m_y, m_width, m_height, m_index, m_age);   
    Serial.println(buf); 
  }
  
  uint16_t m_signature;
  uint16_t m_x;
  uint16_t m_y;
  uint16_t m_width;
  uint16_t m_height;
  int16_t m_angle;
  uint8_t m_index;
  uint8_t m_age;
};

template <class LinkType> class TPixy2;

template <class LinkType> class Pixy2CCC
{
public:
  Pixy2CCC(TPixy2<LinkType> *pixy)
  {
    m_pixy = pixy;
  }
  
  int8_t getBlocks(bool wait=true, uint8_t sigmap=CCC_SIG_ALL, uint8_t maxBlocks=0xff);
  
  uint8_t numBlocks;
  Block *blocks;

private:
  TPixy2<LinkType> *m_pixy;
};

template <class LinkType> int8_t Pixy2CCC<LinkType>::getBlocks(bool wait, uint8_t sigmap, uint8_t maxBlocks)
{
  blocks = NULL;
  numBlocks = 0;
  
  while(1)
  {
    // fill in request data
    m_pixy->m_bufPayload[0] = sigmap;
    m_pixy->m_bufPayload[1] = maxBlocks;
    m_pixy->m_length = 2;
    m_pixy->m_type = CCC_REQUEST_BLOCKS;
  
    // send request
    m_pixy->sendPacket();
    if (m_pixy->recvPacket()==0)
    {
      if (m_pixy->m_type==CCC_RESPONSE_BLOCKS)
      {
        blocks = (Block *)m_pixy->m_buf;
        numBlocks = m_pixy->m_length/sizeof(Block);
        return numBlocks;
      }
	  // deal with busy and program changing states from Pixy (we'll wait)
      else if (m_pixy->m_type==PIXY_TYPE_RESPONSE_ERROR)
      {
        if ((int8_t)m_pixy->m_buf[0]==PIXY_RESULT_BUSY)
        {
          if(!wait)
            return PIXY_RESULT_BUSY; // new data not available yet
		}
	    else if ((int8_t)m_pixy->m_buf[0]!=PIXY_RESULT_PROG_CHANGING)
          return m_pixy->m_buf[0];
      }
    }
    else
      return PIXY_RESULT_ERROR;  // some kind of bitstream error
  
    // If we're waiting for frame data, don't thrash Pixy with requests.
    // We can give up half a millisecond of latency (worst case)	
    delayMicroseconds(500);
  }
}

#endif

No se si vosotros sois capaces de entender todo lo que aparece aquí, a mi me resulta muy complicado.

Por otra parte os adjunto la librería Pixy2.zip por si queréis intentar indagar ahí.

arduino_pixy2-1.0.3.zip (37.1 KB)

Buenas,

Al final resulto ser un problema de hardware. La conexión ICSP bloqueaba los pines del sonar que estaba utilizando y por eso la lectura del sonar daba 0.

Muchas gracias por vuestra tiempo e intentar ayudarme.

Al final resulto ser un problema de hardware. La conexión ICSP bloqueaba los pines del sonar que estaba utilizando y por eso la lectura del sonar daba 0.

Es exactamente lo que te puse.

Ese cable se conecta al SPI, supongo que por defecto esta usando SPI o sea los pines 10, 11, 12 y 13.

Mi sugerencia apuntó al UNO y usas un MEGA pero de todos modos por ahí esta el problema.

Ahora.. no dices como lo resolviste. Yo leo que fue un problema de hardware pero no dices donde pusiste el servo para que no te de problemas o las conexiones del sensor ultrasonido que cambiaste.

Busque los pines en comun entre los diferentes sistemas de arduino Mega y encontré una foto parecida a esta.

|346x500

Observe que los pines que yo usaba para el sonar (50,48,52) estaban compartidos con el ICSP.

Asique los puse en otros pines digitales que estaban libres y funciono.

Liskarra0:
Observe que los pines que yo usaba para el sonar (50,48,52) estaban compartidos con el ICSP.

Por eso siempre ayuda postear una diagrama de conexiones.

Como dice ElEspanol serian al menos 40 post menos.