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

Example 21 with Translation

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

the class MecanumDrive method setTranslation.

/**
 * Set a translation to the drivetrain. This translation should
 * always be relative to the robot, not relative to the field,
 * yourself, or any other stationary object.
 *
 * @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.
 */
@Override
public void setTranslation(Translation translation) {
    this.translation = getDriveModifier().apply(translation);
    MecanumState state = kinematics.calculate(this.translation);
    fr.setPower(state.fr());
    fl.setPower(state.fl());
    br.setPower(state.br());
    bl.setPower(state.bl());
}

Example 22 with Translation

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

the class Follower method getAbsoluteTranslation.

/**
 * Get an absolute translation based on a couple of parameters.
 *
 * <p>
 * This method functions as follows.
 * <ul>
 *     <li>
 *         Determine the angle from the robot's current position to
 *         the robot's target position.
 *     </li>
 *     <li>
 *         Create an imaginary point using the lovely
 *         {@link PointXYZ#inDirection(double, Angle)} method. This point
 *         will always be drawn at a distance of 1. The point's angle
 *         is equal to the angle we just calculated.
 *     </li>
 *     <li>
 *         Convert that point into a translation by getting the X and Y
 *         values of the point. The generated translation simply
 *         re-uses the {@code turn} parameter that's passed into
 *         this method.
 *     </li>
 * </ul>
 * </p>
 *
 * @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 an absolute translation. This translation will always have X
 * and Y values that fit within the bounds (-1.0, 1.0). This translation
 * is also absolute, NOT relative.
 */
static Translation getAbsoluteTranslation(PointXYZ current, PointXYZ target, double speed, double turn) {
    ValidationUtils.validate(current, "current");
    ValidationUtils.validate(target, "target");
    ValidationUtils.validate(speed, "speed");
    ValidationUtils.validate(turn, "turn");
    if (PointXY.equals(current, target))
        return Translation.ZERO.withVz(turn);
    Angle angle = current.angleTo(target).fix();
    PointXYZ targetPoint = PointXYZ.ZERO.inDirection(speed, angle);
    return new Translation(targetPoint.x(), targetPoint.y(), turn);
}

Example 23 with Translation

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

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

the class ExampleTeleOp method exampleTeleOpListeners.

@SuppressWarnings("deprecation")
public void exampleTeleOpListeners() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    AtomicReference<Double> multiplier = new AtomicReference<>(0.5);
    Shifter shifter = new Shifter(1, 1, 5, false, i -> {
    });
    Joystick right = new Joystick(() -> 0d, () -> 0d);
    Joystick left = new Joystick(() -> 0d, () -> 0d);
    Button a = new Button(() -> false);
    Button b = new Button(() -> false);
    Button rightBumper = new Button(() -> false);
    Button leftBumper = new Button(() -> false);
    // bind the following controls:
    // a button       -> shift elevator up a gear
    // b button       -> shift elevator down a gear
    // right bumper   -> set the multiplier to 1.0 (full speed)
    // left bumper    -> set the multiplier to 0.25 (slowest speed)
    // neither bumper -> set the multiplier to 0.5 (normal speed)
    pathfinder.getListenerManager().bind(// whenever the right bumper is pressed (even if held)...
    ListenerMode.CONDITION_IS_MET, rightBumper::isPressed, isPressed -> isPressed, isPressed -> multiplier.set(1.0)).bind(// whenever the left bumper is pressed (even if held)...
    ListenerMode.CONDITION_IS_MET, leftBumper::isPressed, isPressed -> isPressed, isPressed -> multiplier.set(0.25)).bind(// whenever neither bumper is pressed...
    ListenerMode.CONDITION_IS_NOT_MET, () -> SupplierFilter.anyTrue(rightBumper::isPressed, leftBumper::isPressed), isPressed -> isPressed, isPressed -> multiplier.set(0.5)).bind(// whenever the a button is initially pressed...
    ListenerMode.CONDITION_NEWLY_MET, () -> SupplierFilter.trueThenAllTrue(// a must be pressed
    a::isPressed, // a must NOT be pressed
    b::isPressed), isPressed -> isPressed, isPressed -> shifter.shift(ShifterDirection.UP)).bind(// whenever the b button is initially pressed...
    ListenerMode.CONDITION_NEWLY_MET, () -> SupplierFilter.trueThenAllTrue(// b must be pressed
    a::isPressed, // a must NOT be pressed
    b::isPressed), isPressed -> isPressed, isPressed -> shifter.shift(ShifterDirection.DOWN));
    pathfinder.onTick(pf -> {
        double m = multiplier.get();
        double vertical = right.getX();
        double horizontal = right.getY();
        double turn = left.getX();
        Translation translation = new Translation(vertical * m, horizontal * m, turn * m);
        pf.setTranslation(translation);
    }).onTick(pf -> {
    // some magic code that updates the elevator based on
    // what level it's currently on and what level it's
    // trying to get to
    });
    while (true) pathfinder.tick();
}

Example 25 with Translation

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

the class ExampleTimedRobot method teleopPeriodic.

@Override
public void teleopPeriodic() {
    // during teleop, set the robot's translation by using some joystick
    // values. the left stick controls X/Y movement, and the right stick
    // controls the robot's rotation
    pathfinder.tick();
    double vx = leftJoystick.getX();
    double vy = leftJoystick.getY();
    double vz = rightJoystick.getX();
    Translation translation = new Translation(vx, vy, vz);
    pathfinder.setTranslation(translation);
}