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 Driver(Node goal) { _cs = new CommandScript(); _location = new TrackerMatrix(); _robot = new Robot("COM19"); _robot.Open(); _goal = goal; }
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 BackupTest() { TrackerMatrix target = new TrackerMatrix(); // TODO: Initialize to an appropriate value float distance = -50F; // TODO: Initialize to an appropriate value target.goForward(distance); Assert.AreEqual(target.getX(), 0F, .001); Assert.AreEqual(target.getY(), -50F, .001); }
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()); }