public void followPathToGoal()
{
List <PointF> plan = getLocationSequence(_goal).ToList();
//_robot.setLocation(plan.First());
_location = new TrackerMatrix(plan.First().X, plan.First().Y);
//float totalDistance = 0;
foreach (PointF next in plan)
{
if (Math.Abs(_location.getPoint().X - next.X) < .001 &&
Math.Abs(_location.getPoint().Y - next.Y) < .001)
{
continue;
}
_location.goForward(1);
PointF calc = _location.getPoint();
_location.goForward(-1);
int angle = (int)getAngleC(calc, next, _location.getPoint());
if (counterClockwiseTurn(calc, _location.getPoint(), next) > 0)
{
angle = angle * -1;
}
int distance = (int)(getDistance(next, _location.getPoint()) * 1000);
_location.rotate(turnAngle(angle));
_location.goForward(goDistance(distance) / 1000);
}
}
public void AngleTestBelow()
{
TrackerMatrix target = new TrackerMatrix(); // TODO: Initialize to an appropriate value
float angle = -10F; // TODO: Initialize to an appropriate value
target.rotate(angle);
Assert.AreEqual(target.getAngle(), 350F);
}
public void rotatedBoxTest()
{
TrackerMatrix target = new TrackerMatrix(); // TODO: Initialize to an appropriate value
float angle = 90F; // TODO: Initialize to an appropriate value
float distance = 100F;
target.rotate(45F);
target.goForward(distance);
target.rotate(angle);
target.goForward(distance);
target.rotate(angle);
Assert.IsTrue(withinTolerance(target.getX(), -(float)Math.Sqrt(Math.Pow(100, 2) * 2), .001F));
Assert.IsTrue(withinTolerance(target.getY(), 0F, .001F));
target.goForward(distance);
target.rotate(angle);
target.goForward(distance);
target.rotate(angle);
Assert.AreEqual(45, target.getAngle());
}