private void drawMallet(Point offset)
{
Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Normal, () =>
{
// Draw mallet location
var malletLoc = MotorHelper.GetCoordinatesFromOffset(offset);
malletLocationTextBlock.Text = "Mallet (X,Y): (" + malletLoc.X + ", " + malletLoc.Y + ")";
Canvas.SetLeft(mallet, malletLoc.X - mallet.Width / 2);
Canvas.SetTop(mallet, malletLoc.Y - mallet.Height / 2);
mallet.Visibility = Visibility.Visible;
});
}
// Draw the UI
private void runUIThread()
{
ThreadPool.RunAsync(async(s) =>
{
while (!stopThread)
{
var puckPosition = robot.AI.GetPuckPosition();
clearLines();
//drawDot(puckPosition);
if (puckPosition != CoordinateHelper.INVALID_POINT)
{
drawPuck(puckPosition);
}
drawMallet(robot.GetOffsets());
drawCenterOfMass(robot.AI.GetPuckCenterOfMass());
// Draw line from mallet to destination
drawLine(MotorHelper.GetCoordinatesFromOffset(robot.GetOffsets()),
MotorHelper.GetCoordinatesFromOffset(new Point(robot.StepperX.TargetPosition, robot.StepperY.TargetPosition)));
var bouncePts = robot.AI.GetPreviousBouncePoints();
if (bouncePts != null && bouncePts.Count > 0)
{
// Draw lines for bounces
drawBounces(puckPosition, bouncePts);
}
else if (robot.AI.GetPreviousTrajectory() != CoordinateHelper.INVALID_POINT)
{
// Draw line for trajectory
drawTrajectory(puckPosition, robot.AI.GetPreviousTrajectory());
}
setOutputText("Speed: " + robot.AI.GetPuckSpeed());
await Task.Delay(1);
}
});
}
// Decision-making function; given puck position, decide where we should net move the mallet
public Point calculateMalletTarget(Point currentPuckPosition, long currentTime)
{
puckPosition = currentPuckPosition;
Move = MoveType.Straight;
addToHistory(currentPuckPosition, currentTime);
if (positionHistory.Count < 2)
{
return(CoordinateHelper.INVALID_POINT);
}
// Set the default defense position
defenseOffset = new Point(Config.MAX_MALLET_OFFSET_X / 2, 0);
// Calculate center of mass for trajectory calculations
// Get the previous center of mass position
Point prevCenterOfMass = com.Position;
// Add the new puck position to the queue for calculations
com.SafeEnqueue(currentPuckPosition);
// Calculate the new center of mass
Point centerOfMass = com.Calculate();
calculateLine(com.PointQueue.ToArray());
// If we don't have the previous center of mass, we can't do trajectory calculations, so skip this
if (prevCenterOfMass == CoordinateHelper.INVALID_POINT)
{
return(CoordinateHelper.INVALID_POINT);
}
// For speed calculations
var prevPosition = positionHistory[positionHistory.Count - 2];
var currPosition = positionHistory.Last();
// Convert everything to positions (coordinates in the coordinate plane) or offsets (steps for motors)
Point malletTargetOffset = CoordinateHelper.INVALID_POINT;
// Mallet offset
Point currentMalletOffset = new Point(robot.StepperX.CurrentPosition(), robot.StepperY.CurrentPosition());
// Mallet target in steps
Point currentMalletTargetOffset = new Point(robot.StepperX.TargetPosition(), robot.StepperY.TargetPosition());
// Mallet position coordinates
Point currentMalletPosition = MotorHelper.GetCoordinatesFromOffset(currentMalletOffset);
// Mallet target coordinates
Point currentMalletTargetPosition = MotorHelper.GetCoordinatesFromOffset(currentMalletTargetOffset);
// Distance from previous point
Vector distanceFromPrevious = Helper.GetDistance(prevCenterOfMass, centerOfMass);
speed = distanceFromPrevious.H;
// Puck distance from mallet (coordinates)
Vector distanceFromMallet = Helper.GetDistance(currentPuckPosition, currentMalletPosition);
Point trajectoryPoint = CoordinateHelper.INVALID_POINT, bouncePoint = CoordinateHelper.INVALID_POINT;
// List of points where we think the puck will bounce
List <Point> bouncePoints = new List <Point>();
// If this flag has been set, it means we're in the middle of a non-interruptable move
if (DoNotInterrupt)
{
return(CoordinateHelper.INVALID_POINT);
}
// The mallet and puck have probably collided, don't do anything yet
if (Helper.GetDistance(currentMalletPosition, currentPuckPosition).H < 40)
{
return(CoordinateHelper.INVALID_POINT);
}
// Puck is behind the mallet
if (distanceFromMallet.X < 0)
{
// Defensive position in front of goal
malletTargetOffset = defenseOffset;
// Don't interrupt this move until we get to the destination
DoNotInterrupt = true;
}
// Puck is moving away
else if (prevCenterOfMass.X > centerOfMass.X)
{
// Defensive position in front of goal
malletTargetOffset = defenseOffset;
}
else if (distanceFromPrevious.H < 5 && currentPuckPosition.X > Config.TABLE_MID_X_COORDINATE && currentMalletOffset == defenseOffset)
{
if (distanceFromMallet.H < 400)
{
// Get new mallet position
var targetPoint = CoordinateHelper.CalculateTrajectoryPoint(currentMalletPosition, currentPuckPosition, currentPuckPosition.X - 100);
malletTargetOffset = MotorHelper.GetOffsetFromCoordinates(targetPoint);
DoNotInterrupt = true;
}
}
// Puck is moving towards mallet
else if (prevCenterOfMass.X < centerOfMass.X)
{
trajectoryPoint = CoordinateHelper.CalculateTrajectoryPoint(prevCenterOfMass, centerOfMass, currentMalletPosition.X);
if (trajectoryPoint == CoordinateHelper.INVALID_POINT)
{
malletTargetOffset = CoordinateHelper.INVALID_POINT;
}
// We can block the puck from our current location (no Y movement)
else if (trajectoryPoint.X == currentMalletPosition.X)
{
// We're already at the point
if (Math.Abs(trajectoryPoint.Y - currentMalletPosition.Y) < 50)// && currentPuckPosition.X > Config.TABLE_MID_X_COORDINATE)//Helper.GetDistance(currentPuckPosition, currentMalletPosition).H < 400)
{
var targetPoint = CoordinateHelper.CalculateTrajectoryPoint(currentMalletPosition, currentPuckPosition, currentPuckPosition.X - 100);
malletTargetOffset = MotorHelper.GetOffsetFromCoordinates(targetPoint);
DoNotInterrupt = true;
}
else
{
malletTargetOffset = new Point(MotorHelper.GetOffsetXFromCoordinateY(trajectoryPoint.Y), currentMalletOffset.Y);
}
}
else if (trajectoryPoint.X != currentMalletPosition.X)
{
if (speed > 15)
{
double angleAdjust = 1;
double adjustment = .1;
if (trajectoryPoint.X != currentMalletPosition.X)
{
bouncePoints.Add(trajectoryPoint);
bouncePoint = CoordinateHelper.CalculateBouncePoint(centerOfMass, trajectoryPoint, currentMalletPosition.X, angleAdjust += adjustment);
Point prevBouncePoint = trajectoryPoint;
while (bouncePoint.X != currentMalletPosition.X && bouncePoints.Count < 1)
{
bouncePoints.Add(bouncePoint);
bouncePoint = CoordinateHelper.CalculateBouncePoint(prevBouncePoint, bouncePoint, currentMalletPosition.X, angleAdjust += adjustment);
prevBouncePoint = bouncePoint;
}
bouncePoints.Add(bouncePoint);
}
if (bouncePoint.X == currentMalletPosition.X)
{
// Don't react to bounce unless we think it's going to be near the goal
if (bouncePoint.Y > Config.TABLE_GOAL_Y_TOP && bouncePoint.X < Config.TABLE_GOAL_Y_BOTTOM)
{
malletTargetOffset = MotorHelper.GetOffsetFromCoordinates(bouncePoint);
}
}
}
}
else
{
// Defensive position in front of goal
malletTargetOffset = defenseOffset;
}
}
// We have a valid target destination for the mallet
if (malletTargetOffset != CoordinateHelper.INVALID_POINT)
{
// Constrain offset so that mallet doesn't move too far
malletTargetOffset.X = Helper.Constrain(malletTargetOffset.X, 0, Config.MAX_MALLET_OFFSET_X);
malletTargetOffset.Y = Helper.Constrain(malletTargetOffset.Y, 0, Config.MAX_MALLET_OFFSET_Y);
}
previousTrajectoryPoint = trajectoryPoint;
previousBouncePoints = bouncePoints;
return(malletTargetOffset);
}