Examples with Pathfinder me.wobblyyyy.pathfinder2.Pathfinder used on opensource projects

Example 1 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class TestMovementRecorder method testMovementRecorder.

public void testMovementRecorder() {
    Pathfinder pf = Pathfinder.newSimulatedPathfinder(0.01);
    SimulatedOdometry odometry = (SimulatedOdometry) pf.getOdometry();
    Translation translation = new Translation(0.51, 0.51, 0);
    odometry.setVelocity(Angle.DEG_45, 0.5);
    odometry.setTranslation(translation);
    pf.setTranslation(translation);
    pf.getRecorder().start();
    pf.tick();
    ElapsedTimer timer = new ElapsedTimer(true);
    while (timer.elapsedSeconds() < 2) pf.tick();
}

Example 2 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method beforeEach.

@BeforeEach
public void beforeEach() {
    wrapper = new SimulatedWrapper(new SimulatedDrive(), new SimulatedOdometry());
    odometry = wrapper.getOdometry();
    robot = wrapper.getRobot();
    turnController = new ProportionalController(-0.05);
    pathfinder = new Pathfinder(robot, turnController).setSpeed(0.5).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(5));
    factory = new SplineBuilderFactory().setSpeed(0.5).setStep(0.1).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(5));
}

Example 3 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder 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 4 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder 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 5 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder 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());
}