From ad3dc9a3464372fd226a530e2fbcb923aeb0633b Mon Sep 17 00:00:00 2001 From: mixxlto Date: Sat, 17 Jan 2026 14:02:42 -0800 Subject: [PATCH] wafflehouse drivetrain --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/RobotId.java | 4 +- .../constants/swerve/DriveConstants.java | 413 ++++++++++-------- .../java/lib/COTSFalconSwerveConstants.java | 34 ++ 4 files changed, 258 insertions(+), 195 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69d2f08..12f4f8e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,7 @@ public class RobotContainer { default: - case Wafflehouse: + case WaffleHouse: case SwerveCompetition: diff --git a/src/main/java/frc/robot/RobotId.java b/src/main/java/frc/robot/RobotId.java index bd26230..6cba572 100644 --- a/src/main/java/frc/robot/RobotId.java +++ b/src/main/java/frc/robot/RobotId.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.Preferences; */ public enum RobotId { Default, - SwerveCompetition, Vertigo, Vivace, Phil, BetaBot, + WaffleHouse, SwerveCompetition, Vertigo, Vivace, Phil, BetaBot, ClassBot1, ClassBot2, ClassBot3, ClassBot4, TestBed1, TestBed2; @@ -24,7 +24,7 @@ public enum RobotId { */ @Deprecated public boolean isClassBot() { - return this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4; + return this == WaffleHouse || this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4; } /** diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index 3e57260..1f7b10d 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -43,198 +43,227 @@ public class DriveConstants { /** Drive gear ratio for an Mk4i with L2-Plus gearing */ public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0); // all MK4i modules have the same steering gear ratio - public static final double STEER_GEAR_RATIO = 150.0 / 7.0; - - /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */ - public static final double MAX_SPEED = 4.5; - - // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot) - // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length. - public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2)); - - public static final double COSF = 0.9; + public static double STEER_GEAR_RATIO = 150.0 / 7.0; - // The maximum acceleration of the robot, limited by friction - public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION; - // The maximum amount a drive motor can accelerate, independant of friction - // This does nothing if greater than LINEAR_ACCEL - public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL; - // The maximum angular acceleration of the robot - public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2); - - /** - * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint. - *

If this is true, Drivetrain will use the actual current setpoint instead. - */ - public static final boolean USE_ACTUAL_SPEED = false; - - /** - * Disables the deadband and optimization for the modules. - * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband. - * Setting this to true fixes bugs caused by using hte actual current state. - */ - public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false; - - public static final Rotation2d STARTING_HEADING = new Rotation2d(); - - public static final Translation2d[] MODULE_LOCATIONS = { - new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), - new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2), - new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), - new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2) - }; - - public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS); - - public static double STEER_OFFSET_FRONT_LEFT = 302.646; - public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180; - public static double STEER_OFFSET_BACK_LEFT = 165.49+90; - public static double STEER_OFFSET_BACK_RIGHT = 73.563; - - // Heading PID. - public static final double HEADING_P = 5.5; - public static final double HEADING_D = 0; - - public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5); - - // Translational PID - // TODO: Tune this better (low priority since we aren't using it in 2025) - public static final double TRANSLATIONAL_P = 1; - public static final double TRANSLATIONAL_D = 0.001; - - //The PIDs for PathPlanner Command - public static final double PATH_PLANNER_HEADING_P = 3.5; - public static final double PATH_PLANNER_HEADING_D = 0; - - public static final double PATH_PLANNER_TRANSLATIONAL_P = 6; - public static final double PATH_PLANNER_TRANSLATIONAL_D = 0; - - // CAN - public static String DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN; - public static String STEER_MOTOR_CAN = Constants.CANIVORE_CAN; - public static String STEER_ENCODER_CAN = Constants.CANIVORE_CAN; - public static String PIGEON_CAN = Constants.CANIVORE_CAN; - - - public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); - - /* Swerve Current Limiting */ - public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15; - public static final int STEER_PEAK_CURRENT_LIMIT = 15; - public static final double STEER_PEAK_CURRENT_DURATION = 0.01; - public static final boolean STEER_ENABLE_CURRENT_LIMIT = true; - - public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60; - public static final int DRIVE_PEAK_CURRENT_LIMIT = 60; - public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01; - public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; - - /* Motor inversions */ - public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive; - public static final InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive; - - /* Neutral Modes */ - public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake; - public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake; - - /* Drive Motor PID Values */ - public static final double[] P_VALUES = { - 0.1, - 0.1, - 0.1, - 0.1 - }; - public static final double[] I_VALUES = { - 0, - 0, - 0, - 0 - }; - public static final double[] D_VALUES = { - 0, - 0, - 0, - 0 - }; - /* Drive Motor Characterization Values - * Divide SYSID values by 12 to convert from volts to percent output for CTRE */ - public static final double[] S_VALUES = { - 0.11, - 0.11, - 0.11, - 0.11 - }; - public static final double[] V_VALUES = { - 0.11079, - 0.10718, - 0.11009, - 0.1164 - }; - public static final double[] A_VALUES = { - 0.005482, - 0.0049593, - 0.010156, - 0.0065708 - }; - /* Ramp values for drive motors in open loop driving. */ - // Open loop prevents throttle from changing too quickly. - // It will limit it to time given (in seconds) to go from zero to full throttle. - // A small open loop ramp (0.25) helps with tread wear, tipping, etc - public static final double OPEN_LOOP_RAMP = 0.25; - - public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS; - - public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW- - - public static final double SLOW_DRIVE_FACTOR = 0.2; - public static final double SLOW_ROT_FACTOR = 0.1; - - public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO)); - - /** - * Updates the constants if the RobotId is not the competition robot. - */ - public static void update(RobotId robotId) { - if(robotId == RobotId.BetaBot) { - STEER_OFFSET_FRONT_LEFT = 193.884-180; - STEER_OFFSET_FRONT_RIGHT = 110.914; - STEER_OFFSET_BACK_LEFT = 128.054+180; - STEER_OFFSET_BACK_RIGHT = 107.43; - } else if (robotId == RobotId.Vivace) { - STEER_OFFSET_FRONT_LEFT = 100.184+180; - STEER_OFFSET_FRONT_RIGHT = 224.293; - STEER_OFFSET_BACK_LEFT = 304.795-180; - STEER_OFFSET_BACK_RIGHT = 201.177-180; - - ROBOT_MASS = 50; - WHEEL_MOI = 0.000326 * ROBOT_MASS; - - } else if (robotId == RobotId.Vertigo) { - STEER_OFFSET_FRONT_LEFT = 4.185; - STEER_OFFSET_FRONT_RIGHT = 101.519+90; - STEER_OFFSET_BACK_LEFT = 38.997+180; - STEER_OFFSET_BACK_RIGHT = 242.847-90; - - DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - - ROBOT_MASS = 20; - - WHEEL_MOI = 0.000326 * ROBOT_MASS; - - // Falcon Speed - Constants.MAX_RPM = 6080.0; - } else if (robotId == RobotId.Phil) { - ROBOT_MASS = 30; - WHEEL_MOI = 0.000326 * ROBOT_MASS; - - STEER_OFFSET_FRONT_LEFT = 121.463+180; - STEER_OFFSET_FRONT_RIGHT = 284.242; - STEER_OFFSET_BACK_LEFT = 157.676; - STEER_OFFSET_BACK_RIGHT = 77.199; - - DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - } + /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */ + public static final double MAX_SPEED = 4.5; + + // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot) + // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length. + public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2)); + + public static final double COSF = 0.9; - MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); - } + // The maximum acceleration of the robot, limited by friction + public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION; + // The maximum amount a drive motor can accelerate, independant of friction + // This does nothing if greater than LINEAR_ACCEL + public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL; + // The maximum angular acceleration of the robot + public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2); + + /** + * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint. + *

If this is true, Drivetrain will use the actual current setpoint instead. + */ + public static final boolean USE_ACTUAL_SPEED = false; + + /** + * Disables the deadband and optimization for the modules. + * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband. + * Setting this to true fixes bugs caused by using hte actual current state. + */ + public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false; + + public static final Rotation2d STARTING_HEADING = new Rotation2d(); + + public static final Translation2d[] MODULE_LOCATIONS = { + new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), + new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2), + new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), + new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2) + }; + + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS); + + public static double STEER_OFFSET_FRONT_LEFT = 302.646; + public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180; + public static double STEER_OFFSET_BACK_LEFT = 165.49+90; + public static double STEER_OFFSET_BACK_RIGHT = 73.563; + + // Heading PID. + public static final double HEADING_P = 5.5; + public static final double HEADING_D = 0; + + public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5); + + // Translational PID + // TODO: Tune this better (low priority since we aren't using it in 2025) + public static final double TRANSLATIONAL_P = 1; + public static final double TRANSLATIONAL_D = 0.001; + + //The PIDs for PathPlanner Command + public static final double PATH_PLANNER_HEADING_P = 3.5; + public static final double PATH_PLANNER_HEADING_D = 0; + + public static final double PATH_PLANNER_TRANSLATIONAL_P = 6; + public static final double PATH_PLANNER_TRANSLATIONAL_D = 0; + + // CAN + public static String DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN; + public static String STEER_MOTOR_CAN = Constants.CANIVORE_CAN; + public static String STEER_ENCODER_CAN = Constants.CANIVORE_CAN; + public static String PIGEON_CAN = Constants.CANIVORE_CAN; + + + public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + /* Swerve Current Limiting */ + public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15; + public static final int STEER_PEAK_CURRENT_LIMIT = 15; + public static final double STEER_PEAK_CURRENT_DURATION = 0.01; + public static final boolean STEER_ENABLE_CURRENT_LIMIT = true; + + public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60; + public static final int DRIVE_PEAK_CURRENT_LIMIT = 60; + public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01; + public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; + + /* Motor inversions */ + public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive; + + /* Neutral Modes */ + public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake; + public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake; + + /* Drive Motor PID Values */ + public static final double[] P_VALUES = { + 0.1, + 0.1, + 0.1, + 0.1 + }; + public static final double[] I_VALUES = { + 0, + 0, + 0, + 0 + }; + public static final double[] D_VALUES = { + 0, + 0, + 0, + 0 + }; + /* Drive Motor Characterization Values + * Divide SYSID values by 12 to convert from volts to percent output for CTRE */ + public static final double[] S_VALUES = { + 0.11, + 0.11, + 0.11, + 0.11 + }; + public static final double[] V_VALUES = { + 0.11079, + 0.10718, + 0.11009, + 0.1164 + }; + public static final double[] A_VALUES = { + 0.005482, + 0.0049593, + 0.010156, + 0.0065708 + }; + /* Ramp values for drive motors in open loop driving. */ + // Open loop prevents throttle from changing too quickly. + // It will limit it to time given (in seconds) to go from zero to full throttle. + // A small open loop ramp (0.25) helps with tread wear, tipping, etc + public static final double OPEN_LOOP_RAMP = 0.25; + + public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS; + + public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW- + + public static final double SLOW_DRIVE_FACTOR = 0.2; + public static final double SLOW_ROT_FACTOR = 0.1; + + public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO)); + + /** + * Updates the constants if the RobotId is not the competition robot. + */ + public static void update(RobotId robotId) { + if(robotId == RobotId.WaffleHouse){ + STEER_OFFSET_FRONT_LEFT = 300.058594 - 350 + 180; + STEER_OFFSET_FRONT_RIGHT = 65.654297 + 180; + STEER_OFFSET_BACK_LEFT = 38.232422 + 180 + 180; + STEER_OFFSET_BACK_RIGHT = 116.279297 + 180; + + DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); + STEER_GEAR_RATIO = 287.0 / 11.0; + + } else if(robotId == RobotId.SwerveCompetition){ + STEER_OFFSET_FRONT_LEFT = 302.646; + STEER_OFFSET_FRONT_RIGHT = 103.039+180; + STEER_OFFSET_BACK_LEFT = 165.49+90; + STEER_OFFSET_BACK_RIGHT = 73.563; + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } else if(robotId == RobotId.BetaBot) { + STEER_OFFSET_FRONT_LEFT = 193.884-180; + STEER_OFFSET_FRONT_RIGHT = 110.914; + STEER_OFFSET_BACK_LEFT = 128.054+180; + STEER_OFFSET_BACK_RIGHT = 107.43; + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } else if (robotId == RobotId.Vivace) { + STEER_OFFSET_FRONT_LEFT = 100.184+180; + STEER_OFFSET_FRONT_RIGHT = 224.293; + STEER_OFFSET_BACK_LEFT = 304.795-180; + STEER_OFFSET_BACK_RIGHT = 201.177-180; + + ROBOT_MASS = 50; + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } else if (robotId == RobotId.Vertigo) { + STEER_OFFSET_FRONT_LEFT = 4.185; + STEER_OFFSET_FRONT_RIGHT = 101.519+90; + STEER_OFFSET_BACK_LEFT = 38.997+180; + STEER_OFFSET_BACK_RIGHT = 242.847-90; + + DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + + ROBOT_MASS = 20; + + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + // Falcon Speed + Constants.MAX_RPM = 6080.0; + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } else if (robotId == RobotId.Phil) { + ROBOT_MASS = 30; + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + STEER_OFFSET_FRONT_LEFT = 121.463+180; + STEER_OFFSET_FRONT_RIGHT = 284.242; + STEER_OFFSET_BACK_LEFT = 157.676; + STEER_OFFSET_BACK_RIGHT = 77.199; + + DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } else{ + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + } + } } diff --git a/src/main/java/lib/COTSFalconSwerveConstants.java b/src/main/java/lib/COTSFalconSwerveConstants.java index d4a9009..f4b8229 100644 --- a/src/main/java/lib/COTSFalconSwerveConstants.java +++ b/src/main/java/lib/COTSFalconSwerveConstants.java @@ -90,6 +90,26 @@ public class COTSFalconSwerveConstants { return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert); } + /** + * Swerve Drive Specialties - MK5n Module + */ + public static COTSFalconSwerveConstants SDSMK5n(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (287 / 11) : 1 */ + double angleGearRatio = ((287.0 / 11.0)); + + double angleKP = 0.3; + double angleKI = 0.0; + double angleKD = 0.0; + double angleKF = 0.0; + + boolean driveMotorInvert = false; + boolean angleMotorInvert = true; + boolean canCoderInvert = false; + return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert); + } + /* Drive Gear Ratios for all supported modules */ public static class DriveGearRatios { /* SDS MK3 */ @@ -133,6 +153,20 @@ public class COTSFalconSwerveConstants { * SDS MK4i - 6.12 : 1 */ public static final double SDSMK4i_L3 = (6.12); + + /* SDS MK5n */ + /** + * SDS MK5n - 7.13 : 1 + */ + public static final double SDSMK5n_L1_PLUS = (8.13); + /** + * SDS MK4i - 5.9 : 1 + */ + public static final double SDSMK5n_L2_PLUS = (5.9); + /** + * SDS MK4i - 5.36 : 1 + */ + public static final double SDSMK5n_L3_PLUS = (5.35); } } -- 2.39.5