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

Example 6 with PointXYZ

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

the class TestTrajectoryCommands method testLinearTrajectory.

@Test
public void testLinearTrajectory() {
    registry.execute("linearTrajectory", "10,10,45deg", "0.5", "2", "5deg");
    registry.execute("tickUntil");
    AssertionUtils.assertIsNear(new PointXYZ(10, 10, 45), pathfinder.getPosition(), 2, Angle.fromDeg(5));
}

Example 7 with PointXYZ

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

the class TestTrajectoryCommands method testFastTrajectoryWithAutoInitial.

@Test
public void testFastTrajectoryWithAutoInitial() {
    registry.execute("fastTrajectory", "10,10,45deg", "0.5");
    registry.execute("tickUntil");
    AssertionUtils.assertIsNear(new PointXYZ(10, 10, 0), pathfinder.getPosition(), 2, Angle.fromDeg(360));
}

Example 8 with PointXYZ

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

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

the class TestCommandRegistry method testParseManyLines.

@Test
public void testParseManyLines() {
    Assertions.assertFalse(pathfinder.isActive());
    String[] lines = new String[] { "def a 0,0,0", "def b 5,10,0", "def c 10,20,0", "def d 15,25,0", "def e 20,30,0", "splineTo $a\\", "$b\\", "$c\\", "$d\\", "$e", "tickUntil" };
    registry.parse(lines);
    AssertionUtils.assertIsNear(new PointXYZ(20, 30, 0), pathfinder.getPosition(), 2, Angle.fromDeg(5));
}

Example 10 with PointXYZ

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

the class DifferentialDriveOdometry method update.

/**
 * Update the robot's position. After updating the position, return the
 * newly-updated position.
 *
 * @param gyroAngle     the angle of the gyroscope.
 * @param rightDistance the distance of the right encoder. This should
 *                      NOT be the "elapsed distance," or distance since
 *                      the last update: rather, this should be the
 *                      encoder's distance, in total.
 * @param leftDistance  the distance of the left encoder.
 *                      NOT be the "elapsed distance," or distance since
 *                      the last update: rather, this should be the
 *                      encoder's distance, in total.
 * @return the robot's updated posititon.
 */
public PointXYZ update(Angle gyroAngle, double rightDistance, double leftDistance) {
    double deltaRight = rightDistance - previousRightDistance;
    double deltaLeft = leftDistance - previousLeftDistance;
    previousRightDistance = rightDistance;
    previousLeftDistance = leftDistance;
    double averageDelta = Average.of(deltaRight, deltaLeft);
    Angle angle = gyroAngle.add(gyroOffset);
    PointXYZ newPosition = position.applyTranslation(new Translation(0.0, averageDelta, angle.subtract(previousAngle).deg()));
    previousAngle = angle;
    position = newPosition.withZ(angle);
    return position;
}