Examples with Follower me.wobblyyyy.pathfinder2.follower.Follower used on opensource projects

Example 1 with Follower

use of me.wobblyyyy.pathfinder2.follower.Follower in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method followTrajectories.

/**
 * Follow multiple trajectories.
 *
 * @param trajectories a list of trajectories to follow.
 * @return this instance of Pathfinder, used for method chaining.
 */
public Pathfinder followTrajectories(List<Trajectory> trajectories) {
    if (trajectories == null)
        throw new NullPointerException("Cannot follow null trajectories!");
    List<Follower> followers = new ArrayList<>();
    for (Trajectory trajectory : trajectories) followers.add(generator.generate(robot, trajectory));
    follow(followers);
    return this;
}

Example 2 with Follower

use of me.wobblyyyy.pathfinder2.follower.Follower in project Pathfinder2 by Wobblyyyy.

the class GenericFollower method tick.

@Override
public boolean tick(PointXYZ current, Consumer<Translation> consumer) {
    // alright - i'm writing this comment maybe... say... four months?
    // after writing this initial bit of code, and i would just like to
    // say - if you're here because you're trying to figure out what makes
    // the robot move, congrats! you've found it! enjoy some stupidly
    // thoroughly commented and insanely simple code!
    // with love,
    // - colin <3
    // Tue Jan 18 22:34:58
    // p.s. i also made all of the comments in this method lowercase now
    // so it matches the vibe better. you know what i mean?
    ValidationUtils.validate(current, "current");
    ValidationUtils.validate(consumer, "consumer");
    Logger.trace(GenericFollower.class, "Ticking follower (current pos: <%s>)", current, consumer);
    // right here.
    if (trajectory.isDone(current)) {
        Logger.debug(GenericFollower.class, "Finished follower for trajectory <%s>", trajectory);
        // because the trajectory has finished, we give the robot a
        // translation of zero - there's no point in moving anymore, right?
        consumer.accept(Translation.ZERO);
        // rest of the method won't get executed.
        return true;
    }
    // assuming the follower hasn't finished executing its trajectory,
    // we continue by finding a couple values and generating a shiny
    // new translation for the robot to follow.
    // get the next marker by calling the trajectory's nextMarker
    // method. this method should always return a target point we can
    // use to generate a new translation.
    PointXYZ nextMarker = trajectory.nextMarker(current);
    // and of course, the speed, because some trajectories can control
    // speed differently.
    double speed = trajectory.speed(current);
    // determine the delta between the two angles, so we can calculate
    // turn in just a moment.
    double angleDelta = Angle.minimumDelta(current.z(), nextMarker.z());
    // and calculate turn. remember, the turn controller's only job is to
    // get the delta to zero, so by feeding it the delta it should output
    // a value that will minimize that aforementioned delta.
    double turn = turnController.calculate(angleDelta);
    // getRelativeTranslation returns a RELATIVE translation (meaning it
    // can be applied to the robot)
    Translation translation = Follower.getRelativeTranslation(current, nextMarker, speed, turn);
    Logger.trace(GenericFollower.class, "Next marker: <%s>, speed: <%s>, angle delta: <%s deg>, " + "turn value: <%s>, current translation: <%s>", nextMarker, speed, angleDelta, turn, translation);
    // use the consumer to accept a translation we create.
    // instead of creating an absolute translation, which would only help
    // if the robot is facing straight forwards, we have to use a relative
    // translation. the follower interface's static method that we
    // call here (getRelativeTranslation) first generates an absolute
    // translation and then converts it to a relative one. epic sauce.
    consumer.accept(translation);
    // we're not done yet, so return false.
    return false;
}

Example 3 with Follower

use of me.wobblyyyy.pathfinder2.follower.Follower 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 4 with Follower

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

Example 5 with Follower

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