<< return to Pixycam.com

Adapting the ccc.zumo.chace example for any motor controller


#1

Hello, this my firts menssage in this forum.
First of all I would like to apologize for my English. I´m spanish.
I have managed to adapt ccc.zumo.chace example de Pixy2 for any motor controller and I would like to share with you.
This is the arduino code.
#include <Pixy2.h>
#include <PIDLoop.h>
#include <AFMotor.h>

Pixy2 pixy;

//Definir los motores y su posición en las conexiones de shield
AF_DCMotor Motor1(1); // create motor #1, 64KHz pwm
AF_DCMotor Motor2(2); // create motor #2, 64KHz pwm
AF_DCMotor Motor3(3); // create motor #1, 64KHz pwm
AF_DCMotor Motor4(4); // create motor #2, 64KHz pwm

int Vel = 100; // Definimos la velocidad del robot
float P = 0.25; // Proporcion de giro

//límite de velocidad de los motores
#define MAX_TRANSLATE_VELOCITY 250

PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);

void setup()
{
// Debug console
Serial.begin(9600);
Serial.print(“Starting…\n”);

//Motores parados
Paro();

//iniciar pixy
pixy.init();
//usar el programa de colores
pixy.changeProg(“color_connected_componetns”);
}

// Toma el bloque más grande (bloques [0]) que ha existido durante al menos 30 cuadros (1/2 segundo)
// y devuelve su índice, de lo contrario devuelve -1
int16_t acquireBlock()
{
if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age>30)
return pixy.ccc.blocks[0].m_index;

return -1;
}

// Encuentra el bloque con el índice dado. En otras palabras, encontrar el mismo objeto en el actual
// marco - no es el objeto más grande, pero lo guardaremos en acquireBlock ()
// Si no está en el marco actual, devuelve NULL

Block *trackBlock(uint8_t index)
{
uint8_t i;

for (i=0; i<pixy.ccc.numBlocks; i++)
{
if (index==pixy.ccc.blocks[i].m_index)
return &pixy.ccc.blocks[i];
}

return NULL;
}

void loop()
{

static int16_t index = -1;
int32_t panOffset, tiltOffset, headingOffset, right, left;

Block *block=NULL;

pixy.ccc.getBlocks();

if (index==-1) // Buscar…
{
Serial.println(“buscando bloque…”);
index = acquireBlock();
if (index>=0)
Serial.println(“Bloque encontrado!”);
}

// Si hemos encontrado un bloque, encuéntralo, síguelo.
if (index>=0)
block = trackBlock(index);

// Si somos capaces de seguirlo, mover motores
if (block)
{
// calcula los errores de pan y tilt
panOffset = (int32_t)pixy.frameWidth/2 - (int32_t)block->m_x;
tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight/2;

// calcula como se mueven los servos pan y tilt
panLoop.update(panOffset);
tiltLoop.update(tiltOffset);

// mover los servos
pixy.setServos(panLoop.m_command, tiltLoop.m_command);

// calcula los errores de translate and rotate 
panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS/2 + PIXY_RCS_CENTER_POS/8;

rotateLoop.update(panOffset);
translateLoop.update(-tiltOffset);

// guarda la velocidad de translation máxima permitida
if (translateLoop.m_command>MAX_TRANSLATE_VELOCITY)
  translateLoop.m_command = MAX_TRANSLATE_VELOCITY;

// calcular las velocidades de las ruedas izquierda y derecha en función de las velocidades de rotación y de traslación de los servos
 left = -rotateLoop.m_command + translateLoop.m_command;
 right = rotateLoop.m_command + translateLoop.m_command;
 Serial.println(left);
 Serial.println(right);

// El robot se mueve en función de las velocidades left y right.            
      if (left>0 && right>0) 
       {
        Avance();
       }
       else if (left<0 && right<0)
       {
       Retroceso();
 }
 else if (left<right && left<0 && right>0)
       {
        RotarIzquierda();
       }
  else if (left>right && left>0 && right<0) {
        RotarDerecha();
 }

// imprime el bloque que estamos persiguiendo -- esperar hasta el final del bucle para reducir la latencia
block->print();

}

else // si no detecta bloques, para motores, y regresa al estado de búsqueda
{
rotateLoop.reset();
translateLoop.reset();
Paro();
index = -1; // set search state
}

}

void SetVel(int v1, int v2, int v3, int v4)
{ Motor1.setSpeed(v1);
Motor2.setSpeed(v2);
Motor3.setSpeed(v3);
Motor4.setSpeed(v4);
}

void Avance()
{ SetVel(Vel,Vel,Vel,Vel); // Misma velocidad a las 4 ruedas
Motor1.run(FORWARD);
Motor2.run(FORWARD);
Motor3.run(FORWARD);
Motor4.run(FORWARD);
//digitalWrite(48, LOW);
}

void Retroceso()
{ SetVel(Vel,Vel,Vel,Vel); // Misma velocidad a las 4 ruedas
Motor1.run(BACKWARD);
Motor2.run(BACKWARD);
Motor3.run(BACKWARD);
Motor4.run(BACKWARD);
//digitalWrite(48, HIGH);
}

void Paro()
{ Motor1.run(RELEASE);
Motor2.run(RELEASE);
Motor3.run(RELEASE);
Motor4.run(RELEASE);
//digitalWrite(48, LOW);
}
void giroIzquierdaAvance() //Reducimos la velocidad de los motores 1 y 3 en una proporción de 1/4 (P=0.25) para girar a la izquierda mientras va hacia adelante
{
int v = Vel * P ;
SetVel( v, Vel, Vel, v);

 Motor1.run(FORWARD);
 Motor2.run(FORWARD);
 Motor3.run(FORWARD);
 Motor4.run(FORWARD);
 //digitalWrite(48, LOW);

}
void giroIzquierdaAtras() //Reducimos la velocidad de los motores 1 y 3 en una proporción de 1/4 (P=0.25) para girar a la izquierda mientras va hacia atrás
{
int v = Vel * P ;
SetVel( v, Vel, Vel, v);

 Motor1.run(BACKWARD);
 Motor2.run(BACKWARD);
 Motor3.run(BACKWARD);
 Motor4.run(BACKWARD);
 //digitalWrite(48, HIGH);

}
void giroDerechaAvance() //Reducimos la velocidad de los motores 2 y 4 en una proporción de 1/4 (P=0.25) para girar a la derecha mientras va hacia adelante
{
int v = Vel * P ;
SetVel( Vel, v, v, Vel);

  Motor1.run(FORWARD);
  Motor2.run(FORWARD);
  Motor3.run(FORWARD);
  Motor4.run(FORWARD);
  //digitalWrite(48, LOW);

}
void giroDerechaAtras() //Reducimos la velocidad de los motores 2 y 4 en una proporción de 1/4 (P=0.25) para girar a la derecha mientras va hacia atrás
{
int v = Vel * P ;
SetVel( Vel, v, v, Vel);

  Motor1.run(BACKWARD);
  Motor2.run(BACKWARD);
  Motor3.run(BACKWARD);
  Motor4.run(BACKWARD);
  //digitalWrite(48, HIGH);

}
void RotarIzquierda()
{
Motor1.run(BACKWARD);
Motor2.run(FORWARD);
Motor3.run(FORWARD);
Motor4.run(BACKWARD);
//digitalWrite(48, LOW);
}
void RotarDerecha()
{
Motor1.run(FORWARD);
Motor2.run(BACKWARD);
Motor3.run(BACKWARD);
Motor4.run(FORWARD);
//digitalWrite(48, LOW);
}


#2

Hello,
Thanks for sharing :slight_smile: What motor platform are using? If you can post a short video of it working, that would be great!

Edward


#3

Hello, borde.
I have used a motor shield V1 de adafruit, arduino mega y pixy2.
The robot chasis It’s done with 3D printer.
Attached a photo and video.

Thanks.


#4

Nice – thanks so much!