Pixy Robot

El siguiente proyecto es una demostración de cómo es posible interactuar con un robot que reconoce diversos objetos, al mismo tiempo, a través de una cámara de video. Adicionalmente, he programado el microcontrolador para que el robot siga manteniendo la distancia. Con esta cámara (PixyCam), es posible determinar posición exacta del objeto, ángulo del objeto respecto a la posición de la camara, colores, tamaño del objeto, entre otras cualidades interesantes.

The next project is a demonstration of how you can interact with a robot that recognizes various objects at the same time, through a video camera. Additionally, I have programmed the microcontroller for the robot to continue to keep the distance. With this camera (PixyCam), it is possible to determine exact position of the object, angle of the object relative to the position of the camera, color, object size, among other interesting qualities.

Programa para cargar en el Arduino:

Program to load into the Arduino Nano and the Arduino UNO, can be displayed in the next window:


#include <SPI.h>
#include <Pixy.h>
#include <Servo.h>

// Declare the Servo pin

int servoPin1 = 2;
int servoPin2 = 3;
int Xpositie;
int Ypositie;
int Wpositie;
int Hpositie;
int pan;
int tilt;

// Create a servo object

Servo Servo1;
Servo Servo2;


Pixy pixy;

#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
#define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)

class ServoLoop
{
public:
ServoLoop(int32_t pgain, int32_t dgain);

void update(int32_t error);

int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;
};


ServoLoop panLoop(300, 500);
ServoLoop tiltLoop(500, 700);

ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
m_pos = PIXY_RCS_CENTER_POS;
m_pgain = pgain;
m_dgain = dgain;
m_prevError = 0x80000000L;
}

void ServoLoop::update(int32_t error)
{
long int vel;
char buf[32];
if (m_prevError!=0x80000000)
{
vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
//sprintf(buf, "%ld\n", vel);
//Serial.print(buf);
m_pos += vel;
if (m_pos>PIXY_RCS_MAX_POS)
m_pos = PIXY_RCS_MAX_POS;
else if (m_pos<PIXY_RCS_MIN_POS)
m_pos = PIXY_RCS_MIN_POS;
}
m_prevError = error;
}



void setup()
{
Serial.begin(9600);
Serial.print("Starting...\n");

pixy.init();

// We need to attach the servo to the used pin number
Servo1.attach(servoPin1);
Servo2.attach(servoPin2);

// Make servo go to 90 degrees
Servo1.write(1483);
Servo2.write(1495);

}

void loop()
{
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
int32_t panError, tiltError;

blocks = pixy.getBlocks();

if (blocks)
{
panError = X_CENTER-pixy.blocks[0].x;
tiltError = pixy.blocks[0].y-Y_CENTER;

panLoop.update(panError);
tiltLoop.update(tiltError);

pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

i++;

// do this (print) every 50 frames because printing every
// frame would bog down the Arduino
if (i%50==0)
{
sprintf(buf, " Detected %d:\n", blocks);
Serial.print(buf);
for (j=0; j<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();


Xpositie=int(float(pixy.blocks[j].x));
Serial.print(" Pos X: ");
Serial.print(Xpositie);
delay(50);

Ypositie=int(float(pixy.blocks[j].y));
Serial.print(" Pos Y: ");
Serial.print(Ypositie);
delay(50);

Wpositie=int(float(pixy.blocks[j].width));
Serial.print(" Width: ");
Serial.print(Xpositie);
delay(50);

Hpositie=int(float(pixy.blocks[j].height));
Serial.print(" Height: ");
Serial.print(Ypositie);
delay(50);

pan=int(panLoop.m_pos);

// Condición para Avanzar
if (Hpositie < 30 & tiltLoop.m_pos < 800)
{
Serial.print(" Menor que... ");
Servo1.write(1333); //1403
Servo2.write(1645); //1575
}
else
{
Serial.print(" Mayor que... ");
Servo1.write(1483); // parado
Servo2.write(1495); // parado
}

// Condición para Retroceder

if (Hpositie > 80)
{
Servo1.write(1633); // 1575
Servo2.write(1345); // 1403
}

if (pan < 400)
{
Servo1.write(1433); // 1483
Servo2.write(1645); // 1575
}

if (pan > 700)
{
Servo1.write(1333); // 1403
Servo2.write(1545); // 1495 parado
}

Serial.print(" PanLoop Pos: ");
Serial.print(panLoop.m_pos);
Serial.print(" TiltLoop Pos: ");
Serial.print(tiltLoop.m_pos);
}
}
}
}

Autor: Ing. Christian Bodington Esteva.

 

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *