Examples with PointXYZ me.wobblyyyy.pathfinder2.geometry.PointXYZ used on opensource projects

Example 1 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testRightTurn.

@Test
public void testRightTurn() {
    pathfinder.goTo(new PointXYZ(5, 5, 90)).tickUntil(500);
    assertPositionIs(new PointXYZ(5, 5, 90));
    odometry.setTranslation(new Translation(1, 0, 0));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(-1, 0, 0));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(1, 1, 0));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(-1, -1, 0));
    odometry.updatePositionBasedOnVelocity(500);
    Assertions.assertTrue(pathfinder.getPosition().absDistance(new PointXYZ(5, 5, 90)) < 2);
}

Example 2 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testZeroPointTurn.

@Test
public void testZeroPointTurn() {
    pathfinder.followTrajectory(new LinearTrajectory(new PointXYZ(0, 0, 10), 0.5, 2, Angle.fromDeg(5)));
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(0, 0, 10));
}

Example 3 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testOnFinishFollower.

@Test
public void testOnFinishFollower() {
    AtomicInteger count = new AtomicInteger(0);
    Consumer<PointXYZ> consumer = position -> {
        count.getAndIncrement();
    };
    Trajectory a = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.05, 0.01, Angle.fromDeg(1)).reflectX(0).onFinish(consumer);
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.05, 0.01, Angle.fromDeg(1)).reflectY(0).onFinish(consumer);
    Trajectory c = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.05, 0.01, Angle.fromDeg(1)).reflectX(0).reflectY(0).onFinish(consumer);
    pathfinder.followTrajectories(a, b, c);
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(-10, -10, 0));
    Assertions.assertEquals(3, count.get());
}

Example 4 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testRectangle.

@Test
public void testRectangle() {
    pathfinder.followTrajectories(new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5)), new LinearTrajectory(new PointXYZ(10, 0, 0), 0.5, 2, Angle.fromDeg(5)), new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5)), new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5)), new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5)));
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ());
}

Example 5 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testMultipleTrajectories.

@Test
public void testMultipleTrajectories() {
    pathfinder.followTrajectories(new LinearTrajectory(new PointXYZ(10, 10, 270), 0.5, 2, Angle.fromDeg(5)), new LinearTrajectory(new PointXYZ(20, 20, 360), 0.5, 2, Angle.fromDeg(5)));
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(20, 20, 360));
}