questnav

Author: blaze-developer

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

@@ -21,7 +21,6 @@ public interface GyroIO {
   public static class GyroIOInputs {
     public boolean connected = false;
     public Rotation2d yawPosition = new Rotation2d();
-    public double yawVelocityRadPerSec = 0.0;
     public double[] odometryYawTimestamps = new double[] {};
     public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
   }

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

@@ -13,11 +13,12 @@
 
 package frc.robot.subsystems.drive;
 
+import java.util.Queue;
+
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
+
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
-import java.util.Queue;
 
 /** IO implementation for NavX. */
 public class GyroIONavX implements GyroIO {
@@ -34,7 +35,6 @@ public GyroIONavX() {
   public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = navX.isConnected();
     inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw());
-    inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ());
 
     inputs.odometryYawTimestamps =
         yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();

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

@@ -23,39 +23,33 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
 
 /** IO implementation for Pigeon 2. */
 public class GyroIOPigeon2 implements GyroIO {
   private final Pigeon2 pigeon;
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;
   private final StatusSignal<Angle> yaw;
-  private final StatusSignal<AngularVelocity> yawVelocity;
 
   public GyroIOPigeon2(int Pigeon2Id, CANBus bus) {
 
     // Init Pigeon and Statuses
     pigeon = new Pigeon2(Pigeon2Id, bus);
     yaw = pigeon.getYaw();
-    yawVelocity = pigeon.getAngularVelocityZWorld();
 
     pigeon.getConfigurator().apply(new Pigeon2Configuration());
     pigeon.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY);
-    yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
     yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
-    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone());
   }
 
   @Override
   public void updateInputs(GyroIOInputs inputs) {
-    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+    inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK);
     inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
-    inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
 
     inputs.odometryYawTimestamps =
         yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();

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

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.drive;
+
+import java.util.Queue;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.subsystems.vision.QuestNav;
+
+public class GyroIOQuest implements GyroIO {
+
+    private final QuestNav quest_;
+    private final Queue<Double> yawPositionQueue_;
+    private final Queue<Double> yawTimestampQueue_;
+
+    public GyroIOQuest() {
+        quest_ = new QuestNav();
+        yawTimestampQueue_ = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+        yawPositionQueue_ = PhoenixOdometryThread.getInstance().registerSignal(() -> getYaw().getRadians());
+    }
+
+    @Override
+    public void updateInputs(GyroIOInputs inputs) {
+        inputs.connected = quest_.getConnected();
+        inputs.yawPosition = getYaw();
+
+        inputs.odometryYawPositions = yawPositionQueue_
+            .stream()
+            .map(Rotation2d::fromRadians)
+            .toArray(Rotation2d[]::new);
+
+        inputs.odometryYawTimestamps = yawTimestampQueue_
+            .stream()
+            .mapToDouble((Double d) -> d)
+            .toArray();
+    }
+
+    private Rotation2d getYaw() {
+        return quest_.getPose().getRotation();
+    }
+    
+}

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

@@ -159,15 +159,15 @@ public ModuleIOTalonFX(
         // Create drive status signals
         drivePosition = driveTalon.getPosition();
         drivePositionQueue =
-        PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition());
+        PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone());
         driveVelocity = driveTalon.getVelocity();
         driveAppliedVolts = driveTalon.getMotorVoltage();
         driveCurrent = driveTalon.getStatorCurrent();
 
         // Create turn status signals
         turnAbsolutePosition = cancoder.getAbsolutePosition();
         turnPosition = turnTalon.getPosition();
-        turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition());
+        turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone());
         turnVelocity = turnTalon.getVelocity();
         turnAppliedVolts = turnTalon.getMotorVoltage();
         turnCurrent = turnTalon.getStatorCurrent();