From 3fccc8027279d65a98a56dc8aede8ecaf3d62f13 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:13:09 -0800 Subject: [PATCH 1/9] Comp constants --- .../subsystems/swerve/SwerveSubsystem.java | 2 +- .../constants/CompBotSwerveConstants.java | 149 +++++++++++++----- .../swerve/module/ModuleIOReal.java | 2 +- 3 files changed, 111 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index e422313..f662e08 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -400,7 +400,7 @@ private void drive(ChassisSpeeds speeds, boolean openLoop) { for (int i = 0; i < optimizedStates.length; i++) { if (openLoop) { // Heuristic to enable/disable FOC - // enables FOC if the robot is moving at 90% of drivetrain max speed + // enables FOC if the robot is moving at less than 90% of drivetrain max speed final boolean focEnable = Math.sqrt( Math.pow(this.getVelocityRobotRelative().vxMetersPerSecond, 2) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java index f738ca2..192f6db 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java @@ -4,10 +4,18 @@ package frc.robot.subsystems.swerve.constants; +import static edu.wpi.first.units.Units.Pound; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; import frc.robot.components.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.module.Module.ModuleConstants; @@ -15,6 +23,7 @@ /** Add your docs here. */ public class CompBotSwerveConstants extends SwerveConstants { + // TODO!!! @Override public CameraConstants[] getCameraConstants() { // TODO Auto-generated method stub @@ -23,121 +32,181 @@ public CameraConstants[] getCameraConstants() { @Override public String getName() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getName'"); + return "Comp"; // TODO CHANGE ONCE NAMED } @Override public double getTrackWidthX() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTrackWidthX'"); + return Units.inchesToMeters(21.75); } @Override public double getTrackWidthY() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTrackWidthY'"); + return Units.inchesToMeters(21.75); } @Override public double getBumperWidth() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBumperWidth'"); + return Units.inchesToMeters(34.6); } @Override public double getBumperLength() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBumperLength'"); + return Units.inchesToMeters(34.6); } @Override public double getMaxLinearSpeed() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMaxLinearSpeed'"); + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // SDS Mk5n, R1 ratio, no FOC (because FOC is disabled if we're going fast enough) + return Units.feetToMeters(14.9); } @Override public double getMaxLinearAcceleration() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMaxLinearAcceleration'"); + // Calculated in Choreo + return 9.056; } @Override public double getDriveGearRatio() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getDriveGearRatio'"); + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // Mk5n, R1 ratio + return 7.03; } @Override public double getTurnGearRatio() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTurnGearRatio'"); + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + return 287 / 11; } @Override public Mass getMass() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMass'"); + // From CAD (retrieved 1/29/26), with bumpers and battery + return Pound.of(121.28); } + // TODO: CANCODER OFFSETS @Override public ModuleConstants getFrontLeftModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getFrontLeftModuleConstants'"); + return new ModuleConstants( + 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); } @Override public ModuleConstants getFrontRightModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getFrontRightModuleConstants'"); + return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0)); } @Override public ModuleConstants getBackLeftModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBackLeftModuleConstants'"); + return new ModuleConstants( + 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); } @Override public ModuleConstants getBackRightModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBackRightModuleConstants'"); + return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0)); } @Override public int getGyroID() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getGyroID'"); + return 0; } @Override public Pigeon2Configuration getGyroConfig() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getGyroConfig'"); + Pigeon2Configuration config = new Pigeon2Configuration(); + config.MountPose.MountPosePitch = 0.0; + config.MountPose.MountPoseRoll = 0.0; + config.MountPose.MountPoseYaw = 0.0; + return config; } @Override public TalonFXConfiguration getDriveConfig() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getDriveConfiguration'"); + var driveConfig = new TalonFXConfiguration(); + // Current limits + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.CurrentLimits.StatorCurrentLimit = 120.0; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + // Inverts + driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // Sensor + // Meters per second + driveConfig.Feedback.SensorToMechanismRatio = getDriveRotorToMeters(); + // Current control gains + // Gains copied from Kelpie Swerve Constants + // May need tuning + driveConfig.Slot0.kV = 5.0; + // kT (stall torque / stall current) converted to linear wheel frame + driveConfig.Slot0.kA = 0.0; // (9.37 / 483.0) / getDriveRotorToMeters(); // 3.07135116146; + driveConfig.Slot0.kS = 10.0; + driveConfig.Slot0.kP = 300.0; + driveConfig.Slot0.kD = 0.0; // 1.0; + + driveConfig.TorqueCurrent.TorqueNeutralDeadband = 10.0; + + driveConfig.MotionMagic.MotionMagicCruiseVelocity = getMaxLinearSpeed(); + driveConfig.MotionMagic.MotionMagicAcceleration = getMaxLinearAcceleration(); + + return driveConfig; } @Override public TalonFXConfiguration getTurnConfig(int cancoderID) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTurnConfiguration'"); + var turnConfig = new TalonFXConfiguration(); + // Current limits + turnConfig.CurrentLimits.SupplyCurrentLimit = 20.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + // Inverts + turnConfig.MotorOutput.Inverted = + getTurnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // Fused Cancoder + turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + turnConfig.Feedback.FeedbackRemoteSensorID = cancoderID; + turnConfig.Feedback.RotorToSensorRatio = getTurnGearRatio(); + turnConfig.Feedback.SensorToMechanismRatio = 1.0; + turnConfig.Feedback.FeedbackRotorOffset = 0.0; + // Controls Gains + // Copied from Kelpie + turnConfig.Slot0.kV = ((5800 / 60) / getTurnGearRatio()) / 12; + turnConfig.Slot0.kA = 0.031543; + turnConfig.Slot0.kS = 0.27; + turnConfig.Slot0.kP = 20.0; + turnConfig.Slot0.kD = 0.68275; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = (5500 / 60) / getTurnGearRatio(); + turnConfig.MotionMagic.MotionMagicAcceleration = (5500 / 60) / (getTurnGearRatio() * 0.005); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + + return turnConfig; + } + + @Override + public boolean getTurnMotorInverted() { + return false; // Checked this on a module } @Override public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getCancoderConfiguration'"); + final var cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = cancoderOffset.getRotations(); + cancoderConfig.MagnetSensor.SensorDirection = + getTurnMotorInverted() + ? SensorDirectionValue.CounterClockwise_Positive + : SensorDirectionValue.Clockwise_Positive; + return cancoderConfig; } @Override public double getHeadingVelocityKP() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getHeadingVelocityKP'"); + // copied from kelpie + return 6.0; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java index cc29237..13ec955 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java @@ -37,7 +37,7 @@ public static class ModuleIOInputs { public double driveStatorCurrentAmps = 0.0; public double driveSupplyCurrentAmps = 0.0; - // For the turn motor + // For the x motor public boolean turnConnected = false; public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; From 1ed1a0ab42843831cd6c8707a584c2b799a82236 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:14:24 -0800 Subject: [PATCH 2/9] Rename to R1 constants --- src/main/deploy/choreo/rebuiltChoreo.chor | 254 ++++++++++++++++++ .../subsystems/swerve/SwerveSubsystem.java | 4 +- ...nts.java => R1CompBotSwerveConstants.java} | 2 +- 3 files changed, 257 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/choreo/rebuiltChoreo.chor rename src/main/java/frc/robot/subsystems/swerve/constants/{CompBotSwerveConstants.java => R1CompBotSwerveConstants.java} (99%) diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor new file mode 100644 index 0000000..ab33d7f --- /dev/null +++ b/src/main/deploy/choreo/rebuiltChoreo.chor @@ -0,0 +1,254 @@ +{ + "name":"rebuiltChoreo", + "version":2, + "type":"Swerve", + "variables":{ + "expressions":{ + "slow":{ + "dimension":"LinVel", + "var":{ + "exp":"1.15 m / s", + "val":1.15 + } + }, + "slowaccel":{ + "dimension":"LinAcc", + "var":{ + "exp":"1.5 m / s ^ 2", + "val":1.5 + } + } + }, + "poses":{ + "CL":{ + "x":{ + "exp":"1.7636412382125854 m", + "val":1.7636412382125854 + }, + "y":{ + "exp":"4.111053943634033 m", + "val":4.111053943634033 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "CM":{ + "x":{ + "exp":"1.7500560283660889 m", + "val":1.7500560283660889 + }, + "y":{ + "exp":"3.7646303176879883 m", + "val":3.764630317687988 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "CR":{ + "x":{ + "exp":"1.7568485736846924 m", + "val":1.7568485736846924 + }, + "y":{ + "exp":"3.397829055786133 m", + "val":3.397829055786133 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "D":{ + "x":{ + "exp":"1.307854413986206 m", + "val":1.307854413986206 + }, + "y":{ + "exp":"5.920979976654053 m", + "val":5.920979976654053 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "FL":{ + "x":{ + "exp":"7.8203654289245605 m", + "val":7.82036542892456 + }, + "y":{ + "exp":"5.865901947021484 m", + "val":5.865901947021484 + }, + "heading":{ + "exp":"-1.5707963267948966 rad", + "val":-1.5707963267948966 + } + }, + "FLM":{ + "x":{ + "exp":"7.8203654289245605 m", + "val":7.82036542892456 + }, + "y":{ + "exp":"4.503572463989258 m", + "val":4.503572463989258 + }, + "heading":{ + "exp":"-1.5707963267948966 rad", + "val":-1.5707963267948966 + } + }, + "FR":{ + "x":{ + "exp":"7.8203654289245605 m", + "val":7.82036542892456 + }, + "y":{ + "exp":"2.2169458866119385 m", + "val":2.2169458866119385 + }, + "heading":{ + "exp":"1.5707963267948966 rad", + "val":1.5707963267948966 + } + }, + "FRM":{ + "x":{ + "exp":"7.8203654289245605 m", + "val":7.82036542892456 + }, + "y":{ + "exp":"3.55698299407959 m", + "val":3.55698299407959 + }, + "heading":{ + "exp":"1.5707963267948966 rad", + "val":1.5707963267948966 + } + }, + "O":{ + "x":{ + "exp":"0.6150895357131958 m", + "val":0.6150895357131958 + }, + "y":{ + "exp":"0.7198631167411804 m", + "val":0.7198631167411804 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "PLO":{ + "x":{ + "exp":"2.814626932144165 m", + "val":2.814626932144165 + }, + "y":{ + "exp":"7.330637454986572 m", + "val":7.330637454986572 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + }, + "PR":{ + "x":{ + "exp":"2.768587827682495 m", + "val":2.768587827682495 + }, + "y":{ + "exp":"0.7205682992935181 m", + "val":0.7205682992935181 + }, + "heading":{ + "exp":"3.141592653589793 rad", + "val":3.141592653589793 + } + } + } + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"23.75 / 2 in", + "val":0.301625 + }, + "y":{ + "exp":"23.25 / 2 in", + "val":0.295275 + } + }, + "backLeft":{ + "x":{ + "exp":"-23.75 / 2 in", + "val":-0.301625 + }, + "y":{ + "exp":"23.25 / 2 in", + "val":0.295275 + } + }, + "mass":{ + "exp":"121.28 lb", + "val":55.0116826336 + }, + "inertia":{ + "exp":"24001.681 in ^ 2 lb", + "val":7.023843609558215 + }, + "gearing":{ + "exp":"7.03", + "val":7.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "tmax":{ + "exp":"0.9 N * m", + "val":0.9 + }, + "cof":{ + "exp":"2.255", + "val":2.255 + }, + "bumper":{ + "front":{ + "exp":"34.6 / 2 in", + "val":0.43942 + }, + "side":{ + "exp":"34.6 / 2 in", + "val":0.43942 + }, + "back":{ + "exp":"34.6 / 2 in", + "val":0.43942 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index f662e08..dcbd636 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -33,7 +33,7 @@ import frc.robot.components.camera.CameraIOReal; import frc.robot.components.camera.CameraIOSim; import frc.robot.subsystems.swerve.constants.AlphaSwerveConstants; -import frc.robot.subsystems.swerve.constants.CompBotSwerveConstants; +import frc.robot.subsystems.swerve.constants.R1CompBotSwerveConstants; import frc.robot.subsystems.swerve.constants.SwerveConstants; import frc.robot.subsystems.swerve.gyro.GyroIO; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; @@ -70,7 +70,7 @@ public class SwerveSubsystem extends SubsystemBase { public static final SwerveConstants SWERVE_CONSTANTS = Robot.ROBOT_EDITION == RobotEdition.ALPHA ? new AlphaSwerveConstants() - : new CompBotSwerveConstants(); + : new R1CompBotSwerveConstants(); private final Module[] modules; // Front Left, Front Right, Back Left, Back Right private final GyroIO gyroIO; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java similarity index 99% rename from src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java rename to src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java index 192f6db..983e5f6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java @@ -21,7 +21,7 @@ import frc.robot.subsystems.swerve.module.Module.ModuleConstants; /** Add your docs here. */ -public class CompBotSwerveConstants extends SwerveConstants { +public class R1CompBotSwerveConstants extends SwerveConstants { // TODO!!! @Override From 872740632844ae948e20f3a6801d4603dbc1bc99 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:14:50 -0800 Subject: [PATCH 3/9] Move to comp package for organization --- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- .../swerve/constants/{ => comp}/R1CompBotSwerveConstants.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) rename src/main/java/frc/robot/subsystems/swerve/constants/{ => comp}/R1CompBotSwerveConstants.java (98%) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index dcbd636..f2d5b8c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -33,8 +33,8 @@ import frc.robot.components.camera.CameraIOReal; import frc.robot.components.camera.CameraIOSim; import frc.robot.subsystems.swerve.constants.AlphaSwerveConstants; -import frc.robot.subsystems.swerve.constants.R1CompBotSwerveConstants; import frc.robot.subsystems.swerve.constants.SwerveConstants; +import frc.robot.subsystems.swerve.constants.comp.R1CompBotSwerveConstants; import frc.robot.subsystems.swerve.gyro.GyroIO; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; import frc.robot.subsystems.swerve.gyro.GyroIOReal; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java similarity index 98% rename from src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java rename to src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java index 983e5f6..6e2b687 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/R1CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems.swerve.constants; +package frc.robot.subsystems.swerve.constants.comp; import static edu.wpi.first.units.Units.Pound; @@ -18,6 +18,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; import frc.robot.components.camera.Camera.CameraConstants; +import frc.robot.subsystems.swerve.constants.SwerveConstants; import frc.robot.subsystems.swerve.module.Module.ModuleConstants; /** Add your docs here. */ From 7770941e1ab56ef246aa83edef2982489cc5f5fa Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:18:32 -0800 Subject: [PATCH 4/9] R2 constants --- src/main/deploy/choreo/rebuiltChoreo.chor | 4 +-- .../comp/R1CompBotSwerveConstants.java | 2 +- .../comp/R2CompBotSwerveConstants.java | 25 +++++++++++++++++++ 3 files changed, 28 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor index ab33d7f..1d3360c 100644 --- a/src/main/deploy/choreo/rebuiltChoreo.chor +++ b/src/main/deploy/choreo/rebuiltChoreo.chor @@ -206,8 +206,8 @@ "val":7.023843609558215 }, "gearing":{ - "exp":"7.03", - "val":7.03 + "exp":"6.03", + "val":6.03 }, "radius":{ "exp":"2 in", diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java index 6e2b687..835f951 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java @@ -65,7 +65,7 @@ public double getMaxLinearSpeed() { @Override public double getMaxLinearAcceleration() { - // Calculated in Choreo + // Calculated in Choreo for R1 ratio return 9.056; } diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java new file mode 100644 index 0000000..43825b9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.swerve.constants.comp; + +import edu.wpi.first.math.util.Units; + +public class R2CompBotSwerveConstants extends R1CompBotSwerveConstants { + @Override + public double getMaxLinearSpeed() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // SDS Mk5n, R2 ratio, no FOC (because FOC is disabled if we're going fast enough) + return Units.feetToMeters(17.4); + } + + @Override + public double getMaxLinearAcceleration() { + // Calculated in Choreo for R2 ratio + return 7.768; + } + + @Override + public double getDriveGearRatio() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // Mk5n, R2 ratio + return 6.03; + } +} From a03164d82878f93201f17c8c4133935528fbfe68 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:20:43 -0800 Subject: [PATCH 5/9] R3 constants --- src/main/deploy/choreo/rebuiltChoreo.chor | 4 +-- .../comp/R3CompBotSwerveConstants.java | 25 +++++++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor index 1d3360c..753407e 100644 --- a/src/main/deploy/choreo/rebuiltChoreo.chor +++ b/src/main/deploy/choreo/rebuiltChoreo.chor @@ -206,8 +206,8 @@ "val":7.023843609558215 }, "gearing":{ - "exp":"6.03", - "val":6.03 + "exp":"5.27", + "val":5.27 }, "radius":{ "exp":"2 in", diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java new file mode 100644 index 0000000..e74d9bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.swerve.constants.comp; + +import edu.wpi.first.math.util.Units; + +public class R3CompBotSwerveConstants extends R1CompBotSwerveConstants { + @Override + public double getMaxLinearSpeed() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // SDS Mk5n, R3 ratio, no FOC (because FOC is disabled if we're going fast enough) + return Units.feetToMeters(19.9); + } + + @Override + public double getMaxLinearAcceleration() { + // Calculated in Choreo for R3 ratio + return 6.789; + } + + @Override + public double getDriveGearRatio() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // Mk5n, R3 ratio + return 5.27; + } +} From 0e170669b447eb7e98cdbdc753b2cf05966f7454 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 19:21:06 -0800 Subject: [PATCH 6/9] Spotless --- .../swerve/constants/comp/R1CompBotSwerveConstants.java | 7 ++----- .../swerve/constants/comp/R2CompBotSwerveConstants.java | 2 +- .../swerve/constants/comp/R3CompBotSwerveConstants.java | 2 +- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java index 835f951..52e310c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java @@ -13,7 +13,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; @@ -91,8 +90,7 @@ public Mass getMass() { // TODO: CANCODER OFFSETS @Override public ModuleConstants getFrontLeftModuleConstants() { - return new ModuleConstants( - 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); + return new ModuleConstants(0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); } @Override @@ -102,8 +100,7 @@ public ModuleConstants getFrontRightModuleConstants() { @Override public ModuleConstants getBackLeftModuleConstants() { - return new ModuleConstants( - 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); + return new ModuleConstants(2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java index 43825b9..6628376 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R2CompBotSwerveConstants.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.util.Units; public class R2CompBotSwerveConstants extends R1CompBotSwerveConstants { - @Override + @Override public double getMaxLinearSpeed() { // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module // SDS Mk5n, R2 ratio, no FOC (because FOC is disabled if we're going fast enough) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java index e74d9bf..d4f954c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R3CompBotSwerveConstants.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.util.Units; public class R3CompBotSwerveConstants extends R1CompBotSwerveConstants { - @Override + @Override public double getMaxLinearSpeed() { // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module // SDS Mk5n, R3 ratio, no FOC (because FOC is disabled if we're going fast enough) From f53ac5514c7e1ed3973e3e1e21690d9e598a1f1a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 1 Feb 2026 14:46:35 -0800 Subject: [PATCH 7/9] Add ability to set drive and turn motor types in swerve constants --- .../swerve/constants/AlphaSwerveConstants.java | 12 ++++++++++++ .../swerve/constants/CompBotSwerveConstants.java | 12 ++++++++++++ .../swerve/constants/SeahorseSwerveConstants.java | 12 ++++++++++++ .../swerve/constants/SwerveConstants.java | 14 ++++++++++++++ .../subsystems/swerve/module/ModuleIOSim.java | 3 ++- .../java/frc/robot/utils/MaplePhoenixUtil.java | 8 ++++++-- 6 files changed, 58 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index 78470e3..57bfec6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -13,6 +13,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -201,6 +203,16 @@ public ModuleConstants getBackRightModuleConstants() { return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.205)); } + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + @Override public int getGyroID() { return 0; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java index f738ca2..4a7a2ad 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java @@ -7,6 +7,8 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Mass; import frc.robot.components.camera.Camera.CameraConstants; @@ -129,6 +131,16 @@ public TalonFXConfiguration getTurnConfig(int cancoderID) { throw new UnsupportedOperationException("Unimplemented method 'getTurnConfiguration'"); } + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX44; + } + @Override public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { // TODO Auto-generated method stub diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java index 0ec6f5c..1da1ae8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java @@ -9,6 +9,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -57,6 +59,16 @@ public int getGyroID() { return 0; } + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX60; + } + public Pigeon2Configuration getGyroConfig() { Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = -0.002175945555791259; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index c97139b..297db05 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -3,6 +3,8 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; @@ -136,6 +138,18 @@ public double getWheelRadiusMeters() { */ public abstract TalonFXConfiguration getTurnConfig(int cancoderID); + /** + * The motor type of the swerve turn motors (i.e. x44 vs x60) + * @return + */ + public abstract MotorType getTurnMotorType(); + + /** + * The motor type of the swerve drive motors (i.e. x44 vs x60) + * @return + */ + public abstract MotorType getDriveMotorType(); + /** * The configuration for a swerve-module cancoder * diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index dad28f6..e4d45fd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -20,10 +20,11 @@ public ModuleIOSim( // using Async odo) this.simulation = simulation; this.simulation.useDriveMotorController( - new MaplePhoenixUtil.TalonFXMotorControllerSim(driveTalon, true)); + new MaplePhoenixUtil.TalonFXMotorControllerSim(driveTalon, SwerveSubsystem.SWERVE_CONSTANTS.getDriveMotorType(), true)); this.simulation.useSteerMotorController( new MaplePhoenixUtil.TalonFXMotorControllerWithRemoteCancoderSim( turnTalon, + SwerveSubsystem.SWERVE_CONSTANTS.getTurnMotorType(), SwerveSubsystem.SWERVE_CONSTANTS.getTurnMotorInverted(), cancoder, false, diff --git a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java index 2fda1f0..cbf2d7a 100644 --- a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java +++ b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java @@ -20,6 +20,8 @@ import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -36,10 +38,11 @@ public static class TalonFXMotorControllerSim implements SimulatedMotorControlle private final TalonFXSimState talonFXSimState; - public TalonFXMotorControllerSim(TalonFX talonFX, boolean motorInverted) { + public TalonFXMotorControllerSim(TalonFX talonFX, MotorType motorType, boolean motorInverted) { this.id = instances++; this.talonFXSimState = talonFX.getSimState(); + this.talonFXSimState.setMotorType(motorType); talonFXSimState.Orientation = motorInverted ? ChassisReference.Clockwise_Positive @@ -66,11 +69,12 @@ public static class TalonFXMotorControllerWithRemoteCancoderSim public TalonFXMotorControllerWithRemoteCancoderSim( TalonFX talonFX, + MotorType motorType, boolean motorInverted, CANcoder cancoder, boolean encoderInverted, Angle encoderOffset) { - super(talonFX, motorInverted); + super(talonFX, motorType, motorInverted); this.remoteCancoderSimState = cancoder.getSimState(); this.remoteCancoderSimState.Orientation = encoderInverted From ec1943cd0fc60c1c8127a67c8f6ee827a1366cae Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 1 Feb 2026 14:56:02 -0800 Subject: [PATCH 8/9] Spotless --- .../subsystems/swerve/constants/AlphaSwerveConstants.java | 3 +-- .../subsystems/swerve/constants/CompBotSwerveConstants.java | 3 +-- .../subsystems/swerve/constants/SeahorseSwerveConstants.java | 5 ++--- .../robot/subsystems/swerve/constants/SwerveConstants.java | 3 ++- .../java/frc/robot/subsystems/swerve/module/ModuleIOSim.java | 3 ++- src/main/java/frc/robot/utils/MaplePhoenixUtil.java | 1 - 6 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index 57bfec6..e1fcd85 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -210,7 +209,7 @@ public MotorType getTurnMotorType() { @Override public MotorType getDriveMotorType() { - return MotorType.KrakenX60; + return MotorType.KrakenX60; } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java index 4a7a2ad..da3f0a6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Mass; import frc.robot.components.camera.Camera.CameraConstants; @@ -138,7 +137,7 @@ public MotorType getDriveMotorType() { @Override public MotorType getTurnMotorType() { - return MotorType.KrakenX44; + return MotorType.KrakenX44; } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java index 1da1ae8..4643b9b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -61,12 +60,12 @@ public int getGyroID() { @Override public MotorType getDriveMotorType() { - return MotorType.KrakenX60; + return MotorType.KrakenX60; } @Override public MotorType getTurnMotorType() { - return MotorType.KrakenX60; + return MotorType.KrakenX60; } public Pigeon2Configuration getGyroConfig() { diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index 297db05..e854fd6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; @@ -140,12 +139,14 @@ public double getWheelRadiusMeters() { /** * The motor type of the swerve turn motors (i.e. x44 vs x60) + * * @return */ public abstract MotorType getTurnMotorType(); /** * The motor type of the swerve drive motors (i.e. x44 vs x60) + * * @return */ public abstract MotorType getDriveMotorType(); diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index e4d45fd..4ba4d8c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -20,7 +20,8 @@ public ModuleIOSim( // using Async odo) this.simulation = simulation; this.simulation.useDriveMotorController( - new MaplePhoenixUtil.TalonFXMotorControllerSim(driveTalon, SwerveSubsystem.SWERVE_CONSTANTS.getDriveMotorType(), true)); + new MaplePhoenixUtil.TalonFXMotorControllerSim( + driveTalon, SwerveSubsystem.SWERVE_CONSTANTS.getDriveMotorType(), true)); this.simulation.useSteerMotorController( new MaplePhoenixUtil.TalonFXMotorControllerWithRemoteCancoderSim( turnTalon, diff --git a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java index cbf2d7a..a7d3f98 100644 --- a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java +++ b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java @@ -21,7 +21,6 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; From a47bcd2a87a1b673cdac6ade57407b843318c10a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 1 Feb 2026 15:05:25 -0800 Subject: [PATCH 9/9] Add motor type getters to comp bot constants --- .../constants/comp/R1CompBotSwerveConstants.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java index 52e310c..ebd2732 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java @@ -13,6 +13,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; @@ -191,6 +193,16 @@ public boolean getTurnMotorInverted() { return false; // Checked this on a module } + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX44; + } + + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + @Override public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { final var cancoderConfig = new CANcoderConfiguration();