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);
}
What motor platform are using? If you can post a short video of it working, that would be great!