fixed errors from merge

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

@@ -208,6 +208,12 @@ private ArmConstants() {}
         new Translation2d(Units.inchesToMeters(9.27),
             Units.inchesToMeters(12.56));
 
+    public static final Transform3d PIVOT_TRANSLATION_METERS =
+            new Transform3d(Units.inchesToMeters(9.27),
+                    0.0,
+                    Units.inchesToMeters(12.56),
+                    new Rotation3d());
+
     public static final double ARM_LENGTH_METERS = Units.inchesToMeters(22.01);
     public static final double WRIST_LENGTH_METERS = Units.inchesToMeters(14.5);
   }

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -108,6 +109,7 @@ public void handleState() {
     }
   }
 
+  /** Gets the top point of the shooter for checking limits*/
   public Translation2d calculateArmPosition(double armAngle, double wristAngle) {
     return ArmConstants.PIVOT_JOINT_TRANSLATION
         // translate the length + direction of the arm
@@ -119,8 +121,25 @@ public Translation2d calculateArmPosition(double armAngle, double wristAngle) {
                 .minus(Rotation2d.fromDegrees(wristAngle))));
   }
 
-  public ArmSetpoints.ArmSetpoint aimbot(Pose2d robotPose) {
+  /** Gets the transformation of the shooter relative to the drive base */
+  public Transform3d getShooterTransformation() {
+    return Constants.ArmConstants.PIVOT_TRANSLATION_METERS.plus(
+            // Add the movement of the arm
+            GeomUtils.translationToTransform(new Translation3d(
+                    ArmConstants.ARM_LENGTH_METERS,
+                    new Rotation3d(0.0, Units.degreesToRadians(m_inputs.armPositionDegs), 0.0)
+            ))
+    );
+  }
 
+  public double aimbotCalculate(Pose3d robotPose) {
+    Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d());
+    Pose3d shooterPivotPose = robotPose.plus(getShooterTransformation());
+    Transform3d robotToSpeaker = new Transform3d(shooterPivotPose.plus(getShooterTransformation()), speakerPose);
+
+    double groundDistance =
+            Math.sqrt(Math.pow(robotToSpeaker.getX(), 2) + Math.pow(robotToSpeaker.getY(), 2));
+    return Math.atan2(groundDistance, robotToSpeaker.getZ());
   }
 
   /* Command Factories */
@@ -204,24 +223,6 @@ public Command setWristPowerFactory(double power) {
         () -> m_io.setWristVoltage(0.0));
   }
 
-  public Transform3d getShooterTransformation() {
-    return new Transform3d(new Pose3d(),
-        Constants.ArmConstants.PIVOT_TRANSLATION_METERS.plus(
-            GeomUtils.translationToTransform(new Translation3d(
-                Constants.ArmConstants.SHOULDER_BAR_LENGTH_METERS,
-                new Rotation3d(0.0, m_inputs.shoulderPositionRots * Math.PI * 2, 0.0)
-            ))
-        ));
-  }
-
-  public double calcShooterAngle(Pose3d robotPose) {
-    Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d());
-    Pose3d shooterPivotPose = robotPose.plus(getShooterTransformation());
-    Transform3d robotToSpeaker = new Transform3d(shooterPivotPose.plus(getShooterTransformation()), speakerPose);
 
-    double groundDistance =
-        Math.sqrt(Math.pow(robotToSpeaker.getX(), 2) + Math.pow(robotToSpeaker.getY(), 2));
-    return Math.atan2(groundDistance, robotToSpeaker.getZ());
-  }
 }
 

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

@@ -66,7 +66,6 @@ public class DriveSubsystem extends SubsystemBase {
 
   private final VisionSubsystem[] m_cameras;
 
-  public DriveSubsystem (
   private final PIDController m_thetaPid;
   private final PidProperty m_thetaPidProperty;
 

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

@@ -57,5 +57,5 @@ public void setYawVelocityRadPerSec(double yawVelocityRadPerSec) {
     }
   }
 
-  default void updateInputs(GyroIOInputs inputs) {}
+  default void updateInputs(GyroIOInputsAutoLogged inputs) {}
 }

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

@@ -21,7 +21,7 @@ public GyroIOPigeon1(int id) {
   }
 
   @Override
-  public void updateInputs(GyroIOInputs inputs) {
+  public void updateInputs(GyroIOInputsAutoLogged inputs) {
     inputs.setConnected(m_pigeon.getState() == PigeonIMU.PigeonState.Ready);
     inputs.setYawPosition(Rotation2d.fromDegrees(m_pigeon.getYaw()));
     inputs.setYawVelocityRadPerSec((m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20));

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

@@ -50,7 +50,7 @@ public GyroIOPigeon2(boolean phoenixDrive) {
   }
 
   @Override
-  public void updateInputs(GyroIOInputs inputs) {
+  public void updateInputs(GyroIOInputsAutoLogged inputs) {
     inputs.setConnected(BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK));
     inputs.setYawPosition(Rotation2d.fromDegrees(yaw.getValueAsDouble()));
     inputs.setYawVelocityRadPerSec(Units.degreesToRadians(yawVelocity.getValueAsDouble()));

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -240,7 +240,6 @@ public void setTofDistanceIn(double tofDistanceIn) {
       this.tofDistanceIn = tofDistanceIn;
     }
   }
-  }
   default void setMotorVoltageTL(double voltage) {}
   default void setMotorVoltageTR(double voltage) {}
   default void setMotorVoltageBL(double voltage) {}