A bunch of changes, mostly related to shooting balls at the hub dynamically, still need a means of doing this based on a single apriltag, in the event the robot pose is unreliable

src/main/java/frc/robot/RobotContainer.java

@@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -19,15 +21,23 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.AutoConstants;
+import frc.robot.constants.CompetitionConstants;
+import frc.robot.constants.HoodConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Hood;
 import frc.robot.subsystems.PhotonVision;
+import frc.robot.subsystems.Shooter;
 import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
     private PhotonVision vision;
     private Drivetrain drivetrain;
+    private Hood hood;
+    private Shooter shooter;
 
     private CommandXboxController driver;
 
@@ -38,6 +48,8 @@ public class RobotContainer {
     public RobotContainer() {
         vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
+        hood = new Hood();
+        shooter = new Shooter();
 
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
 
@@ -72,6 +84,33 @@ public class RobotContainer {
                 false // TODO Should this be true by default?
             ) 
         );
+
+        shooter.setDefaultCommand(
+            shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
+        );
+
+        hood.setDefaultCommand(hood.trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .plus(CompetitionConstants.KRobotToShooter.getTranslation().toTranslation2d())
+                .getDistance(hubPose.getTranslation());
+                
+            if(HoodConstants.kUseInterpolatorForAngle) {
+                return HoodConstants.kDistanceToAngle.get(distance);
+            } else {
+                // TODO The average actual speeds isn't <i>really</i> the exit velocity of the ball
+                // on a hooded shooter, based on documentation, it's more like 30-50% depending on
+                // hood material, surface friction, etc.
+                return Utilities.shotAngle(
+                    shooter.getAverageActualSpeeds(), 
+                    distance, 
+                    CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters, 
+                    false
+                );
+            }
+        }));
     }
 
     private void configureNamedCommands() {

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

@@ -4,6 +4,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 
 public class CompetitionConstants {
@@ -14,6 +15,11 @@ public class CompetitionConstants {
         AprilTagFields.kDefaultField
     );
 
+    public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
+
+    // TODO Real Values
+    public static final Transform3d KRobotToShooter = new Transform3d();
+
     public static final Pose2d kBlueHubLocation = new Pose2d(
         Units.inchesToMeters(182.11), 
         Units.inchesToMeters(158.84), 

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

@@ -4,6 +4,8 @@ import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+
 public class HoodConstants {
     // TODO Real Values
     public static final int kMotorCANID = 0;
@@ -15,13 +17,18 @@ public class HoodConstants {
     public static final double kV = 0;
     public static final double kA = 0;
     public static final double kStartupAngle = 0;
+    public static final double kMaxManualSpeedMultiplier = .5;
 
     public static final int kCurrentLimit = 15;
 
     public static final boolean kInverted = false;
+    public static final boolean kUseInterpolatorForAngle = false;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
 
+    // TODO This needs to be filled in from some source
+    public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final SparkMaxConfig kConfig = new SparkMaxConfig();

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -21,9 +21,10 @@ public class IntakePivotConstants {
     }
 
     public static final int kLeftMotorCANID = 0;
-    public static final int kRightMotorCANID = 1;
+    public static final int kRightMotorCANID = 0;
 
     public static final double kConversionFactor = 0;
+    public static final double kMaxManualSpeedMultiplier = .5;
 
     public static final double kP = 0;
     public static final double kI = 0;

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -8,23 +8,17 @@ import edu.wpi.first.math.util.Units;
 
 public class ShooterConstants {
     public enum ShooterSpeeds {
-        kHubSpeed(0, 0),
+        kHubSpeed(0),
-        kFeedSpeed(0, 0);
+        kFeedSpeed(0);
 
-        private double frontRollerMPS;
+        private double speedMPS;
-        private double rearRollerMPS;
 
-        private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
+        private ShooterSpeeds(double speedMPS) {
-            this.frontRollerMPS = frontRollerMPS;
+            this.speedMPS = speedMPS;
-            this.rearRollerMPS = rearRollerMPS;
         }
 
-        public double getFrontRollerMPS() {
+        public double getSpeedMPS() {
-            return frontRollerMPS;
+            return speedMPS;
-        }
-
-        public double getRearRollerMPS() {
-            return rearRollerMPS;
         }
     }
 
@@ -33,29 +27,31 @@ public class ShooterConstants {
     public static final double kWheelDiameter = Units.inchesToMeters(6);
 
     // TODO Real values
-    public static final int kFrontShooterMotor1CANID = 0;
+    public static final int kLeftShooterMotor1CANID = 0;
-    public static final int kFrontShooterMotor2CANID = 0;
+    public static final int kLeftShooterMotor2CANID = 0;
-    public static final int kRearShooterMotor1CANID = 0;
+    public static final int kRightShooterMotor1CANID = 0;
-    public static final int kRearShooterMotor2CANID = 0;
+    public static final int kRightShooterMotor2CANID = 0;
 
-    public static final boolean kFrontShooterMotor1Inverted = false;
+    public static final boolean kLeftShooterMotor1Inverted = false;
-    public static final boolean kFrontShooterMotor2Inverted = false;
+    public static final boolean kLeftShooterMotor2Inverted = false;
-    public static final boolean kRearShooterMotor1Inverted = false;
+    public static final boolean kRightShooterMotor1Inverted = false;
-    public static final boolean kRearShooterMotor2Inverted = false;
+    public static final boolean kRightShooterMotor2Inverted = false;
 
-    public static final double kFrontP = 0;
+    public static final double kLeftP = 0;
-    public static final double kFrontI = 0;
+    public static final double kLeftI = 0;
-    public static final double kFrontD = 0;
+    public static final double kLeftD = 0;
-    public static final double kFrontS = 0;
+    public static final double kLeftS = 0;
-    public static final double kFrontV = 0;
+    public static final double kLeftV = 0;
-    public static final double kFrontA = 0;
+    public static final double kLeftA = 0;
 
-    public static final double kRearP = 0;
+    public static final double kRightP = 0;
-    public static final double kRearI = 0;
+    public static final double kRightI = 0;
-    public static final double kRearD = 0;
+    public static final double kRightD = 0;
-    public static final double kRearS = 0;
+    public static final double kRightS = 0;
-    public static final double kRearV = 0;
+    public static final double kRightV = 0;
-    public static final double kRearA = 0;
+    public static final double kRightA = 0;
+
+    public static final double kShooterHeightMeters = 0;
 
     // TODO Is this value sane?
     public static final int kCurrentLimit = 30;
@@ -64,51 +60,51 @@ public class ShooterConstants {
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
-    public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig();
 
 
     static {
-        kFrontMotor1Config
+        kLeftMotor1Config
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor1Inverted);
+            .inverted(kLeftShooterMotor1Inverted);
-        kFrontMotor1Config.absoluteEncoder
+        kLeftMotor1Config.absoluteEncoder
             .positionConversionFactor(kWheelDiameter * Math.PI)
             .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kFrontMotor1Config.closedLoop
+        kLeftMotor1Config.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kFrontP, kFrontI, kFrontD)
+            .pid(kLeftP, kLeftI, kLeftD)
             .outputRange(-1, 1)
             .feedForward
-                .sva(kFrontS, kFrontV, kFrontA);
+                .sva(kLeftS, kLeftV, kLeftA);
 
-        kFrontMotor2Config
+        kLeftMotor2Config
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor2Inverted)
+            .inverted(kLeftShooterMotor2Inverted)
-            .follow(kFrontShooterMotor1CANID);
+            .follow(kLeftShooterMotor1CANID);
 
-        kRearMotor1Config
+        kRightMotor1Config
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRearShooterMotor1Inverted);
+            .inverted(kRightShooterMotor1Inverted);
-        kRearMotor1Config.absoluteEncoder
+        kRightMotor1Config.absoluteEncoder
             .positionConversionFactor(kWheelDiameter * Math.PI)
             .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kRearMotor1Config.closedLoop
+        kRightMotor1Config.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kRearP, kRearI, kRearD)
+            .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
             .feedForward
-                .sva(kRearS, kRearV, kRearA);
+                .sva(kRightS, kRightV, kRightA);
 
-        kRearMotor2Config
+        kRightMotor2Config
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRearShooterMotor2Inverted)
+            .inverted(kRightShooterMotor2Inverted)
-            .follow(kRearShooterMotor1CANID);
+            .follow(kRightShooterMotor1CANID);
     }
 }

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -1,27 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.Optional;
-
-import edu.wpi.first.math.geometry.Pose2d;
-
-/**
- * An interface which ensures a class' ability to provide visual pose information
- * in a consistent way
- */
-public interface IVisualPoseProvider {
-    /**
-     * A record that can contain the two elements necessary for a WPILIB
-     * pose estimator to use the information from a vision system as part of a full
-     * robot pose estimation
-     */
-    public record VisualPose(Pose2d visualPose, double timestamp) {}
-
-    /**
-     * Return a VisualPose or null if an empty Optional if none is available.
-     * Implementation should provide an empty response if it's unable to provide
-     * a reliable pose, or any pose at all.
-     * 
-     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
-     */
-    public Optional<VisualPose> getVisualPose();
-}

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

@@ -1,7 +1,6 @@
 package frc.robot.subsystems;
 
 import java.util.Optional;
-import java.util.OptionalDouble;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
@@ -30,8 +29,8 @@ import frc.robot.constants.AutoConstants;
 import frc.robot.constants.CompetitionConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.SwerveModule;
+import frc.robot.utilities.VisualPose;
 
 public class Drivetrain extends SubsystemBase {
     private SwerveModule frontLeft;
@@ -45,9 +44,6 @@ public class Drivetrain extends SubsystemBase {
 
     private PIDController yawRotationController;
 
-    private PhotonVision camera1;
-    private PhotonVision camera2;
-
     public Drivetrain(Pose2d startupPose) {
         frontLeft = new SwerveModule(
             "FrontLeft", 

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

@@ -48,12 +48,18 @@ public class Hood extends SubsystemBase {
         });
     }
 
-    public Command stop() {
+    public Command manualSpeed(DoubleSupplier speed) {
+        currentTargetRadians = 0;
+
         return run(() -> {
-            motor.disable();
+            motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
         });
     }
 
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
     public double getTargetRadians() {
         return currentTargetRadians;
     }

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

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import java.util.Optional;
+import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -69,8 +70,16 @@ public class IntakePivot extends SubsystemBase {
         });
     }
 
+    public Command manualSpeed(DoubleSupplier speed) {
+        currentTargetPosition = null;
+
+        return run(() -> {
+            leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
     public Command stop() {
-        return maintainPosition(null);
+        return manualSpeed(() -> 0);
     }
 
     public Optional<IntakePivotPosition> getCurrentTargetPosition() {

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

@@ -24,9 +24,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.CompetitionConstants;
 import frc.robot.constants.PhotonConstants;
 import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.PhotonVisionConfig;
+import frc.robot.utilities.VisualPose;
 
 public class PhotonVision extends SubsystemBase {
     private PhotonCamera[] cameras;

src/main/java/frc/robot/subsystems/Shooter.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import java.util.Optional;
+import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -18,54 +19,54 @@ import frc.robot.constants.ShooterConstants;
 import frc.robot.constants.ShooterConstants.ShooterSpeeds;
 
 public class Shooter extends SubsystemBase {
-    private SparkMax frontMotor1;
+    private SparkMax leftMotor1;
-    private SparkMax frontMotor2;
+    private SparkMax leftMotor2;
-    private SparkMax rearMotor1;
+    private SparkMax rightMotor1;
-    private SparkMax rearMotor2;
+    private SparkMax rightMotor2;
 
-    private AbsoluteEncoder frontEncoder;
+    private AbsoluteEncoder leftEncoder;
-    private AbsoluteEncoder rearEncoder;
+    private AbsoluteEncoder rightEncoder;
 
-    private SparkClosedLoopController frontClosedLoopController;
+    private SparkClosedLoopController leftClosedLoopController;
-    private SparkClosedLoopController rearClosedLoopController;
+    private SparkClosedLoopController rightClosedLoopController;
 
     private ShooterSpeeds targetSpeeds;
 
     public Shooter() {
-        frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
+        leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless);
-        frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
+        leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless);
-        rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
+        rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless);
-        rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
+        rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless);
 
-        frontMotor1.configure(
+        leftMotor1.configure(
-            ShooterConstants.kFrontMotor1Config, 
+            ShooterConstants.kLeftMotor1Config, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        rearMotor1.configure(
+        rightMotor1.configure(
-            ShooterConstants.kRearMotor1Config, 
+            ShooterConstants.kRightMotor1Config, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        frontMotor2.configure(
+        leftMotor2.configure(
-            ShooterConstants.kFrontMotor2Config, 
+            ShooterConstants.kLeftMotor2Config, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        rearMotor2.configure(
+        rightMotor2.configure(
-            ShooterConstants.kRearMotor2Config, 
+            ShooterConstants.kRightMotor2Config, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        frontEncoder = frontMotor1.getAbsoluteEncoder();
+        leftEncoder = leftMotor1.getAbsoluteEncoder();
-        rearEncoder = rearMotor1.getAbsoluteEncoder();
+        rightEncoder = rightMotor1.getAbsoluteEncoder();
 
-        frontClosedLoopController = frontMotor1.getClosedLoopController();
+        leftClosedLoopController = leftMotor1.getClosedLoopController();
-        rearClosedLoopController = rearMotor1.getClosedLoopController();
+        rightClosedLoopController = rightMotor1.getClosedLoopController();
 
         // TODO Set this to the initial startup speed
         targetSpeeds = null;
@@ -74,21 +75,16 @@ public class Shooter extends SubsystemBase {
     @Override
     public void periodic() {
         Logger.recordOutput(
-            "Shooter/FrontRollers/TargetMPS", 
+            "Shooter/TargetMPS", 
-            targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedMPS()
         );
 
-        Logger.recordOutput(
+        Logger.recordOutput("Shooter/LeftRollers/CurrentMPS", leftEncoder.getVelocity());
-            "Shooter/RearRollers/TargetMPS", 
+        Logger.recordOutput("Shooter/RightRollers/CurrentMPS", rightEncoder.getVelocity());
-            targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
-        );
-
-        Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
-        Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
 
         // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
-        Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
+        Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
-        Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
+        Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint());
     }
 
     public Command maintainSpeed(ShooterSpeeds speeds) {
@@ -96,24 +92,37 @@ public class Shooter extends SubsystemBase {
 
         return run(() -> {
             if(targetSpeeds == null) {
-                frontMotor1.disable();
+                leftMotor1.disable();
-                rearMotor1.disable();
+                rightMotor1.disable();
             } else {
-                frontClosedLoopController.setSetpoint(
+                leftClosedLoopController.setSetpoint(
-                    targetSpeeds.getFrontRollerMPS(), 
+                    targetSpeeds.getSpeedMPS(), 
                     ControlType.kVelocity
                 );
 
-                rearClosedLoopController.setSetpoint(
+                rightClosedLoopController.setSetpoint(
-                    targetSpeeds.getRearRollerMPS(), 
+                    targetSpeeds.getSpeedMPS(), 
                     ControlType.kVelocity
                 );
             }
         });
     }
 
+    public Command manualSpeed(DoubleSupplier speed) {
+        targetSpeeds = null;
+
+        return run(() -> {
+            leftMotor1.set(speed.getAsDouble());
+            rightMotor1.set(speed.getAsDouble());
+        });
+    }
+
     public Command stop() {
-        return maintainSpeed(null);
+        return manualSpeed(() -> 0);
+    }
+
+    public double getAverageActualSpeeds() {
+        return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2;
     }
 
     public Optional<ShooterSpeeds> getTargetSpeeds() {

src/main/java/frc/robot/utilities/VisualPose.java

@@ -0,0 +1,5 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public record VisualPose(Pose2d visualPose, double timestamp) {}