I have more inquires about the pixy pet project.
As I mentioned earlier, I replaced the Zumo robot with two 2A motors and Arduino motor shield. I modified the code accordingly. It turns correctly to the right, but it does not go straight and it doesn't correctly turn to the left.
Code: Select all
#include <SPI.h>
#include <Pixy.h>
#include <ZumoMotors.h>
#define X_CENTER 160L
#define Y_CENTER 100L
#define RCS_MIN_POS 0L
#define RCS_MAX_POS 1000L
#define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)
//---------------------------------------
// Servo Loop Class
// A Proportional/Derivative feedback
// loop for pan/tilt servo tracking of
// blocks.
// (Based on Pixy CMUcam5 example code)
//---------------------------------------
class ServoLoop
{
public:
ServoLoop(int32_t proportionalGain, int32_t derivativeGain);
void update(int32_t error);
int32_t m_pos;
int32_t m_prevError;
int32_t m_proportionalGain;
int32_t m_derivativeGain;
};
// ServoLoop Constructor
ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)
{
m_pos = RCS_CENTER_POS;
m_proportionalGain = proportionalGain;
m_derivativeGain = derivativeGain;
m_prevError = 0x80000000L;
}
// ServoLoop Update
// Calculates new output based on the measured
// error and the current state.
void ServoLoop::update(int32_t error)
{
long int velocity;
char buf[32];
if (m_prevError!=0x80000000)
{
velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
m_pos += velocity;
if (m_pos>RCS_MAX_POS)
{
m_pos = RCS_MAX_POS;
}
else if (m_pos<RCS_MIN_POS)
{
m_pos = RCS_MIN_POS;
}
}
m_prevError = error;
}
// End Servo Loop Class
//---------------------------------------
Pixy pixy; // Declare the camera object
ServoLoop panLoop(200, 200); // Servo loop for pan
ServoLoop tiltLoop(150, 200); // Servo loop for tilt
ZumoMotors motors; // declare the motors on the zumo
//---------------------------------------
// Setup - runs once at startup
//---------------------------------------
void setup()
{
Serial.begin(9600);
Serial.print("Starting...\n");
pixy.init();
//Setup Channel A
pinMode(12, OUTPUT); //Initiates Motor Channel A pin
pinMode(9, OUTPUT); //Initiates Brake Channel A pin
pinMode(3,OUTPUT); ////Speed
//Setup Channel B
pinMode(13, OUTPUT); //Initiates Motor Channel B pin
pinMode(8, OUTPUT); //Initiates Brake Channel B pin
pinMode(11,OUTPUT); ////Speed
}
uint32_t lastBlockTime = 0;
//---------------------------------------
// Main loop - runs continuously after setup
//---------------------------------------
void loop()
{
uint16_t blocks;
blocks = pixy.getBlocks();
// If we have blocks in sight, track and follow them
if (blocks)
{
int trackedBlock = TrackBlock(blocks);
FollowBlock(trackedBlock);
lastBlockTime = millis();
}
else if (millis() - lastBlockTime > 100)
{
//motors.setLeftSpeed(0);
digitalWrite(13, LOW);digitalWrite(8, HIGH); analogWrite(11, 0); //forward direction B,no Brake, Spins motor at full speed (255)
//motors.setRightSpeed(0);
digitalWrite(12, LOW);digitalWrite(9, HIGH);analogWrite(3, 0); //forward direction A, no Brake, Spins motor at full speed (255)
ScanForBlocks(); //Serial.println("Scan for blocks");
}
}
int oldX, oldY, oldSignature;
//---------------------------------------
// Track blocks via the Pixy pan/tilt mech
// (based in part on Pixy CMUcam5 pantilt example)
//---------------------------------------
int TrackBlock(int blockCount)
{
int trackedBlock = 0;
long maxSize = 0;
Serial.print("blocks =");
Serial.println(blockCount);
for (int i = 0; i < blockCount; i++)
{
if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature))
{
long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
if (newSize > maxSize)
{
trackedBlock = i;
maxSize = newSize;
}
}
}
int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;
panLoop.update(panError);
tiltLoop.update(tiltError);
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
oldX = pixy.blocks[trackedBlock].x;
oldY = pixy.blocks[trackedBlock].y;
oldSignature = pixy.blocks[trackedBlock].signature;
return trackedBlock;
}
//---------------------------------------
// Follow blocks via the Zumo robot drive
//
// This code makes the robot base turn
// and move to follow the pan/tilt tracking
// of the head.
//---------------------------------------
int32_t size = 200;
void FollowBlock(int trackedBlock)
{
int32_t followError = RCS_CENTER_POS - panLoop.m_pos; // How far off-center are we looking now?
Serial.print("followError="); Serial.print(followError);
int leftSpeed = 50; int rightSpeed=50;
// Size is the area of the object.
// We keep a running average of the last 8.
size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
size -= size >> 3;
int forwardSpeed = constrain(255 - (size/100), 0, 255); //// 156, -255 to 255
int32_t differential = (followError + (followError * forwardSpeed))>>8;
Serial.print(followError);
//
if (followError >15)
{
Serial.print("followError="); Serial.print(followError); Serial.print("Turn left");
digitalWrite(12, LOW);digitalWrite(9, LOW); analogWrite(3,constrain(forwardSpeed + differential, 0, 75));
digitalWrite(13, LOW);digitalWrite(8, HIGH); analogWrite(11,0 );
delay(150);
digitalWrite(12, LOW);digitalWrite(9, HIGH); analogWrite(3,0 );
digitalWrite(13, LOW);digitalWrite(8, HIGH); analogWrite(11,0 );
delay(150);
}
if (followError <-15)
{
Serial.print("followError="); Serial.print(followError); Serial.print("turn right");
digitalWrite(12, LOW);digitalWrite(9, HIGH); analogWrite(3,0 );
digitalWrite(13, LOW);digitalWrite(8, LOW); analogWrite(11,constrain(forwardSpeed + differential, 0, 75));
delay(150);
digitalWrite(12, LOW);digitalWrite(9, HIGH); analogWrite(3,0 );
digitalWrite(13, LOW);digitalWrite(8, HIGH); analogWrite(11,0 );
delay(150);
}
if (followError >-15 && followError < 15)
{
Serial.print("followError="); Serial.print(followError); Serial.print("forward");
digitalWrite(12, LOW);digitalWrite(9, LOW); analogWrite(3, 80);
digitalWrite(13, LOW);digitalWrite(8, LOW); analogWrite(11,80);
}
}
//---------------------------------------
// Random search for blocks
//
// This code pans back and forth at random
// until a block is detected
//---------------------------------------
int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;
void ScanForBlocks()
{
if (millis() - lastMove > 20)
{
lastMove = millis();
panLoop.m_pos += scanIncrement;
if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS))
{
tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
scanIncrement = -scanIncrement;
if (scanIncrement < 0)
{
digitalWrite(12, HIGH);digitalWrite(9, HIGH); analogWrite(3, 0); //forward direction A, no Brake, Spins motor at full speed (255)
digitalWrite(13, HIGH);digitalWrite(8, HIGH); analogWrite(11,0); //forward direction B,no Brake, Spins motor at full speed (255)
}
else
{
digitalWrite(12, HIGH);digitalWrite(9, HIGH); analogWrite(3, 0);
digitalWrite(13, HIGH);digitalWrite(8, HIGH); analogWrite(11,0);
}
delay(random(250, 500));
}
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
}
}