Examples
Three files that wire HiveVision into a driving robot.
Drop these into TeamCode. Every number in them is a placeholder, so tune the constants before trusting a path on the field.
Follower
Section titled “Follower”Pedro Pathing 3 dropped FollowerBuilder. A follower is assembled from a localizer, a drivetrain and Foresight instead.
package org.firstinspires.ftc.teamcode;
import com.pedropathing.algorithm.Foresight;import com.pedropathing.algorithm.ForesightConfig;import com.pedropathing.controllers.Controller;import com.pedropathing.follower.Follower;import com.pedropathing.math.Matrix;import com.pedropathing.math.Vector2D;import com.pedropathing.revhub.drivetrains.Mecanum;import com.pedropathing.revhub.drivetrains.MecanumConfig;import com.pedropathing.revhub.localizers.RevHubIMU;import com.pedropathing.revhub.localizers.ThreeWheelIMUConfig;import com.pedropathing.revhub.localizers.ThreeWheelIMULocalizer;import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;import com.qualcomm.robotcore.hardware.DcMotorSimple;import com.qualcomm.robotcore.hardware.HardwareMap;
/** * Pedro Pathing 3.x follower construction - the piece every ball-chase OpMode * needs. Pedro 3 dropped the 2.x FollowerBuilder; a follower is assembled from * three parts: {@code new Follower(localizer, drivetrain, algorithm)}. * * <p>Every number and name below is a placeholder so the file compiles. TUNE ME * for the actual robot before trusting it on the field: motor names, encoder * names, pod offsets, ticks-to-inches, hub orientation and Foresight gains. * * <p>Copy this file into TeamCode next to the OpModes below. */public final class SampleFollowerFactory {
/* ---- TUNE ME: match your robot config ---- */ public static final String LEFT_FRONT = "leftFront"; // also the left encoder public static final String LEFT_REAR = "leftRear"; public static final String RIGHT_FRONT = "rightFront"; // also the right encoder public static final String RIGHT_REAR = "rightRear"; // also the strafe encoder public static final String IMU_NAME = "imu";
/* ---- TUNE ME: drivetrain speed limits (in/s) ---- */ private static final double MAX_FORWARD_VELOCITY = 50.0; private static final double MAX_STRAFE_VELOCITY = 50.0;
/* ---- TUNE ME: how fast the robot can shed speed (in/s^2) ---- */ private static final double NATURAL_FORWARD_DECELERATION = 60.0; private static final double NATURAL_STRAFE_DECELERATION = 60.0;
private SampleFollowerFactory() {}
/** Build a Pedro 3 Follower from the hardware map (placeholder values). */ public static Follower create(HardwareMap hardwareMap) { MecanumConfig mecanumConfig = new MecanumConfig(c -> { c.frontLeftName.set(LEFT_FRONT); c.backLeftName.set(LEFT_REAR); c.frontRightName.set(RIGHT_FRONT); c.backRightName.set(RIGHT_REAR);
c.frontLeftDirection.set(DcMotorSimple.Direction.FORWARD); c.backLeftDirection.set(DcMotorSimple.Direction.FORWARD); c.frontRightDirection.set(DcMotorSimple.Direction.REVERSE); c.backRightDirection.set(DcMotorSimple.Direction.REVERSE); });
ThreeWheelIMUConfig localizerConfig = new ThreeWheelIMUConfig(c -> { c.leftEncoderName.set(LEFT_FRONT); c.rightEncoderName.set(RIGHT_FRONT); c.strafeEncoderName.set(RIGHT_REAR); c.imuName.set(IMU_NAME);
// TUNE ME: pod offsets in inches c.leftPodY.set(5.0); c.rightPodY.set(-5.0); c.strafePodX.set(2.0);
// TUNE ME: ticks -> inches / radians c.forwardTicksToInches.set(0.05); c.strafeTicksToInches.set(0.05); c.turnTicksToRadians.set(0.05);
c.leftEncoderDirection.set(1.0); c.rightEncoderDirection.set(-1.0); c.strafeEncoderDirection.set(-1.0);
// TUNE ME: hub orientation c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.FORWARD))); });
ForesightConfig foresightConfig = new ForesightConfig(c -> { // TUNE ME: Foresight gains c.headingFeedback.set(Controller.pid(0.6, 0.0, 0.015)); c.headingStaticFF.set(Controller.staticFeedforward(0.0)); c.forwardTranslational.set(Controller.pid(0.1, 0.0, 0.0)); c.strafeTranslational.set(Controller.pid(0.1, 0.0, 0.0)); c.brake.set(Controller.pid(0.025, 0.0, 0.0)); c.coast.set(Controller.pid(0.025, 0.0, 0.0));
// TUNE ME: Foresight braking. linear acts on v*|v|, quadratic on v*|v|^2. c.linearBrakeCoefficients.set(Matrix.diag(Vector2D.cartesian(0.005, 0.005))); c.quadraticBrakeCoefficients.set(Matrix.diag(Vector2D.cartesian(0.0005, 0.0005))); // x = quadratic (omega^2) coefficient, y = linear (omega) coefficient c.headingBrakeCoefficients.set(Vector2D.cartesian(0.004, 0.02));
c.maxAchievableForwardVelocity.set(MAX_FORWARD_VELOCITY); c.maxAchievableStrafeVelocity.set(MAX_STRAFE_VELOCITY); c.naturalForwardDeceleration.set(NATURAL_FORWARD_DECELERATION); c.naturalStrafeDeceleration.set(NATURAL_STRAFE_DECELERATION); });
Mecanum drivetrain = new Mecanum(hardwareMap, mecanumConfig); ThreeWheelIMULocalizer localizer = new ThreeWheelIMULocalizer(hardwareMap, localizerConfig); Foresight algorithm = new Foresight(foresightConfig);
return new Follower(localizer, drivetrain, algorithm); }}Detect, plan, drive
Section titled “Detect, plan, drive”Look once, plan one route, follow it.
package org.firstinspires.ftc.teamcode;
import com.pedropathing.follower.Follower;import com.skeletonarmy.marrow.weaver.PathResult;import com.skeletonarmy.marrow.weaver.PedroPathingConverter;import com.qualcomm.hardware.limelightvision.Limelight3A;import com.qualcomm.robotcore.eventloop.opmode.Autonomous;import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;import java.util.List;import org.hivevisionftc.vision.BiobuzzClasses;import org.hivevisionftc.vision.Detection;import org.hivevisionftc.vision.HiveVision;import org.hivevisionftc.vision.Pose;import org.hivevisionftc.vision.VisionConfig;import org.hivevisionftc.vision.weaver.WeaverRoute;
/** * Autonomous OpMode demonstrating target detection and Weaver route generation using HiveVision. */@Autonomous(name = "HiveVision Weaver Auto")public class AutoBallHuntOpMode extends LinearOpMode {
@Override public void runOpMode() { Follower follower = SampleFollowerFactory.create(hardwareMap); Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight");
// Define vision configuration parameters VisionConfig config = new VisionConfig() .cameraPitchDeg(-25.0) .cameraHeightIn(9.2) .targetHeightIn(3.0) .cameraForwardIn(4.0) .cameraLeftIn(0.0) .minConfidence(0.44) .maxStalenessMs(120);
// Initialize HiveVision session HiveVision vision = new HiveVision(limelight, config) .setAllowedClasses(BiobuzzClasses.NECTAR_RED, BiobuzzClasses.NECTAR_BLUE);
telemetry.addData("Status", "Ready - press START"); telemetry.update(); waitForStart();
if (opModeIsActive()) { // Obtain current robot pose // Pedro Pathing 3.x accessors; 2.x used follower.getPose().getX() Pose robotPose = new Pose( follower.pose().x(), follower.pose().y(), follower.pose().heading());
// Fetch detections and generate a Weaver path route List<Detection> detections = vision.fetchDetections(robotPose); PathResult pathResult = WeaverRoute.from(robotPose).maxTargets(2).plan(detections);
if (pathResult != null) { follower.follow(PedroPathingConverter.toPath(pathResult)); while (opModeIsActive() && follower.isBusy()) { follower.update(); telemetry.addData("Status", "Following generated route"); telemetry.update(); } } else { telemetry.addData("Status", "No targets detected"); telemetry.update(); } } }}Re-plan while driving
Section titled “Re-plan while driving”Stays in the loop, plans again whenever the last path finishes, and bends around a zone.
package org.firstinspires.ftc.teamcode;
import com.pedropathing.follower.Follower;import com.skeletonarmy.marrow.weaver.PathResult;import com.skeletonarmy.marrow.weaver.PedroPathingConverter;import com.skeletonarmy.marrow.zones.CircleZone;import com.skeletonarmy.marrow.zones.Point;import com.qualcomm.hardware.limelightvision.Limelight3A;import com.qualcomm.robotcore.eventloop.opmode.Autonomous;import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;import java.util.List;import org.hivevisionftc.vision.BiobuzzClasses;import org.hivevisionftc.vision.Detection;import org.hivevisionftc.vision.HiveVision;import org.hivevisionftc.vision.Pose;import org.hivevisionftc.vision.VisionConfig;import org.hivevisionftc.vision.weaver.WeaverRoute;
/** * Autonomous OpMode demonstrating continuous detection loop, obstacle avoidance, * and path execution using HiveVision and WeaverRoute. */@Autonomous(name = "HiveVision Loop with Obstacles")public class ManualBallChaseOpMode extends LinearOpMode {
@Override public void runOpMode() { Follower follower = SampleFollowerFactory.create(hardwareMap); Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight");
VisionConfig config = new VisionConfig() .cameraPitchDeg(-25.0) .cameraHeightIn(9.2) .targetHeightIn(3.0) .cameraForwardIn(4.0) .cameraLeftIn(0.0) .minConfidence(0.40) .maxStalenessMs(120);
HiveVision vision = new HiveVision(limelight, config) .setAllowedClasses(BiobuzzClasses.NECTAR_RED, BiobuzzClasses.NECTAR_BLUE);
telemetry.addData("Status", "Ready - press START"); telemetry.update(); waitForStart();
while (opModeIsActive()) { follower.update();
// Pedro Pathing 3.x accessors; 2.x used follower.getPose().getX() Pose currentPose = new Pose( follower.pose().x(), follower.pose().y(), follower.pose().heading());
List<Detection> detections = vision.fetchDetections(currentPose);
if (!detections.isEmpty()) { Detection closest = detections.get(0); telemetry.addData("Closest Element", closest.getType()); telemetry.addData("Field X", closest.getX()); telemetry.addData("Field Y", closest.getY());
if (!follower.isBusy()) { PathResult path = WeaverRoute.from(currentPose) .avoid(new CircleZone(new Point(72, 72), 18.0)) .maxTargets(3) .plan(detections);
if (path != null) { follower.follow(PedroPathingConverter.toPath(path)); } } } else { telemetry.addData("Status", "Searching for targets..."); }
telemetry.update(); } }}