Final commit before merging to main to continue progress

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

@@ -17,10 +17,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.PhotonVision;
 import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
+  private PhotonVision vision;
   private Drivetrain drivetrain;
 
   private CommandXboxController driver;
@@ -30,8 +32,11 @@ public class RobotContainer {
   private Timer shiftTimer;
 
   public RobotContainer() {
+    vision = new PhotonVision();
     drivetrain = new Drivetrain();
 
+    vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+
     driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
     shiftTimer = new Timer();

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

@@ -0,0 +1,43 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class HoodConstants {
+    // TODO Real Values
+    public static final int kMotorCANID = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+    public static final double kStartupAngle = 0;
+
+    public static final int kCurrentLimit = 15;
+
+    public static final boolean kInverted = false;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kConfig = new SparkMaxConfig();
+
+    static {
+        kConfig
+            .idleMode(kIdleMode)
+            .inverted(kInverted)
+            .smartCurrentLimit(kCurrentLimit);
+        kConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, Math.PI * 2)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}

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

@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.SwerveModule;
 
 public class Drivetrain extends SubsystemBase {
@@ -172,7 +173,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     // TODO check both cameras
-    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+    /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
         if (camera1 == null) {
             return new PrintCommand("Camera 1 not available");
         }
@@ -196,7 +197,7 @@ public class Drivetrain extends SubsystemBase {
                 () -> false
            )
         );
-    }
+    }*/
 
     public Command drivePathPlannerPath(PathPlannerPath path) {
         if(AutoConstants.kAutoConfigOk) {
@@ -228,6 +229,13 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public void consumeVisualPose(VisualPose pose) {
+        estimator.addVisionMeasurement(
+            pose.visualPose(), 
+            pose.timestamp()
+        );
+    }
+
     public void resetEncoders() {
         frontLeft.resetEncoders();
         frontRight.resetEncoders();

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

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.HoodConstants;
+
+public class Hood extends SubsystemBase {
+    private SparkMax motor;
+
+    private AbsoluteEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private double currentTargetRadians;
+
+    public Hood() {
+        motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
+
+        encoder = motor.getAbsoluteEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        currentTargetRadians = HoodConstants.kStartupAngle;
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
+        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
+        return run(() -> {
+            currentTargetRadians = radianAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            motor.disable();
+        });
+    }
+
+    public double getTargetRadians() {
+        return currentTargetRadians;
+    }
+}

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

@@ -2,10 +2,12 @@ package frc.robot.subsystems;
 
 import java.io.IOException;
 import java.util.ArrayList;
+import java.util.Comparator;
 import java.util.List;
 import java.util.Optional;
 import java.util.OptionalDouble;
 import java.util.function.Consumer;
+import java.util.stream.Stream;
 
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -26,17 +28,17 @@ import frc.robot.interfaces.IVisualPoseProvider;
 import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.PhotonVisionConfig;
 
-public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
+public class PhotonVision extends SubsystemBase {
     private PhotonCamera[] cameras;
     private PhotonPoseEstimator[] estimators;
-    private PhotonPipelineResult[] latestResults;
+    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 PhotonPipelineResult[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());
@@ -45,7 +47,7 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
                 PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
                 PhotonConstants.configs.get(i).robotToCamera()
             );
-            latestResults[i] = null;
+            latestResults.add(null);
         }
 
         poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
@@ -57,10 +59,10 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
             List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
 
             if(!results.isEmpty()) {
-                latestResults[i] = results.get(results.size() - 1);
+                latestResults.set(i, results.get(results.size() - 1));
             }
 
-            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults[i]);
+            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
 
             if(!pose.isEmpty()) {
                 VisualPose visualPose = new VisualPose(
@@ -75,7 +77,27 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
         }
     }
 
-    public Trigger tagPrescenseTrigger(int targetTag) {
+    public void testMethod(int targetID) {
+        Optional<PhotonTrackedTarget> target = latestResults.stream()
+            .filter((p) -> p != null)
+            .map(PhotonPipelineResult::getTargets)
+            .map(List::stream)
+            .reduce(Stream::concat)
+            .get()
+            .filter((p) -> p.getFiducialId() == targetID)
+            .max(
+                Comparator.comparingDouble((ptt) -> {
+                    return (double)ptt.getDetectedObjectConfidence();
+                })
+            );
+
+    }
+
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
+    }
+
+    /*public Trigger tagPrescenseTrigger(int targetTag) {
         return new Trigger(() -> {
             return List.of(latestResults).stream()
                 .filter((p) -> p != null)
@@ -85,8 +107,8 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
                     });
                 });
             });
-    }
-
+    }*/
+/* 
     @Override
     public OptionalDouble getTagDistanceFromCameraByID(int id) {
         if (latestResult == null) {
@@ -172,5 +194,5 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
 
         return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
     }
-    
+    */
 }