Merge branch 'week2' of https://github.com/team4909/2024-Crescendo in…

…to week2

src/main/java/frc/robot/Autos.java

@@ -63,7 +63,7 @@ private Command shoot() {
             m_shooter.runShooter(),
             Commands.waitUntil(m_shooter.readyToShoot)
                 .andThen(m_feeder.shoot().alongWith(m_intake.intake()).withTimeout(0.5)))
-        .andThen(m_shooter.stop(), m_feeder.stop())
+        .andThen(m_shooter.stop(), m_feeder.stop(), m_intake.stop())
         .finallyDo(() -> System.out.println("Shooting complete!"));
   }
 }

src/main/java/frc/robot/Constants.java

@@ -10,7 +10,7 @@
 import java.util.function.BooleanSupplier;
 
 public final class Constants {
-  public static final Mode kCurrentMode = Mode.kReal;
+  public static final Mode kCurrentMode = Mode.kSim;
   public static final RobotName kRobot = RobotName.kViper;
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
   public static final String kDrivetrainCanBus = "CANivore1";
@@ -20,11 +20,11 @@ public final class Constants {
       () ->
           DriverStation.getAlliance().isPresent()
               && DriverStation.getAlliance().get() == Alliance.Red;
-  public static final boolean useWpiFieldLayout = true;
+  private static final boolean kUseWpiFieldLayout = false;
   public static final AprilTagFieldLayout fieldLayout;
 
   static {
-    if (useWpiFieldLayout) {
+    if (kUseWpiFieldLayout) {
       try {
         fieldLayout =
             new AprilTagFieldLayout(
@@ -35,9 +35,7 @@ public final class Constants {
       } catch (IOException e) {
         throw new RuntimeException(e);
       }
-    } else
-      fieldLayout =
-          useWpiFieldLayout ? null : AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
+    } else fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
   }
 
   public static enum Mode {

src/main/java/frc/robot/Robot.java

@@ -20,7 +20,6 @@
 import frc.robot.shooter.Shooter;
 import frc.robot.vision.Vision;
 import org.littletonrobotics.junction.LogFileUtil;
-import org.littletonrobotics.junction.LogTable;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -67,7 +66,6 @@ public Robot() {
     }
 
     Logger.start();
-    LogTable.disableProtobufWarning();
 
     switch (Constants.kCurrentMode) {
       case kReal:
@@ -82,11 +80,11 @@ public Robot() {
       case kSim:
         m_drivetrain = Subsystems.createSimDrivetrain();
         m_vision = Subsystems.createFourCameraVision();
-        m_intake = Subsystems.createBlankIntake();
+        m_intake = Subsystems.createSimIntake();
         m_arm = Subsystems.createSimArm();
         m_climber = Subsystems.createBlankClimber();
         m_shooter = Subsystems.createSimShooter();
-        m_feeder = Subsystems.createBlankFeeder();
+        m_feeder = Subsystems.createSimFeeder();
         break;
       default:
         m_drivetrain = Subsystems.createBlankDrivetrain();
@@ -102,7 +100,7 @@ public Robot() {
     NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier);
     NoteVisualizer.resetNotes();
     NoteVisualizer.showStagedNotes();
-    Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake);
+    final Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake);
     NamedCommands.registerCommand("intake", m_intake.intake());
     NamedCommands.registerCommand("intakeOff", m_intake.idle());
     NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter()));
@@ -195,8 +193,6 @@ public Robot() {
     m_driverController.start().onTrue(m_drivetrain.zeroGyro());
     m_driverController.leftStick().toggleOnTrue(m_arm.aimElbowForTuning());
     m_driverController.rightStick().toggleOnTrue(m_arm.aimWristForTuning());
-    // m_driverController.a().whileTrue(m_climber.unwindWinch());
-    // m_driverController.y().whileTrue(m_climber.windWinch());
     m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake));
     m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb));
     m_driverController.b().whileTrue(Commands.parallel(m_arm.idleCoast(), m_climber.windWinch()));

src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java

@@ -12,25 +12,26 @@
 
 public class ImuIOPigeon2 implements ImuIO {
   private final Pigeon2 m_imu = new Pigeon2(20, Constants.kDrivetrainCanBus);
-  private final StatusSignal<Double> yaw = m_imu.getYaw();
+  private final StatusSignal<Double> m_yawSignal = m_imu.getYaw();
+  private final StatusSignal<Double> m_yawVelocitySignal = m_imu.getAngularVelocityZWorld();
   private final Queue<Double> m_yawPositionQueue, m_yawTimestampQueue;
-  private final StatusSignal<Double> yawVelocity = m_imu.getAngularVelocityZWorld();
 
   public ImuIOPigeon2() {
     m_imu.getConfigurator().apply(new Pigeon2Configuration());
     m_imu.getConfigurator().setYaw(0.0);
-    yaw.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz);
-    yawVelocity.setUpdateFrequency(100.0);
+    m_yawSignal.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz);
+    m_yawVelocitySignal.setUpdateFrequency(100.0);
     m_imu.optimizeBusUtilization();
     m_yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_imu, m_imu.getYaw());
     m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
   }
 
   @Override
   public void updateInputs(ImuIOInputs inputs) {
-    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
-    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
-    inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+    inputs.connected =
+        BaseStatusSignal.refreshAll(m_yawSignal, m_yawVelocitySignal).equals(StatusCode.OK);
+    inputs.yawPosition = Rotation2d.fromDegrees(m_yawSignal.getValueAsDouble());
+    inputs.yawVelocityRadPerSec = Units.degreesToRadians(m_yawVelocitySignal.getValueAsDouble());
 
     inputs.odometryYawPositions =
         m_yawPositionQueue.stream()

src/main/java/frc/robot/vision/Vision.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants;
 import frc.robot.PoseEstimation;
@@ -26,6 +27,7 @@
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
@@ -39,6 +41,7 @@ public class Vision {
   private final boolean kTrustVisionXY = true;
   private final boolean kTrustVisionTheta = true;
   private final boolean kIgnoreVisionInSim = true;
+  private final boolean kIgnoreVisionInAuto = false;
   // #endregion
   private final VisionIO[] io;
   private final VisionIOInputs[] m_inputs;
@@ -92,9 +95,19 @@ public void periodic() {
 
     m_newVisionUpdates = new ArrayList<>();
     final List<Pose3d> estimatedPosesToLog = new ArrayList<>();
-    for (int i = 0; i < io.length; i++) {
-      for (PhotonPipelineResult result : m_inputs[i].results) {
-        Optional<EstimatedRobotPose> poseOptional = m_poseEstimators[i].update(result);
+    for (int ioIndex = 0; ioIndex < io.length; ioIndex++) {
+      for (int resultIndex = 0; resultIndex < m_inputs[ioIndex].results.length; resultIndex++) {
+        final byte[] rawResult = m_inputs[ioIndex].results[resultIndex];
+        final Packet dataPacket = new Packet(1);
+        dataPacket.setData(rawResult);
+        if (dataPacket.getSize() < 1)
+          DriverStation.reportError("Somehow photonvision data packet is empty", true);
+        final PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket);
+        final double timestampSeconds =
+            (m_inputs[ioIndex].timestampsMillis[resultIndex] / 1e6)
+                - (result.getLatencyMillis() / 1e3);
+        result.setTimestampSeconds(timestampSeconds);
+        final Optional<EstimatedRobotPose> poseOptional = m_poseEstimators[ioIndex].update(result);
         poseOptional.ifPresent(
             pose -> {
               Pose3d estimatedPose = pose.estimatedPose;
@@ -142,7 +155,8 @@ public void periodic() {
 
   public Matrix<N3, N1> getEstimationStdDevs(
       Pose2d estimatedPose, List<PhotonTrackedTarget> targets) {
-    if (Constants.kIsSim && kIgnoreVisionInSim)
+    if ((Constants.kIsSim && kIgnoreVisionInSim)
+        || (DriverStation.isAutonomous() && kIgnoreVisionInAuto))
       return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
     int tagCount = 0;
     double totalDistance = 0;

src/main/java/frc/robot/vision/VisionIO.java

@@ -3,20 +3,21 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import org.littletonrobotics.junction.LogTable;
 import org.littletonrobotics.junction.inputs.LoggableInputs;
-import org.photonvision.targeting.PhotonPipelineResult;
 
 public interface VisionIO {
 
   public static class VisionIOInputs implements LoggableInputs {
     public String cameraName = "";
     public Transform3d robotToCamera = new Transform3d();
-    public PhotonPipelineResult[] results = new PhotonPipelineResult[] {};
+    public double[] timestampsMillis = new double[] {};
+    public byte[][] results = new byte[][] {};
     public boolean connected = false;
 
     @Override
     public void toLog(LogTable table) {
       table.put("CameraName", cameraName);
       table.put("RobotToCamera", robotToCamera);
+      table.put("Timestamps", timestampsMillis);
       table.put("ResultCount", results.length);
       for (int i = 0; i < results.length; i++) {
         table.put("Result/" + Integer.toString(i), results[i]);
@@ -28,10 +29,11 @@ public void toLog(LogTable table) {
     public void fromLog(LogTable table) {
       cameraName = table.get("CameraName", cameraName);
       robotToCamera = table.get("RobotToCamera", robotToCamera);
+      timestampsMillis = table.get("Timestamps", new double[] {0.0});
       int resultCount = table.get("ResultCount", 0);
-      results = new PhotonPipelineResult[resultCount];
+      results = new byte[resultCount][];
       for (int i = 0; i < resultCount; i++) {
-        results[i] = table.get("Result/" + Integer.toString(i), new PhotonPipelineResult());
+        results[i] = table.get("Result/" + i, new byte[] {});
       }
       connected = table.get("Connected", false);
     }

src/main/java/frc/robot/vision/VisionIOPhotonVision.java

@@ -6,8 +6,6 @@
 import edu.wpi.first.networktables.RawSubscriber;
 import edu.wpi.first.networktables.TimestampedRaw;
 import edu.wpi.first.wpilibj.Timer;
-import org.photonvision.common.dataflow.structures.Packet;
-import org.photonvision.targeting.PhotonPipelineResult;
 
 public class VisionIOPhotonVision implements VisionIO {
 
@@ -35,19 +33,11 @@ public void updateInputs(VisionIOInputs inputs) {
     inputs.cameraName = m_cameraName;
     inputs.robotToCamera = m_robotToCamera;
     TimestampedRaw[] dataQueue = m_photonDataSubscriber.readQueue();
-    inputs.results = new PhotonPipelineResult[dataQueue.length];
+    inputs.results = new byte[dataQueue.length][];
 
     for (int index = 0; index < dataQueue.length; index++) {
-      Packet dataPacket = new Packet(1);
-      dataPacket.setData(dataQueue[index].value);
-      if (dataPacket.getSize() < 1) {
-        throw new NullPointerException("Data packet is empty. This should NEVER happen.");
-      }
-      PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket);
-      double timestampSeconds =
-          (dataQueue[index].timestamp / 1e6) - (result.getLatencyMillis() / 1e3);
-      result.setTimestampSeconds(timestampSeconds);
-      inputs.results[index] = result;
+      inputs.results[index] = dataQueue[index].value;
+      inputs.timestampsMillis[index] = dataQueue[index].timestamp;
     }
 
     if (dataQueue.length > 0) {