From 49fd1fc2e88f5a8927e5e77fda6cd5977036947d Mon Sep 17 00:00:00 2001 From: iefomit Date: Tue, 31 Mar 2026 00:28:50 -0700 Subject: [PATCH] more pro features --- .../subsystems/drivetrain/Drivetrain.java | 16 ++++++++++ .../robot/subsystems/drivetrain/GyroIO.java | 5 +++ .../subsystems/drivetrain/GyroIOPigeon2.java | 5 +++ .../robot/subsystems/drivetrain/Module.java | 32 +++++++++++++++++-- 4 files changed, 56 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index cb77ac2..736d220 100644 --- 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 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 ca66aa9..1cd5394 100644 --- 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 320c3b8..5b1d9e3 100644 --- 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 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 3699731..d415d04 100644 --- 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 getDrivePositionSignal() { + return drivePosition; + } + + /** returns the turn position status signal for time-synced odometry. */ + public StatusSignal getTurnPositionSignal() { + return turnPosition; + } + + /** returns the turn absolute position status signal for time-synced odometry. */ + public StatusSignal getTurnAbsolutePositionSignal() { + return turnAbsolutePosition; + } + } \ No newline at end of file -- 2.39.5