Examples with Robot me.wobblyyyy.pathfinder2.robot.Robot used on opensource projects

Example 6 with Robot

use of me.wobblyyyy.pathfinder2.robot.Robot in project Pathfinder2 by Wobblyyyy.

the class ExampleTimedRobot method robotInit.

@Override
public void robotInit() {
    // initialize everything. if this was a real implementation, you would
    // not want to use SimulatedDrive or SimulatedOdometry
    drive = new SimulatedDrive();
    odometry = new SimulatedOdometry();
    robot = new Robot(drive, odometry);
    pathfinder = new Pathfinder(robot, controller).setSpeed(0.5).setTolerance(2).setAngleTolerance(Angle.fromDeg(5));
}

Example 7 with Robot

use of me.wobblyyyy.pathfinder2.robot.Robot 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 8 with Robot

use of me.wobblyyyy.pathfinder2.robot.Robot 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 Robot

use of me.wobblyyyy.pathfinder2.robot.Robot in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method moveFor.

/**
 * Move the robot in a certain direction for a certain amount of time.
 * This is a blocking method call, meaning it will block the current thread
 * until its execution has finished (whatever {@code timeoutMs} is).
 *
 * @param translation the translation that will be set to the robot.
 *                    This value may not be null.
 * @param timeoutMs   how long the robot should move for. This value is
 *                    represented in milliseconds and must be greater
 *                    than 0. This value may also not be infinite.
 * @return {@code this, used for method chaining}
 */
@SuppressWarnings("BusyWait")
public Pathfinder moveFor(Translation translation, double timeoutMs) {
    if (translation == null)
        throw new NullPointerException("Cannot use a null translation!");
    if (timeoutMs <= 0 || Double.isInfinite(timeoutMs) || timeoutMs == Double.MAX_VALUE)
        throw new IllegalArgumentException("Invalid timeout!");
    ElapsedTimer timer = new ElapsedTimer(true);
    setTranslation(translation);
    try {
        while (timer.isElapsedLessThan(timeoutMs)) Thread.sleep(Core.pathfinderWaitSleepTimeMs);
    } catch (Exception ignored) {
    }
    return this;
}

Example 10 with Robot

use of me.wobblyyyy.pathfinder2.robot.Robot in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method newEmptyPathfinder.

/**
 * Create a new, "empty" instance of Pathfinder.
 *
 * <p>
 * This is pretty much only useful for debugging or testing purposes.
 * </p>
 *
 * @param coefficient the coefficient to use for the turn controller.
 * @return a new instance of Pathfinder that makes use of both the
 * {@link EmptyDrive} and {@link EmptyOdometry} classes.
 */
public static Pathfinder newEmptyPathfinder(double coefficient) {
    Drive drive = new EmptyDrive();
    Odometry odometry = new EmptyOdometry();
    Robot robot = new Robot(drive, odometry);
    return new Pathfinder(robot, coefficient);
}