From 9021088cd79307b07334181f4912b1fb6dd1874f Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 01:20:55 +0000 Subject: [PATCH 01/17] discretize chassis speeds --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/swerve/Drive.java | 8 +++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6fb7de7..2e5b4a8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,6 +15,8 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static double PERIODIC_LOOP_SEC = 0.02; + public static RobotType ROBOT_TYPE = RobotType.COMP; public static Mode getRobotMode() { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index f9b3af6..1c06dd4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -65,7 +67,11 @@ public void periodic() { } // run modules - SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(targetSpeeds); + + /* use kinematics to get desired module states */ + ChassisSpeeds discretizedSpeeds = + ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC); + SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity()); From 0e88ac86309adbc0b3bd930c456e38675db19085 Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 08:10:54 +0000 Subject: [PATCH 02/17] begin teleopcontroller for swerve --- .../swerve/controllers/TeleopController.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java diff --git a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java new file mode 100644 index 0000000..f5218af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.swerve.controllers; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class TeleopController { + + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; + + public void acceptJoystickInput(double controllerX, double controllerY, double controllerOmega) { + this.controllerX = controllerX; + this.controllerY = controllerY; + this.controllerOmega = controllerOmega; + } + + public ChassisSpeeds update() { + + } +} From 530e6cf7188ccee082c58bf18b4781c75408cb0a Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 09:20:01 +0000 Subject: [PATCH 03/17] teleop drive controller, vector based deadbanding --- .factorypath | 4 -- .../frc/robot/subsystems/swerve/Drive.java | 43 +++++-------------- .../swerve/controllers/TeleopController.java | 39 +++++++++++++++-- 3 files changed, 47 insertions(+), 39 deletions(-) delete mode 100644 .factorypath diff --git a/.factorypath b/.factorypath deleted file mode 100644 index 9a645c6..0000000 --- a/.factorypath +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 1c06dd4..8e3c1d6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,7 +3,6 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -11,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; - +import frc.robot.subsystems.swerve.controllers.TeleopController; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -33,9 +32,10 @@ public enum DriveModes { @AutoLogOutput(key = "Swerve/YawOffset") private Rotation2d gyroYawOffset = new Rotation2d(0); - private ChassisSpeeds teleopTargetSpeeds = new ChassisSpeeds(); private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + private final TeleopController teleopController; + public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { this.gyroIO = gyroIO; @@ -43,6 +43,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[1] = new Module(fr, 1); modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); + + teleopController = new TeleopController(); } @Override @@ -61,7 +63,7 @@ public void periodic() { switch (driveMode) { case TELEOP -> { - targetSpeeds = teleopTargetSpeeds; + targetSpeeds = teleopController.update(arbitraryYaw); } case TRAJECTORY -> {} } @@ -89,35 +91,12 @@ public void periodic() { } public void driveTeleopController(double xAxis, double yAxis, double omega) { - if (driveMode != DriveModes.TELEOP) { // auto align override? - driveMode = DriveModes.TELEOP; + if (DriverStation.isTeleopEnabled()) { + if (driveMode != DriveModes.TELEOP) { + driveMode = DriveModes.TELEOP; + } + teleopController.acceptJoystickInput(xAxis, yAxis, omega); } - - // NWU convention - double theta = Math.atan2(xAxis, yAxis); - double radiusDeadband = - MathUtil.applyDeadband(Math.sqrt((xAxis * xAxis) + (yAxis + yAxis)), 0.07); - double radiusExp = Math.copySign(Math.pow(radiusDeadband, 1.5), radiusDeadband); - - double xVelocity = radiusExp * Math.sin(theta) * DRIVE_CONFIG.maxLinearVelocity(); - double yVelocity = radiusExp * Math.cos(theta) * DRIVE_CONFIG.maxLinearVelocity(); - - /*double xVelocity = - MathUtil.applyDeadband(Math.copySign(xAxis * xAxis, xAxis), 0.07) - * Swerve.DRIVE_CONFIG.maxLinearVelocity(); - double yVelocity = - MathUtil.applyDeadband(Math.copySign(yAxis * yAxis, yAxis), 0.07) - * Swerve.DRIVE_CONFIG.maxLinearVelocity();*/ - double radianVelocity = - MathUtil.applyDeadband(Math.copySign(omega * omega, omega), 0.07) - * DRIVE_CONFIG.maxAngularVelocity(); - - this.teleopTargetSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(xVelocity, yVelocity, radianVelocity, arbitraryYaw); - - Logger.recordOutput("Swerve/Teleop/xVelocity", xVelocity); - Logger.recordOutput("Swerve/Teleop/yVelocity", yVelocity); - Logger.recordOutput("Swerve/Teleop/radianVelocity", radianVelocity); } public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { diff --git a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java index f5218af..2c4e3a9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -1,20 +1,53 @@ package frc.robot.subsystems.swerve.controllers; +import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; public class TeleopController { - + private double controllerX = 0; private double controllerY = 0; private double controllerOmega = 0; + /* accept driver input from joysticks */ public void acceptJoystickInput(double controllerX, double controllerY, double controllerOmega) { - this.controllerX = controllerX; + this.controllerX = controllerX; this.controllerY = controllerY; this.controllerOmega = controllerOmega; } - public ChassisSpeeds update() { + /* update controller with current desired state */ + public ChassisSpeeds update(Rotation2d yaw) { + Translation2d linearVelocity = calculateLinearVelocity(controllerX, controllerY); + + double omega = MathUtil.applyDeadband(controllerOmega, 0.1); + omega = Math.copySign(Math.pow(omega, 1.5), omega); + + // eventaully run off of pose estimation? + return ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * DRIVE_CONFIG.maxLinearVelocity(), + linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(), + omega * DRIVE_CONFIG.maxAngularVelocity(), + yaw); + } + + public Translation2d calculateLinearVelocity(double x, double y) { + // apply deadband, raise magnitude to exponent + double magnitude = MathUtil.applyDeadband(Math.hypot(x, y), 0.1); + magnitude = Math.pow(magnitude, 1.5); + + Rotation2d theta = new Rotation2d(x, y); + Translation2d linearVelocity = + new Pose2d(new Translation2d(), theta) + .transformBy(new Transform2d(magnitude, 0, new Rotation2d())) + .getTranslation(); + return linearVelocity; } } From 3a011fc1f13897e7341a91d1ac2430ea3713aae4 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 01:59:48 +0000 Subject: [PATCH 04/17] prepare for tuning & testing --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- .../robot/subsystems/swerve/DriveConstants.java | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e160785..2c98522 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,14 +95,14 @@ public RobotContainer() { private void configureBindings() { // -----Driver Controls----- - /*swerve.setDefaultCommand( - swerve - .run( - () -> { - swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); - }) - .withName("Drive Teleop"));*/ + swerve.setDefaultCommand( + swerve + .run( + () -> { + swerve.driveTeleopController( + -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + }) + .withName("Drive Teleop")); // -----Intake Controls----- driverA.x().whileTrue(rollers.setTargetCommand(RollerState.INTAKE)); diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 6d527a2..5a4e28a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -48,14 +48,14 @@ public class DriveConstants { case COMP -> new ModuleConfig[] { new ModuleConfig(5, 6, 1, new Rotation2d(0), true, false), new ModuleConfig(7, 8, 2, new Rotation2d(0), true, true), - new ModuleConfig(9, 10, 3, new Rotation2d(0), true, false), - new ModuleConfig(11, 12, 4, new Rotation2d(0), true, true) + new ModuleConfig(11, 12, 3, new Rotation2d(0), true, false), + new ModuleConfig(9, 10, 4, new Rotation2d(0), true, true) }; case DEV -> new ModuleConfig[] { - new ModuleConfig(2, 1, 27, new Rotation2d(1.954), true, false), - new ModuleConfig(13, 12, 26, new Rotation2d(1.465), true, true), - new ModuleConfig(4, 3, 24, new Rotation2d(2.612), true, false), - new ModuleConfig(11, 10, 25, new Rotation2d(-2.563), true, true) + new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false), + new ModuleConfig(13, 12, 26, new Rotation2d(0), true, true), + new ModuleConfig(4, 3, 24, new Rotation2d(0), true, false), + new ModuleConfig(11, 10, 25, new Rotation2d(0), true, true) }; case SIM -> new ModuleConfig[] { new ModuleConfig(0, 0, 0, new Rotation2d(0), true, false), From c891e2e93e2d4ffa86e142e2ed3aac93c361c7e9 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 03:14:31 +0000 Subject: [PATCH 05/17] log module supply currents --- .../frc/robot/subsystems/swerve/ModuleIO.java | 2 ++ .../subsystems/swerve/ModuleIOTalonFX.java | 20 +++++++++++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 79dbba7..33ffdc8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -13,11 +13,13 @@ class ModuleIOInputs { public double drivePositionRads = 0; public double driveVelocityRadsPerSec = 0; public double driveAppliedVolts = 0; + public double driveSupplyCurrent = 0; public Rotation2d steerAbsolutePostion = new Rotation2d(); public Rotation2d steerPosition = new Rotation2d(); public double steerVelocityRadsPerSec = 0; public double steerAppliedVolts = 0; + public double steerSupplyCurrent = 0; } default void updateInputs(ModuleIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index f2fe723..2961aa3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -25,11 +25,13 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal drivePosition; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; private final Supplier steerAbsolutePosition; private final StatusSignal steerPosition; private final StatusSignal steerVelocity; private final StatusSignal steerAppliedVolts; + private final StatusSignal steerSupplyCurrent; private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); @@ -85,22 +87,26 @@ public ModuleIOTalonFX(ModuleConfig config) { drivePosition = driveTalon.getPosition(); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); steerAbsolutePosition = () -> Rotation2d.fromRotations(encoder.getAbsolutePosition().getValueAsDouble()); - // .minus(config.absoluteEncoderOffset()); steerPosition = steerTalon.getPosition(); steerVelocity = steerTalon.getVelocity(); steerAppliedVolts = steerTalon.getMotorVoltage(); + steerSupplyCurrent = steerTalon.getSupplyCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( 100, drivePosition, driveVelocity, driveAppliedVolts, + driveSupplyCurrent, encoder.getAbsolutePosition(), steerPosition, steerVelocity, - steerAppliedVolts); + steerAppliedVolts, + steerSupplyCurrent); driveTalon.optimizeBusUtilization(); steerTalon.optimizeBusUtilization(); @@ -112,17 +118,23 @@ public ModuleIOTalonFX(ModuleConfig config) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.driveMotorConnected = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts).isOK(); + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveSupplyCurrent) + .isOK(); inputs.drivePositionRads = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadsPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrent = driveSupplyCurrent.getValueAsDouble(); inputs.steerMotorConnected = - BaseStatusSignal.refreshAll(steerPosition, steerVelocity, steerAppliedVolts).isOK(); + BaseStatusSignal.refreshAll( + steerPosition, steerVelocity, steerAppliedVolts, steerSupplyCurrent) + .isOK(); inputs.steerAbsolutePostion = steerAbsolutePosition.get(); inputs.steerPosition = Rotation2d.fromRotations(steerPosition.getValueAsDouble()); inputs.steerVelocityRadsPerSec = Units.rotationsToRadians(steerVelocity.getValueAsDouble()); inputs.steerAppliedVolts = steerAppliedVolts.getValueAsDouble(); + inputs.steerSupplyCurrent = steerSupplyCurrent.getValueAsDouble(); } @Override From 9be7b547afd26eb4448bdb22a4a3f393be4f8e8b Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 03:34:27 +0000 Subject: [PATCH 06/17] calibration, placeholder pid --- .../robot/subsystems/swerve/DriveConstants.java | 14 +++++++------- .../robot/subsystems/swerve/ModuleIOTalonFX.java | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 5a4e28a..d03932b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -42,14 +42,14 @@ public class DriveConstants { public static final int GYRO_ID = 0; - // fl, fr, bl, br + // fl, fr, bl, br; negate offsets public static final ModuleConfig[] MODULE_CONFIGS = switch (getRobotType()) { case COMP -> new ModuleConfig[] { - new ModuleConfig(5, 6, 1, new Rotation2d(0), true, false), - new ModuleConfig(7, 8, 2, new Rotation2d(0), true, true), - new ModuleConfig(11, 12, 3, new Rotation2d(0), true, false), - new ModuleConfig(9, 10, 4, new Rotation2d(0), true, true) + new ModuleConfig(5, 6, 1, new Rotation2d(1.1397), true, false), + new ModuleConfig(7, 8, 2, new Rotation2d(0.8038), true, true), + new ModuleConfig(11, 12, 3, new Rotation2d(1.4327), true, false), + new ModuleConfig(9, 10, 4, new Rotation2d(-1.8208), true, true) }; case DEV -> new ModuleConfig[] { new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false), @@ -71,12 +71,12 @@ public class DriveConstants { 0, // steerkS 0, // steerkV 0, // steerkA - 0, // steerkP + 300, // steerkP 0, // steerkD 0, // drivekS 0, // drivekV 0, // drivekA - 0, // drivekP + 1, // drivekP 0, // drivekD 5.357142857142857, 21.428571428571427, diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index 2961aa3..b887d9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -46,7 +46,7 @@ public ModuleIOTalonFX(ModuleConfig config) { encoder = new CANcoder(config.encoderID()); // config - encoderConfig.MagnetSensor.MagnetOffset = config.absoluteEncoderOffset().getRotations(); + encoderConfig.MagnetSensor.MagnetOffset = -config.absoluteEncoderOffset().getRotations(); driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.MotorOutput.Inverted = From 365fae01a94389ea58dd7832ca9642a7f7eb0736 Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 00:23:34 +0000 Subject: [PATCH 07/17] log swerve motor stator currents --- .../frc/robot/subsystems/swerve/ModuleIO.java | 2 ++ .../subsystems/swerve/ModuleIOTalonFX.java | 22 ++++++++++++++++--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 33ffdc8..a97a212 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -14,12 +14,14 @@ class ModuleIOInputs { public double driveVelocityRadsPerSec = 0; public double driveAppliedVolts = 0; public double driveSupplyCurrent = 0; + public double driveStatorCurrent = 0; public Rotation2d steerAbsolutePostion = new Rotation2d(); public Rotation2d steerPosition = new Rotation2d(); public double steerVelocityRadsPerSec = 0; public double steerAppliedVolts = 0; public double steerSupplyCurrent = 0; + public double steerStatorCurrent = 0; } default void updateInputs(ModuleIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index b887d9d..eb39d5c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -26,12 +26,14 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveStatorCurrent; private final Supplier steerAbsolutePosition; private final StatusSignal steerPosition; private final StatusSignal steerVelocity; private final StatusSignal steerAppliedVolts; private final StatusSignal steerSupplyCurrent; + private final StatusSignal steerStatorCurrent; private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); @@ -88,6 +90,7 @@ public ModuleIOTalonFX(ModuleConfig config) { driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveStatorCurrent = driveTalon.getStatorCurrent(); steerAbsolutePosition = () -> Rotation2d.fromRotations(encoder.getAbsolutePosition().getValueAsDouble()); @@ -95,6 +98,7 @@ public ModuleIOTalonFX(ModuleConfig config) { steerVelocity = steerTalon.getVelocity(); steerAppliedVolts = steerTalon.getMotorVoltage(); steerSupplyCurrent = steerTalon.getSupplyCurrent(); + steerStatorCurrent = steerTalon.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( 100, @@ -102,11 +106,13 @@ public ModuleIOTalonFX(ModuleConfig config) { driveVelocity, driveAppliedVolts, driveSupplyCurrent, + driveStatorCurrent, encoder.getAbsolutePosition(), steerPosition, steerVelocity, steerAppliedVolts, - steerSupplyCurrent); + steerSupplyCurrent, + steerStatorCurrent); driveTalon.optimizeBusUtilization(); steerTalon.optimizeBusUtilization(); @@ -119,22 +125,32 @@ public ModuleIOTalonFX(ModuleConfig config) { public void updateInputs(ModuleIOInputs inputs) { inputs.driveMotorConnected = BaseStatusSignal.refreshAll( - drivePosition, driveVelocity, driveAppliedVolts, driveSupplyCurrent) + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveStatorCurrent) .isOK(); inputs.drivePositionRads = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadsPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveSupplyCurrent = driveSupplyCurrent.getValueAsDouble(); + inputs.driveStatorCurrent = driveStatorCurrent.getValueAsDouble(); inputs.steerMotorConnected = BaseStatusSignal.refreshAll( - steerPosition, steerVelocity, steerAppliedVolts, steerSupplyCurrent) + steerPosition, + steerVelocity, + steerAppliedVolts, + steerSupplyCurrent, + steerStatorCurrent) .isOK(); inputs.steerAbsolutePostion = steerAbsolutePosition.get(); inputs.steerPosition = Rotation2d.fromRotations(steerPosition.getValueAsDouble()); inputs.steerVelocityRadsPerSec = Units.rotationsToRadians(steerVelocity.getValueAsDouble()); inputs.steerAppliedVolts = steerAppliedVolts.getValueAsDouble(); inputs.steerSupplyCurrent = steerSupplyCurrent.getValueAsDouble(); + inputs.steerStatorCurrent = steerStatorCurrent.getValueAsDouble(); } @Override From f70d72553497ada79d3941cfdf517946dd7690ed Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 10:01:33 +0000 Subject: [PATCH 08/17] placeholder stator current limits --- .../java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index eb39d5c..aaaa0cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -50,6 +50,12 @@ public ModuleIOTalonFX(ModuleConfig config) { // config encoderConfig.MagnetSensor.MagnetOffset = -config.absoluteEncoderOffset().getRotations(); + driveConfig.CurrentLimits.StatorCurrentLimit = 80; // FIXME + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + steerConfig.CurrentLimits.StatorCurrentLimit = 50; // FIXME + steerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.MotorOutput.Inverted = config.driveInverted() From 2560a44feddf7f452b956854e8ba60cb34cb806c Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 10:17:02 +0000 Subject: [PATCH 09/17] modify module gain constants, change steer controls to motion magic --- .../subsystems/swerve/DriveConstants.java | 43 +++++--------- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 +++++++++---------- 3 files changed, 42 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index d03932b..a583f5e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,30 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 300, // steerkP - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 300, 0, 0), + new MotionProfileGains(0, 0, 0), // FIXME + new Gains(0, 0, 0, 1, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 11, // steerkP - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1.5, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 11, 0, 0), + new MotionProfileGains(0, 0, 0), + new Gains(0, 0, 0, 1.5, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); @@ -114,22 +100,19 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - double steerkS, - double steerkV, - double steerkA, - double steerkP, - double steerkD, - double drivekS, - double drivekV, - double drivekA, - double drivekP, - double drivekD, + Gains steerGains, + MotionProfileGains steerMotionGains, + Gains driveGains, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} + public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} + + public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} + private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index a97a212..70f15e9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.DriveConstants.Gains; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -34,9 +36,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setDriveGains(Gains gains) {} - default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index aaaa0cd..a8f945c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,7 +14,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -39,8 +41,9 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); + private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private final MotionMagicVoltage steerPositionControl = + new MotionMagicVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -72,20 +75,8 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveSlot0( - MODULE_CONSTANTS.drivekP(), - 0, - MODULE_CONSTANTS.drivekD(), - MODULE_CONSTANTS.drivekS(), - MODULE_CONSTANTS.drivekV(), - MODULE_CONSTANTS.drivekA()); - setSteerSlot0( - MODULE_CONSTANTS.steerkP(), - 0, - MODULE_CONSTANTS.steerkD(), - MODULE_CONSTANTS.steerkS(), - MODULE_CONSTANTS.steerkV(), - MODULE_CONSTANTS.steerkA()); + setDriveGains(MODULE_CONSTANTS.driveGains()); + setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -171,24 +162,27 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - driveConfig.Slot0.kP = kP; - driveConfig.Slot0.kI = kI; - driveConfig.Slot0.kD = kD; - driveConfig.Slot0.kS = kS; - driveConfig.Slot0.kV = kV; - driveConfig.Slot0.kA = kA; + public void setDriveGains(Gains gains) { + driveConfig.Slot0.kP = gains.kP(); + driveConfig.Slot0.kI = gains.kI(); + driveConfig.Slot0.kD = gains.kD(); + driveConfig.Slot0.kS = gains.kS(); + driveConfig.Slot0.kV = gains.kV(); + driveConfig.Slot0.kA = gains.kA(); driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - steerConfig.Slot0.kP = kP; - steerConfig.Slot0.kI = kI; - steerConfig.Slot0.kD = kD; - steerConfig.Slot0.kS = kS; - steerConfig.Slot0.kV = kV; - steerConfig.Slot0.kA = kA; + public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { + steerConfig.Slot0.kP = gains.kP(); + steerConfig.Slot0.kI = gains.kI(); + steerConfig.Slot0.kD = gains.kD(); + steerConfig.Slot0.kS = gains.kS(); + steerConfig.Slot0.kV = gains.kV(); + steerConfig.Slot0.kA = gains.kA(); + steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); + steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); + steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); steerTalon.getConfigurator().apply(steerConfig); } } From 4ecb826ba3f50c6bac0c4c147c665b319bd98f74 Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 12:27:49 -0800 Subject: [PATCH 10/17] Revert "modify module gain constants, change steer controls to motion magic" This reverts commit 2560a44feddf7f452b956854e8ba60cb34cb806c. --- .../subsystems/swerve/DriveConstants.java | 43 +++++++++----- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 ++++++++++--------- 3 files changed, 63 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index a583f5e..d03932b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,16 +68,30 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - new Gains(0, 0, 0, 300, 0, 0), - new MotionProfileGains(0, 0, 0), // FIXME - new Gains(0, 0, 0, 1, 0, 0), + 0, // steerkS + 0, // steerkV + 0, // steerkA + 300, // steerkP + 0, // steerkD + 0, // drivekS + 0, // drivekV + 0, // drivekA + 1, // drivekP + 0, // drivekD 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - new Gains(0, 0, 0, 11, 0, 0), - new MotionProfileGains(0, 0, 0), - new Gains(0, 0, 0, 1.5, 0, 0), + 0, // steerkS + 0, // steerkV + 0, // steerkA + 11, // steerkP + 0, // steerkD + 0, // drivekS + 0, // drivekV + 0, // drivekA + 1.5, // drivekP + 0, // drivekD 5.357142857142857, 21.428571428571427, 3.125); @@ -100,19 +114,22 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - Gains steerGains, - MotionProfileGains steerMotionGains, - Gains driveGains, + double steerkS, + double steerkV, + double steerkA, + double steerkP, + double steerkD, + double drivekS, + double drivekV, + double drivekA, + double drivekP, + double drivekD, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} - public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} - - public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} - private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 70f15e9..a97a212 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.swerve.DriveConstants.Gains; -import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -36,9 +34,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveGains(Gains gains) {} + default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} - default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} + default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index a8f945c..aaaa0cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,9 +14,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; -import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -41,9 +39,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private final MotionMagicVoltage steerPositionControl = - new MotionMagicVoltage(0).withUpdateFreqHz(0); + private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -75,8 +72,20 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveGains(MODULE_CONSTANTS.driveGains()); - setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); + setDriveSlot0( + MODULE_CONSTANTS.drivekP(), + 0, + MODULE_CONSTANTS.drivekD(), + MODULE_CONSTANTS.drivekS(), + MODULE_CONSTANTS.drivekV(), + MODULE_CONSTANTS.drivekA()); + setSteerSlot0( + MODULE_CONSTANTS.steerkP(), + 0, + MODULE_CONSTANTS.steerkD(), + MODULE_CONSTANTS.steerkS(), + MODULE_CONSTANTS.steerkV(), + MODULE_CONSTANTS.steerkA()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -162,27 +171,24 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveGains(Gains gains) { - driveConfig.Slot0.kP = gains.kP(); - driveConfig.Slot0.kI = gains.kI(); - driveConfig.Slot0.kD = gains.kD(); - driveConfig.Slot0.kS = gains.kS(); - driveConfig.Slot0.kV = gains.kV(); - driveConfig.Slot0.kA = gains.kA(); + public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { + driveConfig.Slot0.kP = kP; + driveConfig.Slot0.kI = kI; + driveConfig.Slot0.kD = kD; + driveConfig.Slot0.kS = kS; + driveConfig.Slot0.kV = kV; + driveConfig.Slot0.kA = kA; driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { - steerConfig.Slot0.kP = gains.kP(); - steerConfig.Slot0.kI = gains.kI(); - steerConfig.Slot0.kD = gains.kD(); - steerConfig.Slot0.kS = gains.kS(); - steerConfig.Slot0.kV = gains.kV(); - steerConfig.Slot0.kA = gains.kA(); - steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); - steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); - steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); + public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { + steerConfig.Slot0.kP = kP; + steerConfig.Slot0.kI = kI; + steerConfig.Slot0.kD = kD; + steerConfig.Slot0.kS = kS; + steerConfig.Slot0.kV = kV; + steerConfig.Slot0.kA = kA; steerTalon.getConfigurator().apply(steerConfig); } } From 7b321c3fa9e57fa3cbd5df0d4cdee37585ef041b Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 16:48:00 -0800 Subject: [PATCH 11/17] Better tuned pid values --- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../frc/robot/subsystems/swerve/DriveConstants.java | 13 ++++++++----- .../java/frc/robot/subsystems/swerve/Module.java | 5 +++++ .../robot/subsystems/swerve/ModuleIOTalonFX.java | 2 +- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c98522..7cfbc82 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,8 +99,7 @@ private void configureBindings() { swerve .run( () -> { - swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + swerve.driveTeleopController(-0, -driverA.getLeftX(), -driverA.getRightX()); }) .withName("Drive Teleop")); diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index d03932b..7780fa3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,15 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0, // steerkS + 0.2, // steerkS 0, // steerkV 0, // steerkA - 300, // steerkP + 98, // steerkP90 + 3, // steer kI 0, // steerkD - 0, // drivekS - 0, // drivekV + 0.3, // drivekS0.3 + 0.63, // drivekV0.67 0, // drivekA - 1, // drivekP + 2, // drivekP 0, // drivekD 5.357142857142857, 21.428571428571427, @@ -86,6 +87,7 @@ public class DriveConstants { 0, // steerkV 0, // steerkA 11, // steerkP + 0, // steer kI 0, // steerkD 0, // drivekS 0, // drivekV @@ -118,6 +120,7 @@ public record ModuleConstants( double steerkV, double steerkA, double steerkP, + double steerkI, double steerkD, double drivekS, double drivekV, diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index fd409fa..44859c6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -44,6 +44,11 @@ public void runToSetpoint(SwerveModuleState targetState) { moduleIO.runDriveVelocitySetpoint(driveVelocityRads); + Logger.recordOutput("Swerve/Module" + index + "/SteerSetpoint", targetState.angle.getRadians()); + Logger.recordOutput( + "Swerve/Module" + index + "/SteerError", + targetState.angle.getRadians() - inputs.steerAbsolutePostion.getRadians()); + Logger.recordOutput("Swerve/Module" + index + "/DriveVelocitySetpoint", driveVelocityRads); Logger.recordOutput("Swerve/Module" + index + "/DriveVelRadsScalar", driveVelocityRads); } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index aaaa0cd..a51c571 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -81,7 +81,7 @@ public ModuleIOTalonFX(ModuleConfig config) { MODULE_CONSTANTS.drivekA()); setSteerSlot0( MODULE_CONSTANTS.steerkP(), - 0, + MODULE_CONSTANTS.steerkI(), MODULE_CONSTANTS.steerkD(), MODULE_CONSTANTS.steerkS(), MODULE_CONSTANTS.steerkV(), From a854c2ab7aa08fc6607afa646db872f523b49fd8 Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 16:49:54 -0800 Subject: [PATCH 12/17] Forgot to undo testing controls --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cfbc82..2c98522 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,7 +99,8 @@ private void configureBindings() { swerve .run( () -> { - swerve.driveTeleopController(-0, -driverA.getLeftX(), -driverA.getRightX()); + swerve.driveTeleopController( + -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); }) .withName("Drive Teleop")); From 69aed5109789329758a6e6d96b2410724b9a9d4f Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 26 Nov 2024 00:37:35 +0000 Subject: [PATCH 13/17] reintroduce motion magic steer motors, refactor swerve gains --- .../subsystems/swerve/DriveConstants.java | 46 +++++---------- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 +++++++++---------- 3 files changed, 42 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 7780fa3..a583f5e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,32 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0.2, // steerkS - 0, // steerkV - 0, // steerkA - 98, // steerkP90 - 3, // steer kI - 0, // steerkD - 0.3, // drivekS0.3 - 0.63, // drivekV0.67 - 0, // drivekA - 2, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 300, 0, 0), + new MotionProfileGains(0, 0, 0), // FIXME + new Gains(0, 0, 0, 1, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 11, // steerkP - 0, // steer kI - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1.5, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 11, 0, 0), + new MotionProfileGains(0, 0, 0), + new Gains(0, 0, 0, 1.5, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); @@ -116,23 +100,19 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - double steerkS, - double steerkV, - double steerkA, - double steerkP, - double steerkI, - double steerkD, - double drivekS, - double drivekV, - double drivekA, - double drivekP, - double drivekD, + Gains steerGains, + MotionProfileGains steerMotionGains, + Gains driveGains, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} + public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} + + public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} + private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index a97a212..70f15e9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.DriveConstants.Gains; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -34,9 +36,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setDriveGains(Gains gains) {} - default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index a51c571..a8f945c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,7 +14,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -39,8 +41,9 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); + private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private final MotionMagicVoltage steerPositionControl = + new MotionMagicVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -72,20 +75,8 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveSlot0( - MODULE_CONSTANTS.drivekP(), - 0, - MODULE_CONSTANTS.drivekD(), - MODULE_CONSTANTS.drivekS(), - MODULE_CONSTANTS.drivekV(), - MODULE_CONSTANTS.drivekA()); - setSteerSlot0( - MODULE_CONSTANTS.steerkP(), - MODULE_CONSTANTS.steerkI(), - MODULE_CONSTANTS.steerkD(), - MODULE_CONSTANTS.steerkS(), - MODULE_CONSTANTS.steerkV(), - MODULE_CONSTANTS.steerkA()); + setDriveGains(MODULE_CONSTANTS.driveGains()); + setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -171,24 +162,27 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - driveConfig.Slot0.kP = kP; - driveConfig.Slot0.kI = kI; - driveConfig.Slot0.kD = kD; - driveConfig.Slot0.kS = kS; - driveConfig.Slot0.kV = kV; - driveConfig.Slot0.kA = kA; + public void setDriveGains(Gains gains) { + driveConfig.Slot0.kP = gains.kP(); + driveConfig.Slot0.kI = gains.kI(); + driveConfig.Slot0.kD = gains.kD(); + driveConfig.Slot0.kS = gains.kS(); + driveConfig.Slot0.kV = gains.kV(); + driveConfig.Slot0.kA = gains.kA(); driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - steerConfig.Slot0.kP = kP; - steerConfig.Slot0.kI = kI; - steerConfig.Slot0.kD = kD; - steerConfig.Slot0.kS = kS; - steerConfig.Slot0.kV = kV; - steerConfig.Slot0.kA = kA; + public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { + steerConfig.Slot0.kP = gains.kP(); + steerConfig.Slot0.kI = gains.kI(); + steerConfig.Slot0.kD = gains.kD(); + steerConfig.Slot0.kS = gains.kS(); + steerConfig.Slot0.kV = gains.kV(); + steerConfig.Slot0.kA = gains.kA(); + steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); + steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); + steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); steerTalon.getConfigurator().apply(steerConfig); } } From be471afe4dcaf706db897e658ccf2b800d7de8df Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 26 Nov 2024 01:28:08 +0000 Subject: [PATCH 14/17] tune motion profile, kP, feedforward for steer motors to <0.01 error; placeholder drive gains --- .../java/frc/robot/subsystems/swerve/DriveConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index a583f5e..d5ce0da 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,9 +68,9 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - new Gains(0, 0, 0, 300, 0, 0), - new MotionProfileGains(0, 0, 0), // FIXME - new Gains(0, 0, 0, 1, 0, 0), + new Gains(0.25, 2.62, 0, 100, 0, 0), // revisit kP + new MotionProfileGains(4, 64, 640), // revisit all + new Gains(0.3, 0.63, 0, 2, 0, 0), // FIXME placeholder, to do 5.357142857142857, 21.428571428571427, 3.125); From cb2cb8d01e8e33f91648f962a9ff501d6da08fd2 Mon Sep 17 00:00:00 2001 From: Nora Date: Tue, 26 Nov 2024 17:50:29 -0800 Subject: [PATCH 15/17] Turning with triggers implemented --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/subsystems/swerve/Drive.java | 8 ++++++-- src/main/java/frc/robot/subsystems/swerve/Module.java | 1 - .../subsystems/swerve/controllers/TeleopController.java | 2 +- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c98522..fe71830 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,7 +100,10 @@ private void configureBindings() { .run( () -> { swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + -driverA.getLeftY(), + -driverA.getLeftX(), + driverA.getLeftTriggerAxis(), + -driverA.getRightTriggerAxis()); }) .withName("Drive Teleop")); diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 8e3c1d6..28beeba 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,6 +3,7 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -90,12 +91,15 @@ public void periodic() { Logger.recordOutput("Swerve/DriveMode", driveMode); } - public void driveTeleopController(double xAxis, double yAxis, double omega) { + public void driveTeleopController( + double xAxis, double yAxis, double leftTrigger, double rightTrigger) { if (DriverStation.isTeleopEnabled()) { if (driveMode != DriveModes.TELEOP) { driveMode = DriveModes.TELEOP; } - teleopController.acceptJoystickInput(xAxis, yAxis, omega); + + double triggers = MathUtil.applyDeadband(leftTrigger + rightTrigger, 0.07); + teleopController.acceptJoystickInput(xAxis, yAxis, triggers); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 44859c6..ce970c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -48,7 +48,6 @@ public void runToSetpoint(SwerveModuleState targetState) { Logger.recordOutput( "Swerve/Module" + index + "/SteerError", targetState.angle.getRadians() - inputs.steerAbsolutePostion.getRadians()); - Logger.recordOutput("Swerve/Module" + index + "/DriveVelocitySetpoint", driveVelocityRads); Logger.recordOutput("Swerve/Module" + index + "/DriveVelRadsScalar", driveVelocityRads); } diff --git a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java index 2c4e3a9..1756c05 100644 --- a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -27,7 +27,7 @@ public ChassisSpeeds update(Rotation2d yaw) { Translation2d linearVelocity = calculateLinearVelocity(controllerX, controllerY); double omega = MathUtil.applyDeadband(controllerOmega, 0.1); - omega = Math.copySign(Math.pow(omega, 1.5), omega); + omega = Math.copySign(Math.pow(Math.abs(omega), 1.5), omega); // eventaully run off of pose estimation? return ChassisSpeeds.fromFieldRelativeSpeeds( From 97eed50c492d58d27524de47dc7a8dcd994dbe0d Mon Sep 17 00:00:00 2001 From: Nora Date: Tue, 26 Nov 2024 17:54:35 -0800 Subject: [PATCH 16/17] Zeroing --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ src/main/java/frc/robot/subsystems/swerve/Drive.java | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe71830..c5862c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,6 +136,15 @@ private void configureBindings() { flywheels.setVelocityTarget(VelocityTarget.IDLE); }, flywheels)); + + driverA + .start() + .onTrue( + new InstantCommand( + () -> { + swerve.zero(); + }, + swerve)); } private void configureAutos() {} diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 28beeba..f625ba9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -108,4 +108,8 @@ public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { driveMode = DriveModes.TRAJECTORY; } } + + public void zero() { + gyroYawOffset = Rotation2d.fromDegrees(gyroInputs.yawPosition.getDegrees()); + } } From 111f0f9c417616b9a344b5e29b5f23803fe39e32 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 27 Nov 2024 02:19:09 +0000 Subject: [PATCH 17/17] clean up --- src/main/java/frc/robot/RobotContainer.java | 44 ++----------------- .../frc/robot/subsystems/swerve/Drive.java | 16 ++++--- 2 files changed, 13 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c5862c9..dbe4fb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,14 +4,11 @@ package frc.robot; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Mode; import frc.robot.subsystems.flywheels.Flywheels; -import frc.robot.subsystems.flywheels.Flywheels.VelocityTarget; import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX; import frc.robot.subsystems.rollers.Rollers; -import frc.robot.subsystems.rollers.Rollers.RollerState; import frc.robot.subsystems.rollers.intake.Intake; import frc.robot.subsystems.rollers.intake.IntakeIOTalonFX; import frc.robot.subsystems.swerve.Drive; @@ -102,49 +99,16 @@ private void configureBindings() { swerve.driveTeleopController( -driverA.getLeftY(), -driverA.getLeftX(), - driverA.getLeftTriggerAxis(), - -driverA.getRightTriggerAxis()); + driverA.getLeftTriggerAxis() - driverA.getRightTriggerAxis()); }) .withName("Drive Teleop")); + driverA.start().onTrue(swerve.zeroGyroCommand()); + // -----Intake Controls----- - driverA.x().whileTrue(rollers.setTargetCommand(RollerState.INTAKE)); // -----Flywheel Controls----- - // - driverA - .y() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.SHOOT); - }, - flywheels)); - driverA - .b() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.SLOW); - }, - flywheels)); - driverA - .a() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.IDLE); - }, - flywheels)); - - driverA - .start() - .onTrue( - new InstantCommand( - () -> { - swerve.zero(); - }, - swerve)); + } private void configureAutos() {} diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index f625ba9..03bf797 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,12 +3,12 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.subsystems.swerve.controllers.TeleopController; @@ -91,15 +91,13 @@ public void periodic() { Logger.recordOutput("Swerve/DriveMode", driveMode); } - public void driveTeleopController( - double xAxis, double yAxis, double leftTrigger, double rightTrigger) { + public void driveTeleopController(double xAxis, double yAxis, double omega) { if (DriverStation.isTeleopEnabled()) { if (driveMode != DriveModes.TELEOP) { driveMode = DriveModes.TELEOP; } - double triggers = MathUtil.applyDeadband(leftTrigger + rightTrigger, 0.07); - teleopController.acceptJoystickInput(xAxis, yAxis, triggers); + teleopController.acceptJoystickInput(xAxis, yAxis, omega); } } @@ -109,7 +107,11 @@ public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { } } - public void zero() { - gyroYawOffset = Rotation2d.fromDegrees(gyroInputs.yawPosition.getDegrees()); + private void zeroGyro() { + gyroYawOffset = gyroInputs.yawPosition; + } + + public Command zeroGyroCommand() { + return this.runOnce(() -> zeroGyro()); } }