Examples with Pathfinder me.wobblyyyy.pathfinder2.Pathfinder used on opensource projects

Example 31 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class ExamplePathfinder method loop.

/**
 * This is a traditional loop method - it's meant to be run dozens of times
 * per second, over and over and over again. The general premise for this
 * loop is as follows: if Pathfinder is NOT active (meaning it isn't
 * following any trajectories), it'll check for user input using the
 * A, B, X, and Y gamepad buttons. If any of those buttons are pressed,
 * the robot will begin automatically navigating to an associated position.
 * <p>
 * Let's say Pathfinder IS active... what happens then? There will be times
 * when Pathfinder is attempting to control your robot while you'd like to
 * be the one who's in control of it. In this case, you should clear
 * Pathfinder, meaning it will no longer try to follow any paths, meaning
 * you have control over the robot.
 */
@SuppressWarnings("UnnecessaryLocalVariable")
public void loop() {
    if (!pathfinder.isActive()) {
        // If Pathfinder isn't active, let's take a look at our
        // controller inputs.
        PointXYZ targetPoint = null;
        // Pretty cool, right?
        if (gamepadA)
            targetPoint = TARGET_A;
        else if (gamepadB)
            targetPoint = TARGET_B;
        else if (gamepadX)
            targetPoint = TARGET_X;
        else if (gamepadY)
            targetPoint = TARGET_Y;
        if (targetPoint != null) {
            pathfinder.goTo(targetPoint);
        } else {
            // Based on some joysticks, generate a translation.
            // This translation will then be used to drive the robot.
            double moveForwards = joystick1y;
            double moveStrafe = joystick1x;
            double moveRotate = joystick2x;
            Translation translation = new Translation(moveForwards, moveStrafe, moveRotate);
            pathfinder.getRobot().drive().setTranslation(translation);
        }
    }
    if (gamepadStart) {
        // Let's say we want to manually override Pathfinder and regain
        // control of the robot. All we'd have to do:
        pathfinder.clear();
    }
    // ... any other code that you would need in your main loop
    // ex. sensor updates, other motors, you know the deal
    // Tick or update Pathfinder once. Remember, this is absolutely
    // essential - if you don't tick Pathfinder, nothing can happen.
    pathfinder.tick();
}

Example 32 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class ExampleRecording method init.

public void init() {
    drive = new SimulatedDrive();
    odometry = new SimulatedOdometry();
    robot = new Robot(drive, odometry);
    pathfinder = new Pathfinder(robot, 0.02);
}

Example 33 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class ExampleTeleOp method exampleTeleOpListeners.

@SuppressWarnings("deprecation")
public void exampleTeleOpListeners() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    AtomicReference<Double> multiplier = new AtomicReference<>(0.5);
    Shifter shifter = new Shifter(1, 1, 5, false, i -> {
    });
    Joystick right = new Joystick(() -> 0d, () -> 0d);
    Joystick left = new Joystick(() -> 0d, () -> 0d);
    Button a = new Button(() -> false);
    Button b = new Button(() -> false);
    Button rightBumper = new Button(() -> false);
    Button leftBumper = new Button(() -> false);
    // bind the following controls:
    // a button       -> shift elevator up a gear
    // b button       -> shift elevator down a gear
    // right bumper   -> set the multiplier to 1.0 (full speed)
    // left bumper    -> set the multiplier to 0.25 (slowest speed)
    // neither bumper -> set the multiplier to 0.5 (normal speed)
    pathfinder.getListenerManager().bind(// whenever the right bumper is pressed (even if held)...
    ListenerMode.CONDITION_IS_MET, rightBumper::isPressed, isPressed -> isPressed, isPressed -> multiplier.set(1.0)).bind(// whenever the left bumper is pressed (even if held)...
    ListenerMode.CONDITION_IS_MET, leftBumper::isPressed, isPressed -> isPressed, isPressed -> multiplier.set(0.25)).bind(// whenever neither bumper is pressed...
    ListenerMode.CONDITION_IS_NOT_MET, () -> SupplierFilter.anyTrue(rightBumper::isPressed, leftBumper::isPressed), isPressed -> isPressed, isPressed -> multiplier.set(0.5)).bind(// whenever the a button is initially pressed...
    ListenerMode.CONDITION_NEWLY_MET, () -> SupplierFilter.trueThenAllTrue(// a must be pressed
    a::isPressed, // a must NOT be pressed
    b::isPressed), isPressed -> isPressed, isPressed -> shifter.shift(ShifterDirection.UP)).bind(// whenever the b button is initially pressed...
    ListenerMode.CONDITION_NEWLY_MET, () -> SupplierFilter.trueThenAllTrue(// b must be pressed
    a::isPressed, // a must NOT be pressed
    b::isPressed), isPressed -> isPressed, isPressed -> shifter.shift(ShifterDirection.DOWN));
    pathfinder.onTick(pf -> {
        double m = multiplier.get();
        double vertical = right.getX();
        double horizontal = right.getY();
        double turn = left.getX();
        Translation translation = new Translation(vertical * m, horizontal * m, turn * m);
        pf.setTranslation(translation);
    }).onTick(pf -> {
    // some magic code that updates the elevator based on
    // what level it's currently on and what level it's
    // trying to get to
    });
    while (true) pathfinder.tick();
}

Example 34 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class ExampleSplineTo method run.

public void run() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    pathfinder.setSpeed(0.5).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).splineTo(new PointXYZ(0, 10, 0), new PointXYZ(5, 15, 0), new PointXYZ(10, 25, 0)).tickUntil(10_000).splineTo(new PointXYZ(20, 15, 0), new PointXYZ(25, 10, 0), new PointXYZ(30, 5, 0)).tickUntil(10_000);
}