private ObjectBlock getBlock()
{
byte[] readData = getBytes(10);
if (readData != null)
{
ObjectBlock block = new ObjectBlock();
block.Signature = BitConverter.ToUInt16(readData, 0);
block.X = BitConverter.ToUInt16(readData, 2);
block.Y = BitConverter.ToUInt16(readData, 4);
block.Width = BitConverter.ToUInt16(readData, 6);
block.Height = BitConverter.ToUInt16(readData, 8);
return block;
}
return null;
}
// 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
private void followBlock(ObjectBlock trackedBlock)
{
long followError = RCS_CENTER_POS - panLoop.Position;
// Size is the area of the object
// We keep a running average of the last 8
size += trackedBlock.Width * trackedBlock.Height;
size -= size >> 3;
// Forward speed decreases as we approach the object (size is larger)
int forwardSpeed = Constrain(400 - ((int)size / 256), -100, 400);
// Steering differential is proportional to the error times the forward speed
long differential = (followError + (followError * forwardSpeed)) >> 8;
// Adjust the left and right speeds by the steering differential
int leftSpeed = Constrain((int)(forwardSpeed + differential), -400, 400);
int rightSpeed = Constrain((int)(forwardSpeed - differential), -400, 400);
// TODO: Set the motor speeds
}
// Display what the camera is seeing on the page
private async void updateUI(ObjectBlock block)
{
await Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Normal, () =>
{
trackedBlockRect.Visibility = Visibility.Visible;
trackedBlockRect.Width = block.Width;
trackedBlockRect.Height = block.Height;
double xRatio = (block.X - ((double)block.Width / 2)) / PIXY_X_MAX;
double yRatio = (block.Y - ((double)block.Height / 2)) / PIXY_Y_MAX;
Canvas.SetLeft(trackedBlockRect, xRatio * canvas.Width);
Canvas.SetTop(trackedBlockRect, yRatio * canvas.Height);
outputTextBlock.Text = block.ToString();
});
}
// Track blocks via the Pixy pan/tilt mechanism
private ObjectBlock trackBlock(ObjectBlock[] blocks)
{
ObjectBlock trackedBlock = null;
long maxSize = 0;
foreach(ObjectBlock block in blocks)
{
if(oldBlock == null || (block.Signature == oldBlock.Signature))
{
long newSize = block.Height * block.Width;
if(newSize > maxSize)
{
trackedBlock = block;
maxSize = newSize;
}
}
}
long panError = X_CENTER - trackedBlock.X;
long tiltError = trackedBlock.Y - Y_CENTER;
panLoop.Update(panError);
tiltLoop.Update(tiltError);
pixyCam.SetServos(panLoop.Position, tiltLoop.Position);
oldBlock = trackedBlock;
return trackedBlock;
}
