arduino codigo para pixy cam y robot
Páginas: 5 (1075 palabras)
Publicado: 14 de septiembre de 2015
#include // librería para enlasar comunicaciones
#include // librería para pixy cam
#define X_CENTER 160L // coordenadas en x para la torreta
#define Y_CENTER 100L // coordenadas en y para la torreta
#define RCS_MIN_POS 0L // tolerancia de error minima
#define RCS_MAX_POS 1000L // tolerancia de error maxima
#define RCS_CENTER_POS((RCS_MAX_POS-RCS_MIN_POS)/2) // promedi del centro de la torreta
class ServoLoop // declaracion de servos
{
public:
ServoLoop(int32_t pgain, int32_t dgain); // asignarganancias a los servomotores
void update(int32_t error); // error de la torreta y declaracion de variables para las ganancias de la torreta
int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;};
// control PID inicia codigo
ServoLoop panLoop(500, 800);
ServoLoop tiltLoop(700, 900);
ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
m_pos = 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>RCS_MAX_POS)
m_pos = RCS_MAX_POS;
else if (m_pos
//cprintf("%d %d %d\n", m_axis, m_pos, vel);
}
m_prevError = error;
}
// fin del codigo PID
Pixy pixy; // llama a la camara
//declaracion de variables para los datos que la camaraarroja
int signature = 0;
int x = 0; //positon x axis
int y = 0; //position y axis
int width = 0; //object's width
int height = 0; //object's height
int area = 0;
int maxArea = 1000; //max object area
int minArea = 800; //min object area
int Xmin = 140; //min x position
int Xmax = 180; //max x position
int aPhase = 2; //aPhaseon Pin D2
int aEnable = 3; //aEnable on Pin D3
int bPhase = 4; //bPhase on Pin D4
int bEnable = 5; //bEnable on Pin D5
//declaracion de las salidas a el arduino para los PWM
const int EN1 = 2; //pwm, enable motor 1
const int EN2 = 3; //pwm, enable motor 2
const int EN1I = 4; //pwm, enable motor 1
const int EN2I = 5; //pwm, enable motor 2
intSpeed = 250; //motor speed
//drive forward
void forward(){
analogWrite(EN1, Speed);
analogWrite(EN2, Speed);
}
//drive backward
void backward(){
analogWrite(EN1I, Speed);
analogWrite(EN2I, Speed);
}
//rotate right
void turnRight(){
analogWrite(EN1, Speed);
analogWrite(EN2I, Speed);
}
//rotate left
void turnLeft(){
analogWrite(EN1I, Speed);
analogWrite(EN2, Speed);
}//brake
void brake(){
analogWrite(EN1, 0);
analogWrite(EN2, 0);
analogWrite(EN1I, 0);
analogWrite(EN2I, 0);
}
void setup()
{
pinMode(EN1, OUTPUT);
pinMode(EN1I, OUTPUT);
pinMode(EN2, OUTPUT);
pinMode(EN2I, OUTPUT);
brake();
Serial.begin(9600);
Serial.print("Starting...\n");
pixy.init();
//start direction test - can be deleted
forward();
delay(1000);
brake();backward();
delay(1000);
brake();
turnLeft();
delay(1000);
brake();
turnRight();
delay(1000);
brake();
delay(1000);
//end direction test
}
//infinity loop
void loop()
{
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
int32_t panError, tiltError;
blocks = pixy.getBlocks(); //receive data from pixy
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++;
if (i%50==0)
{
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j=0; j
sprintf(buf, " block %d: ", j);
Serial.print(buf);...
#include
#define X_CENTER 160L // coordenadas en x para la torreta
#define Y_CENTER 100L // coordenadas en y para la torreta
#define RCS_MIN_POS 0L // tolerancia de error minima
#define RCS_MAX_POS 1000L // tolerancia de error maxima
#define RCS_CENTER_POS((RCS_MAX_POS-RCS_MIN_POS)/2) // promedi del centro de la torreta
class ServoLoop // declaracion de servos
{
public:
ServoLoop(int32_t pgain, int32_t dgain); // asignarganancias a los servomotores
void update(int32_t error); // error de la torreta y declaracion de variables para las ganancias de la torreta
int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;};
// control PID inicia codigo
ServoLoop panLoop(500, 800);
ServoLoop tiltLoop(700, 900);
ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
m_pos = 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>RCS_MAX_POS)
m_pos = RCS_MAX_POS;
else if (m_pos
//cprintf("%d %d %d\n", m_axis, m_pos, vel);
}
m_prevError = error;
}
// fin del codigo PID
Pixy pixy; // llama a la camara
//declaracion de variables para los datos que la camaraarroja
int signature = 0;
int x = 0; //positon x axis
int y = 0; //position y axis
int width = 0; //object's width
int height = 0; //object's height
int area = 0;
int maxArea = 1000; //max object area
int minArea = 800; //min object area
int Xmin = 140; //min x position
int Xmax = 180; //max x position
int aPhase = 2; //aPhaseon Pin D2
int aEnable = 3; //aEnable on Pin D3
int bPhase = 4; //bPhase on Pin D4
int bEnable = 5; //bEnable on Pin D5
//declaracion de las salidas a el arduino para los PWM
const int EN1 = 2; //pwm, enable motor 1
const int EN2 = 3; //pwm, enable motor 2
const int EN1I = 4; //pwm, enable motor 1
const int EN2I = 5; //pwm, enable motor 2
intSpeed = 250; //motor speed
//drive forward
void forward(){
analogWrite(EN1, Speed);
analogWrite(EN2, Speed);
}
//drive backward
void backward(){
analogWrite(EN1I, Speed);
analogWrite(EN2I, Speed);
}
//rotate right
void turnRight(){
analogWrite(EN1, Speed);
analogWrite(EN2I, Speed);
}
//rotate left
void turnLeft(){
analogWrite(EN1I, Speed);
analogWrite(EN2, Speed);
}//brake
void brake(){
analogWrite(EN1, 0);
analogWrite(EN2, 0);
analogWrite(EN1I, 0);
analogWrite(EN2I, 0);
}
void setup()
{
pinMode(EN1, OUTPUT);
pinMode(EN1I, OUTPUT);
pinMode(EN2, OUTPUT);
pinMode(EN2I, OUTPUT);
brake();
Serial.begin(9600);
Serial.print("Starting...\n");
pixy.init();
//start direction test - can be deleted
forward();
delay(1000);
brake();backward();
delay(1000);
brake();
turnLeft();
delay(1000);
brake();
turnRight();
delay(1000);
brake();
delay(1000);
//end direction test
}
//infinity loop
void loop()
{
static int i = 0;
int j;
uint16_t blocks;
char buf[32];
int32_t panError, tiltError;
blocks = pixy.getBlocks(); //receive data from pixy
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++;
if (i%50==0)
{
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j=0; j
sprintf(buf, " block %d: ", j);
Serial.print(buf);...
Leer documento completo
Regístrate para leer el documento completo.