Examples with SimulatedRobot me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot used on opensource projects

Example 1 with SimulatedRobot

use of me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testZeroPointTurn.

@Test
public void testZeroPointTurn() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory trajectory = new LinearTrajectory(pathfinder.getPosition().withHeadingDegrees(45), 0.5, 2, Angle.fromDeg(2));
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectory(trajectory);
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(15)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(30)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(42)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(48)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(45)));
    pathfinder.tick();
    Assertions.assertFalse(pathfinder.isActive());
}

Example 2 with SimulatedRobot

use of me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testMultipleTrajectories.

@Test
public void testMultipleTrajectories() {
    // look... nobody said tests have to be written WELL...
    // i am sincerely sorry for whatever this mess is
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory a = new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 0, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory c = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory d = new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory e = new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5));
    SimulatedRobot o = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectories(a, b, c, d, e);
    int lastFollowers = pathfinder.getExecutorManager().howManyFollowers();
    int followers;
    setPos(o, new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(5, 5, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 0, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 5, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 10, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(0, 10, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertFalse(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
}

Example 3 with SimulatedRobot

use of me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testSimulation.

@Test
public void testSimulation() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory a = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5));
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectory(a);
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.353, 0.353, 0.0), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(0, 0, 15));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 40));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.4), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 3));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 10, 45), 0.5, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(b);
    odometry.setPosition(new PointXYZ(10, 10, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 10));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.35), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 20));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.25), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(-0.353, -0.353, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 45));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
}

Example 4 with SimulatedRobot

use of me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot in project Pathfinder2 by Wobblyyyy.

the class GenericTrajectoryBenchmarker method followTrajectories.

public void followTrajectories(Blackhole blackhole, Trajectory... trajectories) {
    SimulatedRobot robot = new SimulatedRobot();
    Pathfinder pathfinder = new Pathfinder(robot, -0.05);
    PointXYZ lastPosition = PointXYZ.ZERO;
    double distance = 0;
    for (Trajectory trajectory : trajectories) {
        robot.setPosition(PointXYZ.ZERO);
        pathfinder.followTrajectory(trajectory);
        while (distance < 10) {
            pathfinder.tick();
            distance += pathfinder.getPosition().absDistance(lastPosition);
        }
    }
}

Example 5 with SimulatedRobot

use of me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot in project Pathfinder2 by Wobblyyyy.

the class TestHeadingLock method testHeadingLockAlongLine.

@Test
public void testHeadingLockAlongLine() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.loadBundledPlugins();
    pathfinder.lockHeading(new PointXY(10, 10));
    pathfinder.followTrajectory(new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5)));
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(-0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 45));
    pathfinder.tick();
    Assertions.assertEquals(0, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 90));
    pathfinder.tick();
    Assertions.assertEquals(0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(0, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.265, pathfinder.getTranslation().vz(), 0.01));
    odometry.setPosition(5, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.45, pathfinder.getTranslation().vz(), 0.01));
}