A bunch of random work on Subsystems, lots of work left to do

src/main/java/first/robot/constants/CompetitionConstants.java

@@ -1,5 +1,6 @@
 package first.robot.constants;
 
+import org.wpilib.fields.Field;
 import org.wpilib.fields.Fields;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
@@ -7,7 +8,7 @@ import org.wpilib.math.geometry.Transform3d;
 import org.wpilib.math.util.Units;
 
 public class CompetitionConstants {
-    public static final Fields kTagLayout = Fields.FRC_2026_REBUILT_ANDY_MARK;
+    public static final Field kField = Fields.FRC_2026_REBUILT_ANDY_MARK.loadField();
 
     public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
 

src/main/java/first/robot/constants/DrivetrainConstants.java

@@ -1,5 +1,6 @@
 package first.robot.constants;
 
+import org.wpilib.hardware.bus.CANPort;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.kinematics.SwerveDriveKinematics;
 import org.wpilib.math.linalg.Matrix;
@@ -20,6 +21,8 @@ public class DrivetrainConstants {
     public static final double kRearLeftMagEncoderOffset = 3.761;
     public static final double kRearRightMagEncoderOffset = 2.573;
 
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
     public static final int kFrontLeftDrivingCANID = 4;
     public static final int kFrontRightDrivingCANID = 3;
     public static final int kRearLeftDrivingCANID = 1;

src/main/java/first/robot/constants/HoodConstants.java

@@ -2,6 +2,7 @@ package first.robot.constants;
 
 import java.util.Map;
 
+import org.wpilib.hardware.bus.CANPort;
 import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
 import org.wpilib.math.util.Units;
 
@@ -14,6 +15,8 @@ import first.robot.constants.ShooterConstants.ShooterSpeeds;
 
 public class HoodConstants {
     // TODO Real Values
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+    
     public static final int kMotorCANID = 12;
 
     public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;

src/main/java/first/robot/constants/IntakeRollerConstants.java

@@ -17,7 +17,8 @@ public class IntakeRollerConstants {
     public static final boolean kInvertLeftMotor = false;
     public static final boolean kInvertRightMotor = true;
 
-    public static final double kSpeed = 1;
+    public static final double kIntakeInSpeed = .9;
+    public static final double kIntakeOutSpeed = 1;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 

src/main/java/first/robot/constants/ModuleConstants.java

@@ -1,5 +1,6 @@
 package first.robot.constants;
 
+import org.wpilib.hardware.bus.CANPort;
 import org.wpilib.math.util.Units;
 
 import com.ctre.phoenix6.configs.AudioConfigs;

src/main/java/first/robot/constants/SystemCoreConstants.java

@@ -0,0 +1,7 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
+
+public class SystemCoreConstants {
+    public static final MountOrientation kSystemCoreMountOrientation = MountOrientation.FLAT;
+}

src/main/java/first/robot/subsystems/Drivetrain.java

@@ -0,0 +1,444 @@
+package first.robot.subsystems;
+
+import java.util.List;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.PrintCommand;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.hardware.imu.OnboardIMU;
+import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+import org.wpilib.math.util.MathUtil;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import first.robot.constants.AutoConstants;
+import first.robot.constants.DrivetrainConstants;
+import first.robot.constants.ModuleConstants.ModuleName;
+import first.robot.constants.OIConstants;
+import first.robot.constants.SystemCoreConstants;
+import first.robot.utilities.SwerveModule;
+import first.robot.utilities.Utilities;
+import first.robot.utilities.VisualPose;
+
+public class Drivetrain extends SubsystemBase {
+    private SwerveModule frontLeft;
+    private SwerveModule frontRight;
+    private SwerveModule rearLeft;
+    private SwerveModule rearRight;
+
+    private OnboardIMU imu;
+
+    private SwerveDrivePoseEstimator estimator;
+
+    private PIDController yawRotationController;
+
+    public Drivetrain(Pose2d startupPose) {
+        frontLeft = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kFrontLeft, 
+            DrivetrainConstants.kFrontLeftDrivingCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftAnalogInPort, 
+            DrivetrainConstants.kFrontLeftMagEncoderOffset
+        );
+
+        frontRight = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kFrontRight, 
+            DrivetrainConstants.kFrontRightDrivingCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightAnalogInPort, 
+            DrivetrainConstants.kFrontRightMagEncoderOffset
+        );
+
+        rearLeft = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kRearLeft, 
+            DrivetrainConstants.kRearLeftDrivingCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftAnalogInPort, 
+            DrivetrainConstants.kRearLeftMagEncoderOffset
+        );
+
+        rearRight = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kRearRight, 
+            DrivetrainConstants.kRearRightDrivingCANID, 
+            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightAnalogInPort, 
+            DrivetrainConstants.kRearRightMagEncoderOffset
+        );
+
+        imu = new OnboardIMU(SystemCoreConstants.kSystemCoreMountOrientation);
+
+        yawRotationController = new PIDController(
+            DrivetrainConstants.kHeadingP, 
+            0, 
+            0
+        );
+        yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
+        yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance);
+
+        // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
+        estimator = new SwerveDrivePoseEstimator(
+            DrivetrainConstants.kDriveKinematics, 
+            Rotation2d.fromDegrees(getGyroValue()), 
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            startupPose != null ? startupPose : new Pose2d(),
+            DrivetrainConstants.kSensorFusionOdometryStdDevs,
+            DrivetrainConstants.kVisionOdometryStdDevs
+        );
+
+        if(AutoConstants.kAutoConfigOk) {
+            AutoBuilder.configure(
+                this::getPose, 
+                this::resetOdometry, 
+                this::getCurrentChassisSpeeds, 
+                (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), 
+                AutoConstants.kPPDriveController, 
+                AutoConstants.kRobotConfig,
+                () -> {
+                    Optional<Alliance> alliance = MatchState.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == Alliance.RED;
+                        }
+                        return false;
+                }, 
+                this
+            );
+        }
+    }
+
+    @Override
+    public void periodic() {
+        estimator.update(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            }
+        );
+
+        frontLeft.periodic();
+        frontRight.periodic();
+        rearLeft.periodic();
+        rearRight.periodic();
+
+        Logger.recordOutput("Drivetrain/Pose", getPose());
+        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
+        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
+        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
+    }
+
+    /**
+     * Can be used to run an individual module on the drive base a static speed while maintaining a static angle.
+     * 
+     * Good for diagnosing issues with swerve module configuration. Essentially useless otherwise.
+     * 
+     * @param name The ModuleName enumeration that indicates which module you want to control
+     * @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at
+     * @param staticAngleDegrees The static angle in degrees that you want the wheel to face
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) {
+        SwerveModule module = List.of(
+            frontLeft,
+            frontRight,
+            rearLeft,
+            rearRight
+        ).stream()
+        .filter((m) -> m.getModuleName() == name)
+        .findFirst()
+        .get();
+
+        return run(() -> {
+            module.setDesiredState(new SwerveModuleVelocity(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)
+            ));
+        });
+    }
+
+    public Command disableOutputs() {
+        return run(() -> {
+            frontLeft.disableOutput();
+            frontRight.disableOutput();
+            rearLeft.disableOutput();
+            rearRight.disableOutput();
+        });  
+    }
+
+    /**
+     * Rotates the robot to a face a given Pose2d position on the field
+     * 
+     * Note that this Command does not provide a means of timeout. If you are
+     * using this in an auto context, this Command should be decorated with 
+     * withTimeout(<some_value>). Otherwise, you will be waiting for the PID
+     * Controller doing the work to report that it is at the desired setpoint. 
+     * 
+     * @param targetPose The Pose2d object to rotate the robot towards
+     * @param rotate180 When false, the front of the robot faces the specified pose, when true
+     *   the back of the robot faces the specified pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
+            .until(yawRotationController::atSetpoint);
+    }
+
+    /**
+     * Locks the robots rotation to face the Alliance Hub on the field. 
+     * 
+     * This method is innately aware of which hub to face based on the assigned alliance color.
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the center of the alliance hub. This
+     * method assumes that the robots estimated pose is reasonably accurate. 
+     * 
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the hub, when true, the back
+     *   of the robot faces the hub
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return lockRotationToSuppliedPose(
+            Utilities::getHubPose, 
+            xSpeed, 
+            ySpeed, 
+            rotate180
+        );
+    }
+
+    /**
+     * Locks the robots rotation to face a particular pose on the field
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the Pose2d object being supplied. This 
+     * method assumes that the robots estimated pose is reasonably accurate.
+     * 
+     * @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
+     *   of the robot faces the supplied pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> {
+                    Pose2d faceTowards = poseSupplier.get();
+
+                    Rotation2d targetRotation = new Rotation2d(
+                        faceTowards.getX() - getPose().getX(), 
+                        faceTowards.getY() - getPose().getY()
+                    );
+
+                    if(rotate180) {
+                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
+                    }
+
+                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
+                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
+
+                    double outputPower = -yawRotationController.calculate(
+                        getHeading().getRadians(),
+                        targetRotation.getRadians()
+                    );
+
+                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
+
+                    return outputPower;
+                }, 
+                () -> true
+            )
+        );
+    }
+
+    /**
+     * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from
+     * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced
+     * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in
+     * other terms, to center the provided tag in the camera's image frame.
+     * 
+     * @param yaw The "yaw" of the tag source relative to the center of the image frame
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
+                () -> true
+            )
+        );
+    }
+
+    public Command drivePathPlannerPath(PathPlannerPath path) {
+        if(AutoConstants.kAutoConfigOk) {
+            return AutoBuilder.followPath(path);
+        } else {
+            return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled");
+        }
+        
+    }
+
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
+        // TODO Specific Alliance code?
+        return run(() -> {
+            drive(
+                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
+                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
+                fieldRelative.getAsBoolean()
+            );
+        });
+    }
+
+    public Command setX() {
+        return run(() -> {
+            frontLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45)));
+            frontRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45)));
+            rearLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45)));
+            rearRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45)));
+        });
+    }
+
+    public Command zeroHeading() {
+        return run(() -> {
+            imu.resetYaw();;
+            estimator.resetRotation(new Rotation2d(0));
+        });
+    }
+
+    public void consumeVisualPose(VisualPose pose) {
+        if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) {
+            return;
+        }
+        
+        estimator.addVisionMeasurement(
+            pose.visualPose(), 
+            pose.timestamp()
+        );
+    }
+
+    public void resetEncoders() {
+        frontLeft.resetEncoders();
+        frontRight.resetEncoders();
+        rearLeft.resetEncoders();
+        rearRight.resetEncoders();
+    }
+
+    public void resetOdometry(Pose2d pose) {
+        estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            pose
+        );
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+        double xSpeedDelivered = 0;
+        double ySpeedDelivered = 0;
+
+        if(p != 0){
+            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        }else{
+            xSpeedDelivered = 0;
+            ySpeedDelivered = 0;
+        }
+
+        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
+
+        ChassisVelocities velocities = new ChassisVelocities(xSpeedDelivered, ySpeedDelivered, rotationDelivered);
+
+        SwerveModuleVelocity[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities(
+            fieldRelative ?
+                velocities.toFieldRelative(Rotation2d.fromDegrees(getGyroValue())) : velocities 
+        );
+
+        setModuleStates(swerveModuleStates);
+    }
+
+    public void driveWithChassisSpeeds(ChassisVelocities speeds) {
+        ChassisVelocities discreteSpeeds = speeds.discretize(0.2);
+        SwerveModuleVelocity[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities(discreteSpeeds);
+
+        setModuleStates(newStates);
+    }
+
+    public void setModuleStates(SwerveModuleVelocity[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+        frontLeft.setDesiredState(desiredStates[0]);
+        frontRight.setDesiredState(desiredStates[1]);
+        rearLeft.setDesiredState(desiredStates[2]);
+        rearRight.setDesiredState(desiredStates[3]);
+    }
+
+    public ChassisVelocities getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisVelocities(
+            frontLeft.getState(),
+            frontRight.getState(),
+            rearLeft.getState(),
+            rearRight.getState()
+        );
+    }
+
+    public Pose2d getPose() {
+        return estimator.getEstimatedPosition();
+    }
+
+    public double getGyroValue() {
+        return imu.getYawRadians() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+    }
+
+    public Rotation2d getHeading() {
+        return estimator.getEstimatedPosition().getRotation();
+    }
+
+    public double getHeadingDegrees() {
+        return estimator.getEstimatedPosition().getRotation().getDegrees();
+    }
+}
+

src/main/java/first/robot/subsystems/Hood.java

@@ -0,0 +1,183 @@
+package first.robot.subsystems;
+
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
+import org.wpilib.math.util.MathUtil;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.HoodConstants;
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+import first.robot.utilities.Utilities;
+
+public class Hood extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    //private Trigger resetTrigger;
+    //private Trigger timerTrigger;
+
+    //private Timer resetTimer;
+
+    private double currentTargetDegrees;
+
+    public Hood() {
+        motor = new SparkMax(
+            HoodConstants.kCANPort, 
+            HoodConstants.kMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        motor.configure(
+            HoodConstants.kConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );   
+
+        encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
+
+        controller = motor.getClosedLoopController();
+
+        /*resetTimer = new Timer();
+        resetTimer.reset();
+
+        resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
+        resetTrigger.onTrue(new InstantCommand(resetTimer::start));
+        resetTrigger.onFalse(new InstantCommand(() -> {
+            resetTimer.stop();
+            resetTimer.reset();
+        }));
+
+        timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset));
+        timerTrigger.onTrue(new InstantCommand(() -> {
+            encoder.setPosition(0);
+            resetTimer.reset();
+        }));*/
+
+        currentTargetDegrees = HoodConstants.kStartupAngle;
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber(
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        SmartDashboard.putBoolean(
+            "HoodAtSetpoint", 
+            controller.isAtSetpoint()
+        );
+        
+        Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
+        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
+    }
+
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
+    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
+        return run(() -> {
+            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
+        });
+    }
+
+    /**
+     * An automated form of resetting the hood position sensing.
+     * 
+     * Run down at the full manual speed (note that this is affected by the 
+     * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true
+     * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) 
+     * for reset for the amount of specified by kTimeAboveThresholdToReset). Once
+     * that returns true, the motor is stopped until the timer trigger switches to false
+     * (i.e. it has reset the position automatically, because that's how it's configured, 
+     * and resets the timer to 0, which makes the timer trigger false)
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezero() {
+        return manualSpeed(() -> -1)
+            .until(timerTrigger)
+            .andThen(
+                stop().until(timerTrigger.negate())
+        );
+    }
+
+    /**
+     * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers
+     * to reset the hood position to zero. Note that this method doesn't have any time limiting
+     * factor to it, as soon as the current goes above the threshold specified by 
+     * kAmpsToTriggerPositionReset the encoder position will be set to zero
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezeroNoTimer() {
+        return manualSpeed(() -> -1)
+            .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
+            .andThen(new InstantCommand(() -> encoder.setPosition(0)));
+    }*/
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        currentTargetDegrees = 0;
+
+        return run(() -> {
+            motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public double getTargetDegrees() {
+        return currentTargetDegrees;
+    }
+}

src/main/java/first/robot/subsystems/IntakePivot.java

@@ -0,0 +1,125 @@
+package first.robot.subsystems;
+
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.command2.SubsystemBase;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.IntakePivotConstants;
+import first.robot.constants.IntakePivotConstants.IntakePivotPosition;
+
+public class IntakePivot extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private IntakePivotPosition currentTargetPosition;
+
+    public IntakePivot() {
+        leftMotor = new SparkMax(
+            IntakePivotConstants.kCANPort,
+            IntakePivotConstants.kLeftMotorCANID, 
+            MotorType.kBrushless
+        );
+        rightMotor = new SparkMax(
+            IntakePivotConstants.kCANPort,
+            IntakePivotConstants.kRightMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        leftMotor.configure(
+            IntakePivotConstants.KLeftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakePivotConstants.kRightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        controller = leftMotor.getClosedLoopController();
+
+        encoder = leftMotor.getEncoder();
+        encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians());
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput(
+            "IntakePivot/TargetPosition", 
+            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
+        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
+        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command maintainPosition(IntakePivotPosition position) {
+        return run(() -> {
+            currentTargetPosition = position;
+
+            if(currentTargetPosition == null) {
+                leftMotor.disable();
+            } else {
+                controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
+            }
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            currentTargetPosition = null;
+            
+            leftMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+            rightMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(double time){
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time),
+            manualSpeed(() -> 0.75).withTimeout(time)
+        );
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(DoubleSupplier time) {
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
+            manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
+        );
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public Optional<IntakePivotPosition> getCurrentTargetPosition() {
+        return Optional.ofNullable(currentTargetPosition);
+    }
+}
+

src/main/java/first/robot/subsystems/IntakeRoller.java

@@ -0,0 +1,64 @@
+package first.robot.subsystems;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.IntakeRollerConstants;
+
+public class IntakeRoller extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    public IntakeRoller() {
+        leftMotor = new SparkMax(
+            IntakeRollerConstants.kCANPort,
+            IntakeRollerConstants.kLeftMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        rightMotor = new SparkMax(
+            IntakeRollerConstants.kCANPort,
+            IntakeRollerConstants.kRightMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        leftMotor.configure(
+            IntakeRollerConstants.leftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakeRollerConstants.rightMotorConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
+    }
+
+    public Command runIn() {
+        return run(() -> {
+            leftMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed);
+            rightMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed);
+        });
+    }
+
+    public Command runOut() {
+        return run(() -> {
+            leftMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed);
+            rightMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            leftMotor.setThrottle(0);
+            rightMotor.setThrottle(0);
+        });
+    }
+    
+}

src/main/java/first/robot/subsystems/PhotonVision.java

@@ -0,0 +1,204 @@
+package first.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+import java.util.function.Consumer;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Transform3d;
+
+import first.robot.constants.CompetitionConstants;
+import first.robot.constants.PhotonConstants;
+import first.robot.utilities.PhotonVisionConfig;
+import first.robot.utilities.VisualPose;
+
+/**
+ * This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as
+ * a "resource", that is, something that is not inherently a physical mechanism to be controlled.
+ * 
+ * A "resource" in this instance should be thought of as something that can be safely shared
+ * by other Subsystems generally without collision if more that one Subsystem requires the 
+ * "resource" at any given time.
+ * 
+ * Resources should <i>NOT</i> produce Commands, they should not have a default Command.
+ * Resources do not have behaviors, and because Commands are in of themselves behaviors,
+ * this class should not have Commands. 
+ * 
+ * Part of the thinking behind creating the PhotonVision components this way is to rely
+ * on the CommandScheduler to call periodic. If this weren't the case, some other subsystem
+ * would have to manage calling for periodic updates, while still sharing the resource with 
+ * other subsystems <i>somehow</i>. 
+ * 
+ * This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs"
+ * List in the PhotonConstants file, you change what is set up internally in this class. 
+ * 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras,
+ * 2 estimators, etc. etc. 
+ */
+public class PhotonVision extends SubsystemBase {
+    private PhotonCamera[] cameras;
+    private PhotonPoseEstimator[] estimators;
+    private List<PhotonPipelineResult> latestResults;
+
+    private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;
+
+    public PhotonVision() {
+        cameras = new PhotonCamera[PhotonConstants.configs.size()];
+        estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
+        latestResults = new ArrayList<PhotonPipelineResult>();
+
+        for(int i = 0; i < PhotonConstants.configs.size(); i++) {
+            cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName());
+            estimators[i] = new PhotonPoseEstimator(
+                CompetitionConstants.kField, 
+                PhotonConstants.configs.get(i).robotToCamera()
+            );
+            latestResults.add(null);
+        }
+
+        poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < cameras.length; i++) {
+            List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
+
+            if(!results.isEmpty()) {
+                latestResults.set(i, results.get(results.size() - 1));
+
+                Optional<EstimatedRobotPose> pose = estimators[i].estimateCoprocMultiTagPose(latestResults.get(i));
+
+                if(!pose.isEmpty()) {
+                    VisualPose visualPose = new VisualPose(
+                        cameras[i].getName(),
+                        pose.get().estimatedPose.toPose2d(), 
+                        pose.get().timestampSeconds
+                    );
+
+                    for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                        consumer.accept(visualPose);
+                    }
+                }
+            }
+        }
+    }
+
+    /**
+     * Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return Optional.empty().
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that 
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still return
+     * some sort of pose. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An Optional object containing a Pose3d object, or Optional.empty() if
+     *   the tag is not present anywhere in the robots field of view. 
+     */
+    public Optional<Pose3d> getBestPoseForTag(int tagID) {
+        PhotonVisionConfig config = null;
+        Transform3d bestCameraToTarget = null;
+        float bestConfidence = -1;
+
+        for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) {
+            if(latestResults.get(cameraIndex) == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) {
+                    continue;
+                }
+
+                config = PhotonConstants.configs.get(cameraIndex);
+                bestCameraToTarget = target.bestCameraToTarget;
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestCameraToTarget == null) {
+            return Optional.empty();
+        }
+
+        // This is based on what PhotonVision does for multitag Pose estimation
+        // See PhotonPoseEstimator.multiTagOnCoprocStrategy
+        // TODO This doesn't currently account for the offset of the tag relative to say the hub
+        // unclear if that offset amount will be important or not
+        return Optional.of(Pose3d.ZERO
+            .plus(bestCameraToTarget.inverse())
+            .relativeTo(CompetitionConstants.kField.getOrigin())
+            .plus(config.robotToCamera().inverse()));
+    }
+
+    /**
+     * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return OptionalDouble.empty()
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still
+     * return some sort of yaw value.
+     * 
+     * Note that the yaw value here is the yaw of the observed tag relative to the center
+     * of the cameras image frame. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An OptionalDouble object containing a Double representing the described yaw
+     *   of the AprilTag specified, or OptionalDouble.empty() if the tag is not present
+     *   anywhere in the robots field of view
+     */
+    public OptionalDouble getBestYawForTag(int tagID) {
+        double bestTagYaw = -1;
+        float bestConfidence = -1;
+
+        for(PhotonPipelineResult result: latestResults) {
+            if(result == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: result.getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) {
+                    continue;
+                }
+
+                bestTagYaw = target.getYaw();
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestConfidence == -1) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(bestTagYaw);
+    }
+
+    /**
+     * Add a Consumer of VisualPose records to the PhotonVision resource. 
+     * 
+     * Each consumer will receive a VisualPose object when any camera produces a new
+     * VisualPose. 
+     * 
+     * The number of Poses produced in a given 20ms cycle is the same number as how many
+     * cameras there are on the robot, assuming those cameras see enough tags to generate a pose, 
+     * as currently all cameras configuration will generate a Pose2d
+     * 
+     * @param consumer The lambda, functional reference, or Consumer implementing object 
+     *  that will consume Poses produced by the PhotonVision resource.
+     */
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
+    }
+}
+

vendordeps/photonlib-v2027.0.0-alpha-2.json

@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2027.0.0-alpha-2",
+  "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "wpilibYear": "2027_alpha7",
   "mavenUrls": [
@@ -13,7 +13,20 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2027.0.0-alpha-2",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxsystemcore",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.wpilib.fields",
+      "artifactId": "fields-cpp",
+      "version": "wpilib",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -28,7 +41,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2027.0.0-alpha-2",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -43,7 +56,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2027.0.0-alpha-2",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -60,12 +73,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2027.0.0-alpha-2"
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2027.0.0-alpha-2"
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870"
     }
   ]
 }