OK. That was fun, but how does it work?
The Pixy Robot code consists of two main control systems: Object Tracking with the Pixy Camera and the pan/tilt mechanism and Object Following with the Zumo robot base.
Together these two systems produce a very natural looking response where the 'head' turns in response to motion and the 'body' follows.
Both control systems are based on Feedback Control Loops. For a detailed explanation of how Feedback Control works, see the Feedback Control Basics page in this guide.
Tracking Objects
Object tracking is implemented in the TrackBlock function. The hard work of object detection and location is handled by the image processing system inside the Pixy camera. It analyzes the image and identifies objects matching the color characteristics of the object being tracked. It then reports the position size and colors of all the detected objects back to the Arduino.In the Arduino, we use this information to adjust the pan and tilt servos to try to keep the tracked object in the center of the field of view.
//---------------------------------------
// 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;
}
The Pan/Tilt control is implemented using 2 instances of the ServoLoop class - one for the pan and one for the tilt. ServoLoop is a feedback control loop using both Proportional + Derivative (PD) control. The measurements are the x (for pan) and y (for tilt) positions of the blocks reported by the Pixy Camera. The setpoints are the x, y position of the center of the camera's view. And the outputs are the servo positions.
On each pass through the main loop, we calculate the errors for the pan and tilt controls as the difference between the measurements and the setpoints. Then we invoke the ServoLoop control algorithms to calculate the outputs.
On each pass through the main loop, we calculate the errors for the pan and tilt controls as the difference between the measurements and the setpoints. Then we invoke the ServoLoop control algorithms to calculate the outputs.
//---------------------------------------
// 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
//---------------------------------------
Following Objects
The object following behavior is implemented in the FollowBlock function. FollowBlock uses just proportional control. But we have two measurements (size and pan position) and two outputs (left and right drive motors).The size (block height times width) gives us a rough idea of how far away the object is and we use that to calculate the 'forwardSpeed'. This makes the robot slow down as it approaches the object. If the object appears larger than the setpoint value, forwardSpeed will become negative and the robot will back up.
//---------------------------------------
// 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 = 400;
void FollowBlock(int trackedBlock)
{
int32_t followError = RCS_CENTER_POS - panLoop.m_pos; // How far off-center are we looking now?
// 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;
// Forward speed decreases as we approach the object (size is larger)
int forwardSpeed = constrain(400 - (size/256), -100, 400);
// Steering differential is proportional to the error times the forward speed
int32_t differential = (followError + (followError * forwardSpeed))>>8;
// Adjust the left and right speeds by the steering differential.
int leftSpeed = constrain(forwardSpeed + differential, -400, 400);
int rightSpeed = constrain(forwardSpeed - differential, -400, 400);
// And set the motor speeds
motors.setLeftSpeed(leftSpeed);
motors.setRightSpeed(rightSpeed);
}
The pan position (one of the outputs of the tracking control) tells us how far the head is turned away from the setpoint (straight-ahead). This value is used to control the speed differential between the left and right motors - causing the robot to turn toward the object it is following.
