more pro features

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index cb77ac2879a0fc7285b5721193ff669a43632e13..736d220ebcb432f367bf004311fdae663372b670 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -9,6 +9,7 @@ import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.BaseStatusSignal;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.VecBuilder;
@@ -189,6 +190,21 @@ public class Drivetrain extends SubsystemBase {
     @Override
     public void periodic() {
         odometryLock.lock(); // Prevents odometry updates while reading data
+        
+        java.util.List<BaseStatusSignal> signals = new java.util.ArrayList<>();
+        var gyroYawSignal = gyroIO.getYawSignal();
+        if (gyroYawSignal != null) {
+            signals.add(gyroYawSignal);
+        }
+        for (var module : modules) {
+            signals.add(module.getDrivePositionSignal());
+            signals.add(module.getTurnPositionSignal());
+            signals.add(module.getTurnAbsolutePositionSignal());
+        }
+        if (!signals.isEmpty()) {
+            BaseStatusSignal.waitForAll(0.1, signals.toArray(new BaseStatusSignal[0]));
+        }
+        
         gyroIO.updateInputs(gyroInputs);
         Logger.processInputs("Drive/Gyro", gyroInputs);
         for (var module : modules) {

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
index ca66aa973031304971c6822b3179e8b49b43a747..1cd53941c543f625701c87bddb5252c5d4196373 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
@@ -16,6 +16,8 @@ package frc.robot.subsystems.drivetrain;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
 
+import com.ctre.phoenix6.StatusSignal;
+
 public interface GyroIO {
   @AutoLog
   public static class GyroIOInputs {
@@ -29,4 +31,7 @@ public interface GyroIO {
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}
+
+  /** returns the yaw status signal for time-synced odometry. */
+  public default StatusSignal<?> getYawSignal() { return null; }
 }

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
index 320c3b8919940e0ea0d5f7e594689ceb84152ad5..5b1d9e3b67c5c9cc4d2de6bcbcd6cfa18ff6ba2d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
@@ -71,4 +71,9 @@ public class GyroIOPigeon2 implements GyroIO {
     yawTimestampQueue.clear();
     yawPositionQueue.clear();
   }
+
+  @Override
+  public StatusSignal<Angle> getYawSignal() {
+    return yaw;
+  }
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
index 3699731702f3314a4735a5932fcdbe3c18937de1..d415d047cb4e40aedc39be8a103440cd6a759df5 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
@@ -9,6 +9,7 @@ import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
@@ -19,8 +20,8 @@ import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 
 import edu.wpi.first.math.MathUtil;
@@ -153,7 +154,6 @@ public class Module implements ModuleIO{
             turnVelocity,
             turnAppliedVolts,
             turnCurrent);
-        ParentDevice.optimizeBusUtilizationForAll(driveMotor, angleMotor);
         
         setDesiredState(new SwerveModuleState(0, getAngle()), false);
     }
@@ -314,6 +314,13 @@ public class Module implements ModuleIO{
 
     private void configAngleMotor() {
         angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+        
+        // configure FusedCANcoder feedback
+        FeedbackConfigs feedbackConfigs = new FeedbackConfigs();
+        feedbackConfigs.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+        feedbackConfigs.FeedbackRemoteSensorID = moduleConstants.getEncoderPort();
+        angleMotor.getConfigurator().apply(feedbackConfigs);
+        
         CurrentLimitsConfigs config = new CurrentLimitsConfigs();
         config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
         config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
@@ -328,6 +335,9 @@ public class Module implements ModuleIO{
         angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
         angleMotor.setPosition(0);
         
+        // optimize bus utilization for angle motor
+        angleMotor.optimizeBusUtilization();
+        
         resetToAbsolute();
     }
 
@@ -372,6 +382,9 @@ public class Module implements ModuleIO{
         driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
         driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
         
+        // optimize bus utilization for drive motor
+        driveMotor.optimizeBusUtilization();
+        
     }
 
     public SwerveModuleState getState() {
@@ -426,4 +439,19 @@ public class Module implements ModuleIO{
         return inputs.odometryTimestamps;
     }
 
+    /** returns the drive position status signal for time-synced odometry. */
+    public StatusSignal<Angle> getDrivePositionSignal() {
+        return drivePosition;
+    }
+
+    /** returns the turn position status signal for time-synced odometry. */
+    public StatusSignal<Angle> getTurnPositionSignal() {
+        return turnPosition;
+    }
+
+    /** returns the turn absolute position status signal for time-synced odometry. */
+    public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+        return turnAbsolutePosition;
+    }
+
 }
\ No newline at end of file