Examples with Controller me.wobblyyyy.pathfinder2.control.Controller used on opensource projects

Example 6 with Controller

use of me.wobblyyyy.pathfinder2.control.Controller 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 7 with Controller

use of me.wobblyyyy.pathfinder2.control.Controller in project Pathfinder2 by Wobblyyyy.

the class TestFollowerExecutor method testSingleFollowerExecution.

@Test
public void testSingleFollowerExecution() {
    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());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertTrue(executor.tick());
}

Example 8 with Controller

use of me.wobblyyyy.pathfinder2.control.Controller in project Pathfinder2 by Wobblyyyy.

the class TestGenericFollowerGenerator method testGeneration.

@Test
public void testGeneration() {
    Robot robot = new Robot(new SimulatedDrive(), new SimulatedOdometry());
    Controller controller = new GenericTurnController(0.1);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 10), 0.5, 0.1, Angle.fromDeg(3));
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    Assertions.assertEquals(trajectory, follower.getTrajectory());
}

Example 9 with Controller

use of me.wobblyyyy.pathfinder2.control.Controller 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 10 with Controller

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