Manualshelf

Datasheet

ManualsBrandsADAFRUIT ManualsDev KitsFitting aid
31323334353637383940
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.
//---------------------------------------
// 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;
}