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

Example 1 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 3 with Translation

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

the class TestSimulatedChassis method testRectangle.

private void testRectangle(double vx, double vy, double vz) {
    odometry.setRawPosition(new PointXYZ(0, 0, 0));
    // first group of 4
    odometry.setTranslation(new Translation(vx, 0, vz));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, vy, vz));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(-vx, 0, vz));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, -vy, vz));
    odometry.updatePositionBasedOnVelocity(500);
    assertPositionIs(new PointXYZ(0, 0, 0));
    Translation a = new Translation(vx * vx, vy * vy, vz * vz).multiply(4);
    // second group of 4
    odometry.setTranslation(new Translation(vx, 0, vz).multiply(a));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, vy, vz).multiply(a));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(-vx, 0, vz).multiply(a));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, -vy, vz).multiply(a));
    odometry.updatePositionBasedOnVelocity(500);
    assertPositionIs(new PointXYZ(0, 0, 0));
    Translation b = new Translation(3, 3, 3);
    // ... and of course, the third group of 4
    odometry.setTranslation(new Translation(vx, 0, vz).multiply(b));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, vy, vz).multiply(b));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(-vx, 0, vz).multiply(b));
    odometry.updatePositionBasedOnVelocity(500);
    odometry.setTranslation(new Translation(0, -vy, vz).multiply(b));
    odometry.updatePositionBasedOnVelocity(500);
    assertPositionIs(new PointXYZ(0, 0, 0));
}

Example 4 with Translation

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

the class TestSimulatedDrive method testTranslation.

@Test
public void testTranslation() {
    Translation translation = new Translation(0, 0, 0);
    SimulatedDrive drive = new SimulatedDrive();
    drive.setTranslation(translation);
    Assertions.assertEquals(translation, drive.getTranslation());
}

Example 5 with Translation

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