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

Example 26 with Robot

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

the class CircleSurround method closestPointBetweenAngles.

/**
 * Get the closest point along a circle between two given angles.
 * I don't really know how to explain this, so if you're confused,
 * just look at the code and try and figure it out.
 *
 * @param robotPosition the robot's current position.
 * @param center        the circle's center point.
 * @param radius        the radius of the circle.
 * @param minimumAngle  the minimum angle.
 * @param maximumAngle  the maximum angle.
 * @return the point along a circle closest to the robot. This point's
 * heading will be facing towards the center of the circle.
 */
public static PointXYZ closestPointBetweenAngles(PointXYZ robotPosition, PointXY center, double radius, Angle minimumAngle, Angle maximumAngle) {
    if (robotPosition == null)
        throw new NullPointException("Robot position may not be null!");
    if (center == null)
        throw new NullPointException("Center point may not be null!");
    if (radius < 0)
        throw new IllegalArgumentException("Radius values must be greater than 0!");
    Angle centerToRobot = center.angleTo(robotPosition).angleWithMinAndMax(minimumAngle, maximumAngle);
    Angle robotToCenter = robotPosition.angleTo(center);
    return center.inDirection(radius, centerToRobot).withHeading(robotToCenter);
}

Example 27 with Robot

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

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

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

the class ExampleRecording method init.

public void init() {
    drive = new SimulatedDrive();
    odometry = new SimulatedOdometry();
    robot = new Robot(drive, odometry);
    pathfinder = new Pathfinder(robot, 0.02);
}