Examples with Trajectory com.team254.lib_2014.trajectory.Trajectory used on opensource projects

Example 31 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestSplineTrajectory method runTest.

private void runTest(PointXYZ[] points) {
    Spline spline = MonotoneCubicSpline.fromPoints(points);
    PointXYZ[] reversedPoints = new PointXYZ[points.length];
    System.arraycopy(points, 0, reversedPoints, 0, points.length);
    ArrayUtils.reverse(reversedPoints);
    Spline reverseSpline = MonotoneCubicSpline.fromPoints(reversedPoints);
    Trajectory trajectory = new SplineTrajectory(spline, Angle.DEG_45, 0.5, 0.1, 2, Angle.fromDeg(5));
    Trajectory reverseTrajectory = new SplineTrajectory(reverseSpline, Angle.DEG_0, 0.5, -0.1, 2, Angle.fromDeg(5));
    follow(trajectory, points[points.length - 1].withZ(Angle.DEG_45));
    follow(reverseTrajectory, reversedPoints[points.length - 1].withZ(Angle.ZERO));
}

Example 32 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestableRobot method testTestTrajectory.

@Test
public void testTestTrajectory() {
    Assertions.assertThrows(RuntimeException.class, () -> {
        Trajectory trajectory = new EmptyTrajectory();
        PointXYZ target = PointXYZ.ZERO;
        double maxTimeMs = 0;
        testTrajectory(trajectory, target, maxTimeMs);
    });
}

Example 33 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestFollowerExecutor method testSingleFollowerExecution.

@Test
public void testSingleFollowerExecution() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    List<Follower> list = new ArrayList<Follower>() {

        {
            add(follower);
        }
    };
    FollowerExecutor executor = new FollowerExecutor(odometry, drive, list);
    Assertions.assertFalse(executor.tick());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertTrue(executor.tick());
}

Example 34 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestGenericFollowerGenerator method testGeneration.

@Test
public void testGeneration() {
    Robot robot = new Robot(new SimulatedDrive(), new SimulatedOdometry());
    Controller controller = new GenericTurnController(0.1);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 10), 0.5, 0.1, Angle.fromDeg(3));
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    Assertions.assertEquals(trajectory, follower.getTrajectory());
}

Example 35 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestExecutorManager method testExecutorManager2.

@Test
public void testExecutorManager2() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory1 = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Trajectory trajectory2 = new LinearTrajectory(new PointXYZ(20, 20, 45), 1.0, 0.1, Angle.fromDeg(3));
    Trajectory trajectory3 = new LinearTrajectory(new PointXYZ(30, 30, 90), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower1 = generator.generate(robot, trajectory1);
    Follower follower2 = generator.generate(robot, trajectory2);
    Follower follower3 = generator.generate(robot, trajectory3);
    List<Follower> followers = new ArrayList<Follower>() {

        {
            add(follower1);
            add(follower2);
            add(follower3);
        }
    };
    ExecutorManager manager = new ExecutorManager(robot);
    manager.addExecutor(followers);
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(20, 20, 30));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(20, 20, 45));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(30, 30, 45));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(30, 30, 90));
    manager.tick();
    Assertions.assertFalse(manager.isActive());
}