Examples with Translation me.wobblyyyy.pathfinder2.geometry.Translation used on opensource projects

Example 11 with Translation

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

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 13 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 14 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation in project Pathfinder2 by Wobblyyyy.

the class AutoRotator method apply.

@Override
public Translation apply(Translation translation) {
    if (!isActive)
        return translation;
    if (centerPoint == null)
        throw new NullPointerException("Attempted to use an AutoRotator without having previously " + "set a center point, make sure to use the" + "setCenterPoint(PointXY) method to do that.");
    PointXYZ position = pathfinder.getPosition();
    double delta = Angle.minimumDelta(position.z(), position.angleTo(centerPoint).add(angleOffset));
    return translation.withVz(turnController.calculate(delta));
}

Example 15 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation in project Pathfinder2 by Wobblyyyy.

the class MecanumKinematics method toTranslation.

public Translation toTranslation(MecanumState state) {
    SimpleMatrix wheelSpeedsMatrix = new SimpleMatrix(4, 1);
    wheelSpeedsMatrix.setColumn(0, 0, state.fl(), state.fr(), state.bl(), state.br());
    SimpleMatrix speedVector = forwardsKinematics.mult(wheelSpeedsMatrix);
    return new Translation(speedVector.get(0, 0), speedVector.get(1, 0), speedVector.get(2, 0));
}