Merge branch 'main' of https://github.com/SFUnity/Mayhem-2024-Offseason

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,21 +1,44 @@
 package frc.robot.subsystems.drive;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.GeneralUtil;
+
 import java.util.function.Supplier;
 
+import org.littletonrobotics.junction.Logger;
+
 public class Drive extends SubsystemBase {
   private final DriveIO io;
+  private final GyroIO gyroIO;
+  private final DriveIOInputsAutoLogged driveInputs = new DriveIOInputsAutoLogged();
+  private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+
+  Rotation2d rawGyroRotation = new Rotation2d();
 
-  public Drive(DriveIO io) {
+
+  public Drive(DriveIO io, GyroIO gyroIO) {
     this.io = io;
+    this.gyroIO = gyroIO;
   }
 
+
   public void periodic() {
-    // TODO: Logs
-    // io.updateInputs(inputs);
-    // Logger.processInputs("Shooter/Feeder", inputs);
-    // GeneralUtil.logSubsystem(this, "Shooter/Feeder");
+    io.updateInputs(driveInputs);
+    Logger.processInputs("Shooter/Feeder", driveInputs);
+    GeneralUtil.logSubsystem(this, "Shooter/Feeder");
+
+    // Update gyro angle
+    if (gyroInputs.connected) {
+      // Use the real gyro angle
+      rawGyroRotation = gyroInputs.yawPosition;
+    } else {
+      // Use the angle delta from the kinematics and module deltas
+      Twist2d twist = DriveConstants.kinematics.toTwist2d(driveInputs.leftPosition, driveInputs.rightPosition);
+      rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+    }
   }
 
   private void fullStop() {

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -8,5 +8,6 @@ public final class DriveConstants {
   public static final int leftMotorID = 0;
 
   // TODO: FAKE change trackwidth later
-  public static final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(5);
+  public static final DifferentialDriveKinematics kinematics =
+      new DifferentialDriveKinematics(0.508);
 }

src/main/java/frc/robot/subsystems/drive/DriveIO.java

@@ -10,6 +10,9 @@ public static class DriveIOInputs {
 
     public double xSpeed = 0.0;
     public double zRotation = 0.0;
+
+    public double leftPosition = 0.0;
+    public double rightPosition = 0.0;
   }
 
   default void updateInputs(DriveIOInputs inputs) {}

src/main/java/frc/robot/subsystems/drive/GyroIO.java

@@ -0,0 +1,29 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface GyroIO {
+  @AutoLog
+  public static class GyroIOInputs {
+    public boolean connected = false; // instantiated as false bc
+    // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736
+    public Rotation2d yawPosition = new Rotation2d();
+    public double yawVelocityRadPerSec = 0.0;
+  }
+
+  public default void updateInputs(GyroIOInputs inputs) {}
+}

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -0,0 +1,44 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** IO implementation for Pigeon2 */
+public class GyroIOPigeon2 implements GyroIO {
+  private final Pigeon2 pigeon = new Pigeon2(20);
+  private final StatusSignal<Double> yaw = pigeon.getYaw();
+  private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
+
+  public GyroIOPigeon2() {
+    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    pigeon.getConfigurator().setYaw(0.0);
+    yaw.setUpdateFrequency(100.0);
+    yawVelocity.setUpdateFrequency(100.0);
+    pigeon.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+    inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+  }
+}

src/main/java/frc/robot/util/GeneralUtil.java

@@ -0,0 +1,24 @@
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public abstract class GeneralUtil {
+  public static void logSubsystem(SubsystemBase s, String sName) {
+    String key = sName + "/command";
+    Logger.recordOutput(
+        key, s.getCurrentCommand() != null ? s.getCurrentCommand().getName() : "none");
+  }
+
+  public static void logFullSubsystem(SubsystemBase s, String sName) {
+    sName += "/cmdInfo/";
+    Logger.recordOutput(sName + "hasDefault", s.getDefaultCommand() != null);
+    Logger.recordOutput(
+        sName + "default",
+        s.getDefaultCommand() != null ? s.getDefaultCommand().getName() : "none");
+    Logger.recordOutput(sName + "hasCommand", s.getCurrentCommand() != null);
+    Logger.recordOutput(
+        sName + "command",
+        s.getCurrentCommand() != null ? s.getCurrentCommand().getName() : "none");
+  }
+}

src/main/java/frc/robot/util/PoseManager.java

@@ -7,20 +7,13 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import frc.robot.subsystems.drive.DriveConstants;
 import org.littletonrobotics.junction.AutoLogOutput;
 
 public class PoseManager {
-  private SwerveModulePosition[] lastModulePositions = // For reseting pose
-      new SwerveModulePosition[] {
-        new SwerveModulePosition(),
-        new SwerveModulePosition(),
-        new SwerveModulePosition(),
-        new SwerveModulePosition()
-      };
+
   private Rotation2d lastGyroAngle = new Rotation2d();
   private Twist2d robotVelocity = new Twist2d();
   private double lastYawVelocity = 0.0;