From 78f1e1afe9a98439247e50ea3107d18b5125a7d5 Mon Sep 17 00:00:00 2001 From: Zach Rutman Date: Thu, 1 Aug 2024 17:05:31 -0700 Subject: [PATCH] feat: update swerve config for krakens and adjusted inversion (#53) --- .../org/team1540/robot2024/Constants.java | 12 ++++----- .../subsystems/drive/ModuleIOTalonFX.java | 5 ++-- .../robot2024/util/swerve/SwerveFactory.java | 26 ++++++++++++++++--- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 71de1fa9..14b70cd7 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -52,10 +52,10 @@ public static class SwerveConfig { public static final String CAN_BUS = "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; - public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 9; - public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 2; - public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 4 : 6; - public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 3 : 5; + public static final int FRONT_LEFT = 4; + public static final int FRONT_RIGHT = 3; + public static final int BACK_LEFT = 1; + public static final int BACK_RIGHT = 7; public static final int PIGEON_ID = 9; } @@ -208,8 +208,8 @@ public static class Elevator { public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; public static final double GEAR_RATIO = 11.571; - public static final int LEADER_ID = 7; - public static final int FOLLOWER_ID = 8; + public static final int LEADER_ID = 7; // RIGHT + public static final int FOLLOWER_ID = 8; // LEFT public static final int LEFT_FLIPPER_ID = 1; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index 9d055d52..c261d034 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -130,7 +129,7 @@ public void setTurnVoltage(double volts) { @Override public void setDriveBrakeMode(boolean enable) { MotorOutputConfigs config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; + driveTalon.getConfigurator().refresh(config); config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; driveTalon.getConfigurator().apply(config); } @@ -138,7 +137,7 @@ public void setDriveBrakeMode(boolean enable) { @Override public void setTurnBrakeMode(boolean enable) { MotorOutputConfigs config = new MotorOutputConfigs(); - config.Inverted = IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + turnTalon.getConfigurator().refresh(config); config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; turnTalon.getConfigurator().apply(config); } diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java index 2df557a4..857cd610 100644 --- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java +++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java @@ -47,7 +47,6 @@ public static class SwerveModuleHW { public final TalonFX driveMotor; public final TalonFX turnMotor; public final CANcoder cancoder; - private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { if (id < 1 || id > 9) { throw new IllegalArgumentException("Swerve module id must be between 1 and 9"); @@ -56,6 +55,27 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { canbus = ""; } + boolean invertTurn = true; + boolean invertDrive = true; + switch (corner) { + case FRONT_LEFT: + invertTurn = false; + invertDrive = false; + break; + case FRONT_RIGHT: + invertTurn = true; + invertDrive = false; + break; + case BACK_LEFT: + invertTurn = true; + invertDrive = false; + break; + case BACK_RIGHT: + invertTurn = false; + invertDrive = true; + break; + } + int driveID = 30 + id; int turnID = 20 + id; int canCoderID = 10 + id; @@ -72,11 +92,11 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0; driveConfig.CurrentLimits.SupplyTimeThreshold = 0.1; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + driveConfig.MotorOutput.Inverted = invertDrive ? InvertedValue.CounterClockwise_Positive: InvertedValue.Clockwise_Positive; turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + turnConfig.MotorOutput.Inverted = invertTurn ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; turnConfig.Feedback.FeedbackRemoteSensorID = canCoderID; turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; turnConfig.Feedback.SensorToMechanismRatio = 1.0;