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

Example 21 with Pathfinder

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

the class ExampleListeners method exampleShifter.

@SuppressWarnings("InfiniteLoopStatement")
public void exampleShifter() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Shifter shifter = new Shifter(1, 1, 5, false, s -> {
    });
    pathfinder.getListenerManager().bind(ListenerMode.CONDITION_NEWLY_MET, this::aButton, b -> b, b -> shifter.shift(ShifterDirection.UP));
    pathfinder.getListenerManager().bind(ListenerMode.CONDITION_NEWLY_MET, this::bButton, b -> b, b -> shifter.shift(ShifterDirection.DOWN));
    pathfinder.getListenerManager().bind(ListenerMode.CONDITION_NEWLY_MET, this::xButton, b -> b, b -> shifter.setGear(1));
    pathfinder.getListenerManager().bind(ListenerMode.CONDITION_NEWLY_MET, this::yButton, b -> b, b -> shifter.setGear(5));
    while (true) {
        pathfinder.tick();
    }
}

Example 22 with Pathfinder

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

the class ExampleListeners method examplePositionListeners.

@SuppressWarnings({ "CodeBlock2Expr", "unchecked", "InfiniteLoopStatement" })
public void examplePositionListeners() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Gamepad gamepad = new Gamepad();
    // first up, some listeners for the robot's actual position. you can
    // create listeners fairly easily:
    // (also, notice how method chaining is used to make code more
    // readable - you don't have to do this!)
    pathfinder.addListener(new Listener(ListenerMode.CONDITION_NEWLY_MET, () -> {
        // this will be executed ONCE whenever x exceeds 500.
        // in order for this to be executed again, x will have to
        // dip below 500 and then come back over 500 again
        System.out.println("x crossed over 500!");
    }, () -> {
        return pathfinder.getPosition().x() > 500;
    })).addListener(new Listener(// here's a nicer-looking way to do the same exact thing.
    ListenerMode.CONDITION_NEWLY_MET, () -> System.out.println("y crossed over 500!"), () -> pathfinder.getPosition().y() > 500)).addListener(new Listener(ListenerMode.CONDITION_NEWLY_MET, () -> System.out.println("x AND y crossed over 500! wow!"), () -> {
        PointXY position = pathfinder.getPosition();
        return position.x() > 500 && position.y() > 500;
    }));
    // time for more listeners! this time, these listeners demonstrate
    // how you could use listeners to bind functionality to a button.
    pathfinder.addListener(new Listener(ListenerMode.CONDITION_NEWLY_MET, () -> {
        System.out.println("the A button has been pressed!");
    }, gamepad::a));
    pathfinder.addListener(new Listener(ListenerMode.CONDITION_NEWLY_NOT_MET, () -> {
        System.out.println("the A button has been released!");
    }, gamepad::a));
    // here's a more complex condition...
    // notice how we make use of the oh-so-lovely lambda syntax available
    // in java to make this code significantly more tolerable
    pathfinder.addListener(new Listener(ListenerMode.CONDITION_NEWLY_MET, () -> System.out.println("the right and left joysticks have" + "magnitudes above 0.5!"), () -> gamepad.joysticks.right().getMagnitude() > 0.5 && gamepad.joysticks.left().getMagnitude() > 0.5));
    // and here's an alternative way to have multiple preconditions.
    pathfinder.addListener(new Listener(ListenerMode.CONDITION_NEWLY_NOT_MET, () -> {
        System.out.println("the right and left joysticks have" + "magnitudes below 0.5!");
    }, // precondition #1
    () -> {
        return gamepad.joysticks.right().getMagnitude() > 0.5;
    }, // precondition #2
    () -> {
        return gamepad.joysticks.left().getMagnitude() > 0.5;
    }));
    // implementation, but yeah.
    while (true) {
        pathfinder.tick();
    }
}

Example 23 with Pathfinder

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

the class ZoneProcessor method update.

/**
 * Based on the provided point, call the correct methods of each of
 * the zones.
 *
 * <ul>
 *     <li>
 *         If the robot has ENTERED the zone (previously, it was not in
 *         the zone, but now it is), the zone's {@link Zone#onEnter(Pathfinder)}
 *         method will be called.
 *     </li>
 *     <li>
 *         If the robot has EXITED the zone (previously, it was in the zone,
 *         but now it is not), the zone's {@link Zone#onExit(Pathfinder)}
 *         method will be called.
 *     </li>
 *     <li>
 *         If the robot is INSIDE the zone (this will be activated every
 *         time the {@code onEnter} method is called, as well as whenever
 *         the robot is inside the zone), the zone's
 *         {@link Zone#whileInside(Pathfinder)} method will be called.
 *     </li>
 * </ul>
 *
 * @param pathfinder the instance of Pathfinder.
 */
public void update(Pathfinder pathfinder) {
    if (zones.size() == 0)
        return;
    PathfinderPluginManager manager = pathfinder.getPluginManager();
    List<Zone> lastZones = currentZones;
    currentZones = getContainingZones(pathfinder.getPosition());
    List<Zone> enteredZones = getEnteredZones(lastZones, currentZones);
    List<Zone> exitedZones = getExitedZones(lastZones, currentZones);
    for (Zone zone : enteredZones) {
        zone.onEnter(pathfinder);
        manager.onEnterZone(pathfinder, zone);
    }
    for (Zone zone : currentZones) {
        zone.whileInside(pathfinder);
        manager.whileInsideZone(pathfinder, zone);
    }
    for (Zone zone : exitedZones) {
        zone.onExit(pathfinder);
        manager.onExitZone(pathfinder, zone);
    }
}

Example 24 with Pathfinder

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

the class TestHeadingLock method testHeadingLockAlongLine.

@Test
public void testHeadingLockAlongLine() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.loadBundledPlugins();
    pathfinder.lockHeading(new PointXY(10, 10));
    pathfinder.followTrajectory(new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5)));
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(-0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 45));
    pathfinder.tick();
    Assertions.assertEquals(0, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 90));
    pathfinder.tick();
    Assertions.assertEquals(0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(0, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.265, pathfinder.getTranslation().vz(), 0.01));
    odometry.setPosition(5, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.45, pathfinder.getTranslation().vz(), 0.01));
}

Example 25 with Pathfinder

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

the class Pathfinder method followTrajectory.

/**
 * Follow a single trajectory from the global trajectory map. If the
 * trajectory cannot be found in the map, this method will throw an
 * exception indicating that the trajectory could not be found, and will
 * provide suggestions for what the user could have been trying to find.
 *
 * @param trajectoryName the name of the trajectory to follow.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder followTrajectory(final String trajectoryName) {
    if (TRAJECTORY_MAP.size() < 1)
        throw new IllegalStateException("Cannot follow a trajectory " + "by name without first adding at least 1 trajectory " + "to the global trajectory map by using one of the " + "addTrajectory methods!");
    // throw an exception containing a list of all of the potential names
    if (!TRAJECTORY_MAP.containsKey(trajectoryName)) {
        List<String> potentialNames = new ArrayList<>(10);
        TRAJECTORY_MAP.forEach((name, trajectory) -> {
            String lowercase = name.toLowerCase();
            // just to make debugging easier
            if (lowercase.contains(trajectoryName.toLowerCase()))
                potentialNames.add(name);
            else if (TRAJECTORY_MAP.size() < 8)
                potentialNames.add(name);
        });
        // have free time during class and nothing better to be doing...
        if (potentialNames.size() == 0) {
            final String shortName;
            if (trajectoryName.length() > 3)
                shortName = trajectoryName.substring(0, 3);
            else
                shortName = trajectoryName;
            TRAJECTORY_MAP.forEach((name, trajectory) -> {
                String lowercase = name.toLowerCase();
                if (lowercase.contains(shortName))
                    potentialNames.add(name);
            });
        }
        StringBuilder builder = new StringBuilder(100);
        builder.append("Could not follow trajectory with name <%s>. ");
        builder.append("did you mean one of these? ");
        int size = potentialNames.size();
        for (int i = 0; i < size; i++) {
            String potentialName = potentialNames.get(i);
            builder.append(potentialName);
            if (i < size - 1)
                builder.append(", ");
        }
        throw new NullTrajectoryException(StringUtils.format(builder.toString(), trajectoryName));
    }
    // if the trajectory is found, just follow it
    return followTrajectory(getTrajectory(trajectoryName));
}