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

Example 11 with Robot

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

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

the class SwerveDrive method setTranslation.

/**
 * Set a translation to the robot. In the case of this swerve chassis,
 * this does a couple of things. Firstly, it applies the modifier to
 * the inputted translation. Secondly, it calculates a swerve chassis
 * state based on the translation. Thirdly, it determines the swerve
 * module states of each individual module. And finally, it sets the
 * state to each of these modules, making the robot move.
 *
 * @param translation a translation the robot should act upon. This
 *                    translation should always be <em>relative</em>,
 *                    meaning whatever the translation says should make
 *                    the robot act accordingly according to the robot's
 *                    position and the robot's current heading. I'm
 *                    currently exhausted and just about entirely unable
 *                    to type, so this isn't coherent, but guess what -
 */
@Override
public void setTranslation(Translation translation) {
    translation = modifier.apply(translation);
    this.translation = translation;
    RelativeSwerveState state = kinematics.calculate(translation);
    RelativeSwerveModuleState frState = state.fr();
    RelativeSwerveModuleState flState = state.fl();
    RelativeSwerveModuleState brState = state.br();
    RelativeSwerveModuleState blState = state.bl();
    this.frontRightModule.set(frState);
    this.frontLeftModule.set(flState);
    this.backRightModule.set(brState);
    this.backLeftModule.set(blState);
}

Example 13 with Robot

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

the class Follower method getRelativeTranslation.

/**
 * Get a relative translation by first generating an absolute translation
 * and then converting it into a relative one.
 *
 * @param current the robot's current position.
 * @param target  the robot's target point / marker.
 * @param speed   the speed at which the robot should move.
 * @param turn    how much the robot should turn.
 * @return a relative translation. This translation, like any absolute
 * translation, will always have X and Y values that fit within the bounds
 * (-1.0, 1.0). The Z aspect of the translation will always be equal to
 * whatever turn value is provided.
 */
static Translation getRelativeTranslation(PointXYZ current, PointXYZ target, double speed, double turn) {
    Translation absoluteTranslation = getAbsoluteTranslation(current, target, speed, turn);
    Translation relativeTranslation = absoluteTranslation.toRelative(current.z());
    Logger.trace(Follower.class, "absolute translation: <%s> relative translation: <%s> " + "current: <%s> target: <%s> speed: <%s> turn: <%s>", absoluteTranslation, relativeTranslation, current, target, speed, turn);
    return relativeTranslation;
}

Example 14 with Robot

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

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

the class PathOptimizer method optimize.

/**
 * Optimize a path by removing collinear points. This reduces the size
 * of the path, and, in some cases, reduces the length of the path by
 * allowing your robot to move in ways it wouldn't be able to have
 * previously.
 *
 * @param path the path to optimize.
 * @return an optimized path.
 */
public static List<PointXY> optimize(List<PointXY> path) {
    List<PointXY> optimized = new ArrayList<>(path.size());
    PointXY start = path.get(0);
    PointXY end = path.get(path.size() - 1);
    // remove collinear points
    PointXY lastCollinear = null;
    for (int i = 1; i < path.size() - 1; i++) {
        PointXY previous = path.get(i - 1);
        PointXY current = path.get(i);
        PointXY next = path.get(i + 1);
        if (PointXY.areCollinear(previous, current, next)) {
            lastCollinear = current;
        } else {
            if (lastCollinear == null) {
                optimized.add(current);
            } else {
                optimized.add(lastCollinear);
                lastCollinear = null;
            }
        }
    }
    // if there's still a collinear point to add, add it
    if (lastCollinear != null)
        optimized.add(lastCollinear);
    if (optimized.size() == 0)
        return new ArrayList<>();
    // make sure the optimized path includes the start and end points
    if (!optimized.get(0).equals(start))
        optimized.add(0, start);
    if (!optimized.get(optimized.size() - 1).equals(end))
        optimized.add(end);
    // edge case - if the path is entirely linear, return a 2-point
    // path
    int size = optimized.size();
    if (size == 3) {
        PointXY a = optimized.get(0);
        PointXY b = optimized.get(1);
        PointXY c = optimized.get(2);
        if (PointXY.areCollinear(a, b, c))
            optimized.remove(b);
        return optimized;
    }
    // do a second pass to further optimize the path
    List<PointXY> toRemove = new ArrayList<>(optimized.size());
    for (int i = 1; i < size - 1; i++) {
        PointXY previous = path.get(i - 1);
        PointXY current = path.get(i);
        PointXY next = path.get(i + 1);
        if (PointXY.areCollinear(previous, current, next)) {
            toRemove.add(current);
        }
    }
    for (PointXY point : toRemove) {
        optimized.remove(point);
    }
    return optimized;
}