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

Example 11 with Trajectory

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

the class ExamplePathfinder method betterGoToSomePoints.

public void betterGoToSomePoints() {
    // generate a list of trajectories w/ LinearTrajectoryBuilder
    // the setSpeed method is used to dynamically adjust the speed of
    // the robot - for example, the speed is halved, then the speed
    // is reset, then the speed is doubled
    List<Trajectory> trajectories = new LinearTrajectoryBuilder(SPEED, TOLERANCE, ANGLE_TOLERANCE, PointXYZ.ZERO).goTo(new PointXYZ(0, 0, 0)).goTo(new PointXYZ(10, 0, 0)).setSpeed(SPEED / 2).goTo(new PointXYZ(10, 10, 0)).setSpeed(SPEED).goTo(new PointXYZ(0, 10, 0)).setSpeed(SPEED * 2).goTo(new PointXYZ(0, 0, 0)).getTrajectories();
    // follow the trajectories with a timeout of 10 seconds. if more than
    // 10 seconds pass and the trajectories haven't finished yet, stop
    // following the trajectories
    pathfinder.followTrajectories(trajectories).tickUntil(10_000);
}

Example 12 with Trajectory

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

the class ExampleTimedRobot method autonomousInit.

@Override
public void autonomousInit() {
    // clear it, just in case there's already a trajectory
    pathfinder.clear();
    // make the robot follow a spline: all we have to do now is tick it!
    pathfinder.splineTo(new PointXYZ(0, 0, 0), new PointXYZ(10, 0, 0), new PointXYZ(10, 10, 0), new PointXYZ(0, 10, 0), new PointXYZ(0, 0, 0));
}

Example 13 with Trajectory

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

the class ExampleSpline method run.

public void run() {
    // before anything else, we have to get some stuff set up.
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    // alright! trajectory time! let's see what's up.
    // as you can see, this is mostly pretty self-explanatory.
    // you create an AdvancedSplineTrajectoryBuilder and use the
    // add methods provided by that class to construct a trajectory.
    // this method works well, but it's a bit verbose - there's a
    // solution to that problem you'll see in just a moment.
    Trajectory trajectory1 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(0, 0, 0)).add(new PointXYZ(4, 6, 0)).add(new PointXYZ(6, 12, 0)).add(new PointXYZ(8, 24, 0)).build();
    Trajectory trajectory2 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(8, 24, 0)).add(new PointXYZ(6, 36, 0)).add(new PointXYZ(4, 40, 0)).add(new PointXYZ(0, 42, 0)).build();
    // thankfully, there's an easier way to create trajectories just like
    // that - we can make use of a "SplineBuilderFactory"
    SplineBuilderFactory factory = new SplineBuilderFactory().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5));
    // set the default speed, step, tolerance, and angle tolerance
    // values for the factory. all of the spline builders produced by
    // the factory will have have these values by default.
    // now we can create new trajectories, without having to repeat
    // the same 4 lines for each of the trajectories.
    Trajectory trajectory3 = factory.builder().add(0, 60, Angle.fromDeg(0)).add(new PointXYZ(20, 60, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(40, 70, 0)).build();
    Trajectory trajectory4 = factory.builder().add(new PointXYZ(40, 70, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(20, 60, 0)).add(0, 60, Angle.fromDeg(0)).build();
    // time to actually make the robot move now! once again, most of
    // these methods are fairly self-explanatory. basically, follow
    // the first two trajectories, come to a complete stop, and then
    // follow the next two trajectories, but each of those trajectories
    // should have an individual timeout of 10 seconds.
    pathfinder.followTrajectories(trajectory1, trajectory2).andThen(pf -> {
        // any other code you want to be executed after the
        // trajectory is finished
        pf.setTranslation(new Translation(0, 0, 0));
    }).followTrajectory(trajectory3).tickUntil(10_000).followTrajectory(trajectory4).tickUntil(10_000);
}

Example 14 with Trajectory

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

the class TestTaskTrajectory method testMaxTimeTaskTrajectory.

@Test
public void testMaxTimeTaskTrajectory() {
    ElapsedTimer timer = new ElapsedTimer(true);
    Trajectory trajectory = new TaskTrajectoryBuilder().setMaxTimeMs(10).setIsFinished(() -> false).build();
    testTrajectory(trajectory, pathfinder.getPosition(), 100);
    Assertions.assertTrue(timer.elapsedMs() < 10);
}

Example 15 with Trajectory

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

the class TestAdvancedSplineTrajectory method testSplineTo.

private void testSplineTo(PointXYZ... points) {
    if (points.length < 1)
        throw new IllegalArgumentException();
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setStep(0.1).setTolerance(tolerance).setSpeed(speed).setAngleTolerance(angleTolerance);
    builder.setInterpolationMode(mode);
    for (PointXYZ point : points) builder.add(point);
    builder.setInterpolationMode(InterpolationMode.DEFAULT);
    Trajectory trajectory = builder.build();
    PointXYZ target = points[points.length - 1];
    testTrajectory(trajectory, target);
}