// 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();
});
}
// 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();
});
}
// 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)
{
Debug.WriteLine(
string.Format(
"Follow: {0}",
trackedBlock.ToString()));
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;
const int MinSpeed = -400;
const int MaxSpeed = 400;
// Forward speed decreases as we approach the object (size is larger)
int forwardSpeed = Constrain(400 - ((int)size / 256), MinSpeed / 2, MaxSpeed);
// 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), MinSpeed, MaxSpeed);
int rightSpeed = Constrain((int)(forwardSpeed - differential), MinSpeed, MaxSpeed);
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;
const float PowerScaleDown = 0.75f;
leftPower = (Math.Abs(leftSpeed) / (float)MaxSpeed) * PowerScaleDown;
rightPower = (Math.Abs(rightSpeed) / (float)MaxSpeed) * PowerScaleDown;
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);
}