Examples with Trajectory me.wobblyyyy.pathfinder2.trajectory.Trajectory used on opensource projects

Example 41 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestMultiTargetTrajectory method testMultiTargetTrajectory.

@Test
public void testMultiTargetTrajectory() {
    TrajectoryTarget[] targets = new MultiTargetBuilder().setPrecision(TargetPrecision.FAST).setSpeed(0.5).setTolerance(2.0).setAngleTolerance(Angle.fromDeg(5)).addTargetPoint(new PointXYZ(10, 10, 0)).addTargetPoint(new PointXYZ(10, 10, 10)).addTargetPoint(new PointXYZ(-10, -10, 0)).build();
    MultiTargetTrajectory trajectory = new MultiTargetTrajectory(targets);
    PointXYZ a = trajectory.nextMarker(new PointXYZ(0, 0, 0));
    PointXYZ b = trajectory.nextMarker(new PointXYZ(10, 0, 0));
    PointXYZ c = trajectory.nextMarker(new PointXYZ(10, 5, 0));
    Assertions.assertEquals(a, b);
    Assertions.assertEquals(b, c);
    PointXYZ d = trajectory.nextMarker(new PointXYZ(10, 10, 0));
    Assertions.assertEquals(d, new PointXYZ(10, 10, 10));
    PointXYZ e = trajectory.nextMarker(new PointXYZ(10, 10, 10));
    PointXYZ f = trajectory.nextMarker(new PointXYZ(0, 0, 10));
    PointXYZ g = trajectory.nextMarker(new PointXYZ(-5, -5, 5));
    Assertions.assertEquals(e, f);
    Assertions.assertEquals(f, g);
    PointXYZ h = trajectory.nextMarker(new PointXYZ(-10, -10, 0));
    Assertions.assertTrue(trajectory.isDone(new PointXYZ(-10, -10, 0)));
}

Example 42 with Trajectory

use of me.wobblyyyy.pathfinder2.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 43 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class GenericTrajectoryTester method follow.

public void follow(Trajectory trajectory, PointXYZ point) {
    // this is pretty disgusting code, but it gets the job done
    // basically just spawn a new monitor thread to ensure that it doesn't
    // take over a certain amount of time to execute the trajectory. if it
    // does, something's broken, so the test should fail
    pathfinder.followTrajectory(trajectory);
    ElapsedTimer timer = new ElapsedTimer();
    AtomicBoolean hasNotExpired = new AtomicBoolean(true);
    AtomicBoolean hasExecuted = new AtomicBoolean(false);
    Thread monitor = new Thread(() -> {
        timer.start();
        try {
            while (!hasExecuted.get()) Thread.sleep(1);
            while (timer.elapsedMs() < maximumExecutionTimeMs) {
                Thread.sleep(1);
                if (!pathfinder.isActive())
                    break;
            }
            if (pathfinder.isActive())
                hasNotExpired.set(false);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    });
    monitor.start();
    pathfinder.tickUntil(hasNotExpired::get, pf -> hasExecuted.set(true));
    if (!hasNotExpired.get())
        throw new RuntimeException(StringUtils.format("Trajectory <%s> (target <%s>) took more than %s " + "milliseconds to execute, meaning something " + "went wrong with following the trajectory!", trajectory, point, maximumExecutionTimeMs));
    assertPositionIs(point);
}

Example 44 with Trajectory

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

Example 45 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestExecutorManager method testExecutorManager.

@Test
public void testExecutorManager() {
    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());
    ExecutorManager manager = new ExecutorManager(robot);
    Assertions.assertFalse(manager.isActive());
    manager.addExecutor(list);
    Assertions.assertTrue(manager.isActive());
    Assertions.assertFalse(manager.isInactive());
    Assertions.assertFalse(manager.tick());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertEquals(1, manager.howManyExecutors());
    Assertions.assertTrue(manager.tick());
    Assertions.assertFalse(manager.isActive());
    Assertions.assertTrue(manager.isInactive());
}