diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor new file mode 100644 index 0000000..753407e --- /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":"5.27", + "val":5.27 + }, + "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 e546a3c..1433bca 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.CompBotSwerveConstants; 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; @@ -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; @@ -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/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index 78470e3..e1fcd85 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,7 @@ 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 +202,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 deleted file mode 100644 index f738ca2..0000000 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// 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; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Mass; -import frc.robot.components.camera.Camera.CameraConstants; -import frc.robot.subsystems.swerve.module.Module.ModuleConstants; - -/** Add your docs here. */ -public class CompBotSwerveConstants extends SwerveConstants { - - @Override - public CameraConstants[] getCameraConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getCameraConstants'"); - } - - @Override - public String getName() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getName'"); - } - - @Override - public double getTrackWidthX() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTrackWidthX'"); - } - - @Override - public double getTrackWidthY() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTrackWidthY'"); - } - - @Override - public double getBumperWidth() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBumperWidth'"); - } - - @Override - public double getBumperLength() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBumperLength'"); - } - - @Override - public double getMaxLinearSpeed() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMaxLinearSpeed'"); - } - - @Override - public double getMaxLinearAcceleration() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMaxLinearAcceleration'"); - } - - @Override - public double getDriveGearRatio() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getDriveGearRatio'"); - } - - @Override - public double getTurnGearRatio() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTurnGearRatio'"); - } - - @Override - public Mass getMass() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMass'"); - } - - @Override - public ModuleConstants getFrontLeftModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getFrontLeftModuleConstants'"); - } - - @Override - public ModuleConstants getFrontRightModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getFrontRightModuleConstants'"); - } - - @Override - public ModuleConstants getBackLeftModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBackLeftModuleConstants'"); - } - - @Override - public ModuleConstants getBackRightModuleConstants() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getBackRightModuleConstants'"); - } - - @Override - public int getGyroID() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getGyroID'"); - } - - @Override - public Pigeon2Configuration getGyroConfig() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getGyroConfig'"); - } - - @Override - public TalonFXConfiguration getDriveConfig() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getDriveConfiguration'"); - } - - @Override - public TalonFXConfiguration getTurnConfig(int cancoderID) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getTurnConfiguration'"); - } - - @Override - public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getCancoderConfiguration'"); - } - - @Override - public double getHeadingVelocityKP() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getHeadingVelocityKP'"); - } -} 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..4643b9b 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,7 @@ 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 +58,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..e854fd6 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,7 @@ 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 +137,20 @@ 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/constants/comp/R1CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java new file mode 100644 index 0000000..ebd2732 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1CompBotSwerveConstants.java @@ -0,0 +1,222 @@ +// Copyright (c) FIRST and other WPILib contributors. +// 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.comp; + +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 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; +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. */ +public class R1CompBotSwerveConstants extends SwerveConstants { + + // TODO!!! + @Override + public CameraConstants[] getCameraConstants() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getCameraConstants'"); + } + + @Override + public String getName() { + return "Comp"; // TODO CHANGE ONCE NAMED + } + + @Override + public double getTrackWidthX() { + return Units.inchesToMeters(21.75); + } + + @Override + public double getTrackWidthY() { + return Units.inchesToMeters(21.75); + } + + @Override + public double getBumperWidth() { + return Units.inchesToMeters(34.6); + } + + @Override + public double getBumperLength() { + return Units.inchesToMeters(34.6); + } + + @Override + public double 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() { + // Calculated in Choreo for R1 ratio + return 9.056; + } + + @Override + public double getDriveGearRatio() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + // Mk5n, R1 ratio + return 7.03; + } + + @Override + public double getTurnGearRatio() { + // From https://www.swervedrivespecialties.com/collections/kits/products/mk5n-swerve-module + return 287 / 11; + } + + @Override + public Mass getMass() { + // From CAD (retrieved 1/29/26), with bumpers and battery + return Pound.of(121.28); + } + + // TODO: CANCODER OFFSETS + @Override + public ModuleConstants getFrontLeftModuleConstants() { + return new ModuleConstants(0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); + } + + @Override + public ModuleConstants getFrontRightModuleConstants() { + return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0)); + } + + @Override + public ModuleConstants getBackLeftModuleConstants() { + return new ModuleConstants(2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); + } + + @Override + public ModuleConstants getBackRightModuleConstants() { + return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0)); + } + + @Override + public int getGyroID() { + return 0; + } + + @Override + public Pigeon2Configuration 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() { + 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) { + 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 MotorType getTurnMotorType() { + return MotorType.KrakenX44; + } + + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + + @Override + public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { + 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() { + // copied from kelpie + return 6.0; + } +} 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..6628376 --- /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; + } +} 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..d4f954c --- /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; + } +} 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; 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..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,10 +20,12 @@ 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..a7d3f98 100644 --- a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java +++ b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java @@ -20,6 +20,7 @@ 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 +37,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 +68,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