public List <ObjectBlock> GetBlocks(uint maxBlocks)
{
uint checksum, w;
List <ObjectBlock> objectBlocks = new List <ObjectBlock>();
if (!skipStart)
{
if (GetStart() == 0)
{
return(null);
}
}
else
{
skipStart = false;
}
for (int i = 0; i < maxBlocks;)
{
// After the sync sequence, the rest of the block is read in this order
checksum = getWord();
if (checksum == PIXY_START_WORD || checksum == PIXY_START_WORD_CC)
{
skipStart = true;
return(objectBlocks);
}
else if (checksum == 0)
{
return(objectBlocks);
}
ObjectBlock block = getBlock();
// The checksum should be the sum of all the other values
uint calculatedSum = block.Signature + block.X + block.Y + block.Width + block.Height;
// Make sure that the checksum that came with the block and our sum match
if (calculatedSum == checksum)
{
var args = new PixyCamEventArgs();
args.Block = block;
objectBlocks.Add(block);
i++;
}
w = getWord();
if (w == PIXY_START_WORD || w == PIXY_START_WORD_CC)
{
}
else
{
return(objectBlocks);
}
}
return(objectBlocks);
}
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);
}
// Display what the camera is seeing on the page
private async void updateUI(ObjectBlock block)
{
await Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Low, () =>
{
if (!blockDict.ContainsKey(block.Signature))
{
addNewRect(block);
}
blockDict[block.Signature].Visibility = Visibility.Visible;
blockDict[block.Signature].Width = block.Width;
blockDict[block.Signature].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(blockDict[block.Signature], xRatio * canvas.ActualWidth);
Canvas.SetTop(blockDict[block.Signature], yRatio * canvas.ActualHeight);
outputTextBlock.Text = block.ToString();
});
}
private void addNewRect(ObjectBlock block)
{
Rectangle rect = new Rectangle();
if(blockColors.Length >= block.Signature)
{
rect.Fill = new SolidColorBrush(blockColors[block.Signature - 1]);
}
else
{
rect.Fill = new SolidColorBrush(Colors.Gray);
}
rect.Width = block.Width;
rect.Height = block.Height;
canvas.Children.Add(rect);
blockDict.Add(block.Signature, rect);
}
// 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);
float leftPower, rightPower;
ZumoMotorDirection leftDir, rightDir;
if (leftSpeed >= 0)
leftDir = ZumoMotorDirection.Forward;
else
leftDir = ZumoMotorDirection.Backward;
if (rightSpeed >= 0)
rightDir = ZumoMotorDirection.Forward;
else
rightDir = ZumoMotorDirection.Backward;
leftPower = Math.Abs(leftSpeed) / 500.0f;
rightPower = Math.Abs(rightSpeed) / 500.0f;
Debug.WriteLine(
string.Format("followError={0}, size={1}, differential={2}, leftPower={3}, rightPower={4}",
followError,
size,
differential,
leftPower,
rightPower));
motorDriver.SetLeftMotorPower(leftDir, leftPower);
motorDriver.SetRightMotorPower(rightDir, rightPower);
}
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;
}
// 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();
});
}
// 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
}
// 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;
}
}
}
if (trackedBlock != null)
{
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;
}
