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; }