diff --git a/src/main/deploy/config-common.jsonc b/src/main/deploy/config-common.jsonc new file mode 100644 index 0000000..fb0bc98 --- /dev/null +++ b/src/main/deploy/config-common.jsonc @@ -0,0 +1,118 @@ +{ + "FEATURE_FLAGS": { + "ENABLE_DRIVE": true + }, + "DRIVE_INFO": { + "STATUS_SIGNAL_FREQUENCY": 200, + "PIGEON_ID": 1, + "FRONT_LEFT_MODULE_DRIVE_MOTOR": 2, + "FRONT_LEFT_MODULE_STEER_MOTOR": 3, + "FRONT_LEFT_MODULE_STEER_ENCODER": 4, + "FRONT_LEFT_MODULE_STEER_OFFSET": 52.053, + "FRONT_LEFT_DRIVE_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KI": 0, + "KP": 0.38, + "KS": 0.24, + "KV": 0.123, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + "FRONT_LEFT_STEER_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KS": 0.5, + "KI": 0, + "KP": 4, + "KV": 0.15, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + "FRONT_RIGHT_MODULE_DRIVE_MOTOR": 8, + "FRONT_RIGHT_MODULE_STEER_MOTOR": 9, + "FRONT_RIGHT_MODULE_STEER_ENCODER": 10, + "FRONT_RIGHT_MODULE_STEER_OFFSET": -16.963, + "FRONT_RIGHT_DRIVE_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KI": 0, + "KP": 0.38, + "KS": 0.24, + "KV": 0.123, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + "FRONT_RIGHT_STEER_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KS": 0.5, + "KI": 0, + "KP": 4, + "KV": 0.15, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + "BACK_LEFT_MODULE_DRIVE_MOTOR": 5, + "BACK_LEFT_MODULE_STEER_MOTOR": 6, + "BACK_LEFT_MODULE_STEER_ENCODER": 7, + "BACK_LEFT_MODULE_STEER_OFFSET": 104.678, + "BACK_LEFT_DRIVE_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KI": 0, + "KP": 0.38, + "KS": 0.24, + "KV": 0.123, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "CounterClockwise_Positive" + }, + "BACK_LEFT_STEER_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KS": 0.5, + "KI": 0, + "KP": 4, + "KV": 0.15, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + "BACK_RIGHT_MODULE_DRIVE_MOTOR": 11, + "BACK_RIGHT_MODULE_STEER_MOTOR": 12, + "BACK_RIGHT_MODULE_STEER_ENCODER": 13, + "BACK_RIGHT_MODULE_STEER_OFFSET": -48.779, + "BACK_RIGHT_DRIVE_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KI": 0, + "KP": 0.38, + "KS": 0.24, + "KV": 0.123, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "CounterClockwise_Positive" + }, + "BACK_RIGHT_STEER_MOTOR_CONFIG": { + "KA": 0, + "KD": 0, + "KS": 0.5, + "KI": 0, + "KP": 4, + "KV": 0.15, + "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0, + "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive" + }, + // 1/6.98 + // https://docs.wcproducts.com/welcome/gearboxes/wcp-swerve-x2/general-info/ratio-options + // X1 with 11 tooth pinion + "DRIVE_GEAR_RATIO": 0.1432664756446991, + "ANGLE_GEAR_RATIO": 0.08264462809917356, + "WHEEL_DIAMETER_METERS": 0.0975325866, + "TRACKWIDTH_METERS": 0.635, + "WHEELBASE_METERS": 0.4826, + "FULL_WIDTH_METERS": 0.9, + "FULL_LENGTH_METERS": 0.9, + "BUMPER_HEIGHT_METERS": 0.1, + "MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND": 7.33038286, // 420 deg/s measured 22. March + "MAX_LINEAR_VELOCITY_METERS_PER_SECOND": 4.0 // Estimated March 28 + } +} diff --git a/src/main/deploy/config-simulation.jsonc b/src/main/deploy/config-simulation.jsonc new file mode 100644 index 0000000..82aee30 --- /dev/null +++ b/src/main/deploy/config-simulation.jsonc @@ -0,0 +1,4 @@ +// Overrides for simulation go here. +{ + "ROBORIO_CANBUS": "simulation" +} \ No newline at end of file diff --git a/src/main/java/com/team973/frc2025/RobotConfig.java b/src/main/java/com/team973/frc2025/RobotConfig.java new file mode 100644 index 0000000..67c42c1 --- /dev/null +++ b/src/main/java/com/team973/frc2025/RobotConfig.java @@ -0,0 +1,107 @@ +package com.team973.frc2025; + +import com.fasterxml.jackson.core.JsonParser; +import com.fasterxml.jackson.core.json.JsonReadFeature; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.team973.frc2025.shared.RobotInfo; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; + +public class RobotConfig { + + private static boolean m_initialized; + private static RobotInfo m_robotInfo; + + // The following are set once during initialization so that we can log them + private static String m_configProfile; + + public static RobotInfo get() { + maybeInitialize(); + return m_robotInfo; + } + + public static String getConfigProfile() { + maybeInitialize(); + return m_configProfile; + } + + private static synchronized void maybeInitialize() { + if (m_initialized) { + return; + } + List configPaths = getConfigPaths(); + System.err.printf("Using configSources: %s\n", configPaths); + m_robotInfo = getMergedConfig(configPaths); + m_initialized = true; + } + + public static String readConfigProfile() throws IOException { + String path = Filesystem.getOperatingDirectory().toPath().resolve("profile").toString(); + return Files.readString(Paths.get(path)); + } + + public static RobotInfo getMergedConfig(List configPaths) { + RobotInfo res = new RobotInfo(); + + for (String path : configPaths) { + + ObjectMapper mapper = new ObjectMapper(); + mapper.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, true); + mapper.configure(JsonReadFeature.ALLOW_TRAILING_COMMA.mappedFeature(), true); + mapper.setDefaultMergeable(true); + mapper.configure(JsonParser.Feature.ALLOW_COMMENTS, true); + mapper.configOverride(List.class).setMergeable(false); + try { + mapper.readerForUpdating(res).readValue(new File(path)); + } catch (com.fasterxml.jackson.core.JsonParseException e) { + System.err.printf("JSON parsing error in %s: %s\n", path, e.getMessage()); + throw new RuntimeException(e); + } catch (IOException e) { + System.err.println("Could not read file: " + e.getMessage()); + throw new RuntimeException(e); + } + } + + return res; + } + + /** configSuffix is something like "common", "simulation", */ + public static String absPathFromConfigSuffix(String configSuffix) { + return Filesystem.getDeployDirectory() + .toPath() + .resolve("config-" + configSuffix.trim() + ".jsonc") + .toString(); + } + + public static List getConfigPaths() { + List res = new ArrayList<>(); + + // Every robot starts out with the base config (config-common.yaml) + res.add(absPathFromConfigSuffix("common")); + + if (RobotBase.isSimulation()) { + m_configProfile = "simulation"; + res.add(absPathFromConfigSuffix("simulation")); + } else { + try { + m_configProfile = readConfigProfile(); + res.add(absPathFromConfigSuffix(m_configProfile)); + } catch (IOException e) { + // On a competition robot this isn't really a big deal because the common + // config should be complete, so we can continue on here. Setting the + // config profile to ERROR will signal that something failed during setup. + System.err.printf("Failed to read config profile: %s\n", e.getMessage()); + m_configProfile = "ERROR"; + } + } + + return res; + } +} diff --git a/src/main/java/com/team973/frc2025/shared/RobotInfo.java b/src/main/java/com/team973/frc2025/shared/RobotInfo.java index ed18f58..0a54e69 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -1,151 +1,174 @@ package com.team973.frc2025.shared; -import static edu.wpi.first.units.Units.Inches; - +import com.team973.lib.devices.GreyTalonFX; import com.team973.lib.util.SwerveModuleConfig; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.system.plant.DCMotor; -import org.ironmaple.simulation.drivesims.COTS; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; /** Robot info, specs, dimensions. */ public class RobotInfo { - public static final String CANIVORE_CANBUS = "Canivore"; // "Canivore"; - public static final String ROBORIO_CANBUS = ""; + public static String CANIVORE_CANBUS = "Canivore"; + public String ROBORIO_CANBUS = ""; + + public FeatureFlags FEATURE_FLAGS = new FeatureFlags(); + public DriveInfo DRIVE_INFO = new DriveInfo(); - public static final DriveInfo DRIVE_INFO = new DriveInfo(); + public static class FeatureFlags { + public boolean ENABLE_DRIVE; + } public static class DriveInfo { - public final int STATUS_SIGNAL_FREQUENCY = 200; + public int STATUS_SIGNAL_FREQUENCY = 200; - public final int PIGEON_ID = 1; + public int PIGEON_ID = 1; - public final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 2; - public final int FRONT_LEFT_MODULE_STEER_MOTOR = 3; - public final int FRONT_LEFT_MODULE_STEER_ENCODER = 4; - public final double FRONT_LEFT_MODULE_STEER_OFFSET = -95.273; + public int FRONT_LEFT_MODULE_DRIVE_MOTOR; + public int FRONT_LEFT_MODULE_STEER_MOTOR; + public int FRONT_LEFT_MODULE_STEER_ENCODER; + public double FRONT_LEFT_MODULE_STEER_OFFSET; + public GreyTalonFX.Config FRONT_LEFT_DRIVE_MOTOR_CONFIG; + public GreyTalonFX.Config FRONT_LEFT_STEER_MOTOR_CONFIG; - public final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 8; - public final int FRONT_RIGHT_MODULE_STEER_MOTOR = 9; - public final int FRONT_RIGHT_MODULE_STEER_ENCODER = 10; - public final double FRONT_RIGHT_MODULE_STEER_OFFSET = 152.929; + public int FRONT_RIGHT_MODULE_DRIVE_MOTOR; + public int FRONT_RIGHT_MODULE_STEER_MOTOR; + public int FRONT_RIGHT_MODULE_STEER_ENCODER; + public double FRONT_RIGHT_MODULE_STEER_OFFSET; + public GreyTalonFX.Config FRONT_RIGHT_DRIVE_MOTOR_CONFIG; + public GreyTalonFX.Config FRONT_RIGHT_STEER_MOTOR_CONFIG; - public final int BACK_LEFT_MODULE_DRIVE_MOTOR = 5; - public final int BACK_LEFT_MODULE_STEER_MOTOR = 6; - public final int BACK_LEFT_MODULE_STEER_ENCODER = 7; - public final double BACK_LEFT_MODULE_STEER_OFFSET = 44.472; + public int BACK_LEFT_MODULE_DRIVE_MOTOR; + public int BACK_LEFT_MODULE_STEER_MOTOR; + public int BACK_LEFT_MODULE_STEER_ENCODER; + public double BACK_LEFT_MODULE_STEER_OFFSET; + public GreyTalonFX.Config BACK_LEFT_DRIVE_MOTOR_CONFIG; + public GreyTalonFX.Config BACK_LEFT_STEER_MOTOR_CONFIG; - public final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 11; - public final int BACK_RIGHT_MODULE_STEER_MOTOR = 12; - public final int BACK_RIGHT_MODULE_STEER_ENCODER = 13; - public final double BACK_RIGHT_MODULE_STEER_OFFSET = 100.458; + public int BACK_RIGHT_MODULE_DRIVE_MOTOR; + public int BACK_RIGHT_MODULE_STEER_MOTOR; + public int BACK_RIGHT_MODULE_STEER_ENCODER; + public double BACK_RIGHT_MODULE_STEER_OFFSET; + public GreyTalonFX.Config BACK_RIGHT_DRIVE_MOTOR_CONFIG; + public GreyTalonFX.Config BACK_RIGHT_STEER_MOTOR_CONFIG; - public final double DRIVE_GEAR_RATIO = - (10.0 / 54.0) * (40.0 / 16.0) * (15.0 / 45.0); // x3:10, 6.48:1 + public double DRIVE_GEAR_RATIO; - public final double ANGLE_GEAR_RATIO = (10.0 / 22.0) * (16.0 / 88.0); // 12.1:1 + public double ANGLE_GEAR_RATIO; - public final double WHEEL_DIAMETER_METERS = 0.1016; - public final double WHEEL_CIRCUMFERENCE_METERS = WHEEL_DIAMETER_METERS * Math.PI; + public double WHEEL_DIAMETER_METERS; + + public double getWheelCircumferenceMeters() { + return WHEEL_DIAMETER_METERS * Math.PI; + } /** * The left-to-right distance between the drivetrain wheels Should be measured from center to * center. */ - public static final double TRACKWIDTH_METERS = 0.5334; + public double TRACKWIDTH_METERS; /** * The front-to-back distance between the drivetrain wheels. Should be measured from center to * center. */ - public static final double WHEELBASE_METERS = 0.5334; - - public final double OPEN_LOOP_RAMP = 0.0; - public final double CLOSED_LOOP_RAMP = 0.0; - - /* Angle Motor PID Values */ - public final double ANGLE_KP = 5.5; // 6.5; - public final double ANGLE_KI = 0.0; - public final double ANGLE_KD = 0.0; - public final double ANGLE_KF = 0.0; - public final double ANGLE_KV = 0.0; + public double WHEELBASE_METERS; - /* Drive Motor PID Values */ - public final double DRIVE_KP = 0.38; - public final double DRIVE_KI = 0.0; - public final double DRIVE_KD = 0.0; - public final double DRIVE_KF = 0.12; + public double FULL_WIDTH_METERS; - /* Motor Inverts */ - public final boolean DRIVE_MOTOR_INVERT = true; - public final boolean ANGLE_MOTOR_INVERT = true; + public double FULL_LENGTH_METERS; - /* Angle Encoder Invert */ - public final boolean CANCODER_INVERT = false; + public double BUMPER_HEIGHT_METERS; - // final public double FALCON_TRAP_FREE_SPEED = 6380.0; - public final double KRAKEN_TRAP_FREE_SPEED = 6000.0; - public final double MAX_ACCELERATION_METERS_PER_SECOND = 3.0; // 4.3; - public final double LINEAR_METERS_PER_WHEEL_ROTATIONS = WHEEL_DIAMETER_METERS * Math.PI; + public double getLinearMetersPerWheelRotations() { + return WHEEL_DIAMETER_METERS * Math.PI; + } /** Measured Max Speed: 4.724 MPS */ - public final double MAX_VELOCITY_METERS_PER_SECOND = - (KRAKEN_TRAP_FREE_SPEED / 60.0 * DRIVE_GEAR_RATIO * LINEAR_METERS_PER_WHEEL_ROTATIONS); + public double getMaxVelocityMetersPerSecond() { + return MAX_LINEAR_VELOCITY_METERS_PER_SECOND; + } /** Measured Max Angular Speed: 12.65 RadPS */ - public final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = 13.3; - - public final SwerveModuleConfig FRONT_LEFT_CONSTANTS = - new SwerveModuleConfig( - FRONT_LEFT_MODULE_DRIVE_MOTOR, - FRONT_LEFT_MODULE_STEER_MOTOR, - FRONT_LEFT_MODULE_STEER_ENCODER, - FRONT_LEFT_MODULE_STEER_OFFSET); - public final SwerveModuleConfig FRONT_RIGHT_CONSTANTS = - new SwerveModuleConfig( - FRONT_RIGHT_MODULE_DRIVE_MOTOR, - FRONT_RIGHT_MODULE_STEER_MOTOR, - FRONT_RIGHT_MODULE_STEER_ENCODER, - FRONT_RIGHT_MODULE_STEER_OFFSET); - public final SwerveModuleConfig BACK_LEFT_CONSTANTS = - new SwerveModuleConfig( - BACK_LEFT_MODULE_DRIVE_MOTOR, - BACK_LEFT_MODULE_STEER_MOTOR, - BACK_LEFT_MODULE_STEER_ENCODER, - BACK_LEFT_MODULE_STEER_OFFSET); - public final SwerveModuleConfig BACK_RIGHT_CONSTANTS = - new SwerveModuleConfig( - BACK_RIGHT_MODULE_DRIVE_MOTOR, - BACK_RIGHT_MODULE_STEER_MOTOR, - BACK_RIGHT_MODULE_STEER_ENCODER, - BACK_RIGHT_MODULE_STEER_OFFSET); - - public final SwerveDriveKinematics SWERVE_KINEMATICS = - new SwerveDriveKinematics( - new Translation2d(TRACKWIDTH_METERS / 2.0, WHEELBASE_METERS / 2.0), - new Translation2d(TRACKWIDTH_METERS / 2.0, -WHEELBASE_METERS / 2.0), - new Translation2d(-TRACKWIDTH_METERS / 2.0, WHEELBASE_METERS / 2.0), - new Translation2d(-TRACKWIDTH_METERS / 2.0, -WHEELBASE_METERS / 2.0)); - - public final DriveTrainSimulationConfig DRIVE_TRAIN_SIMULATION_CONFIG = - DriveTrainSimulationConfig.Default() - // Specify gyro type (for realistic gyro drifting and error simulation) - .withGyro(COTS.ofPigeon2()) - // Specify swerve module (for realistic swerve dynamics) - .withSwerveModule( - COTS.ofMark4( - DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 - DCMotor.getKrakenX60(1), // Steer motor is a Kraken - COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels - 3)) // L3 Gear ratio - // Configures the track length and track width (spacing between swerve modules) - .withTrackLengthTrackWidth(Inches.of(26), Inches.of(26)) - // Configures the bumper size (dimensions of the robot bumper) - .withBumperSize(Inches.of(34), Inches.of(34)); - - public final Pose2d SIM_STARTING_POSE = new Pose2d(7.18, 5.7, Rotation2d.fromDegrees(180)); + public double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; + + public double MAX_LINEAR_VELOCITY_METERS_PER_SECOND; + + private SwerveModuleConfig m_frontLeftConstants; + + public SwerveModuleConfig getFrontLeftConstants() { + if (m_frontLeftConstants == null) { + m_frontLeftConstants = + new SwerveModuleConfig( + FRONT_LEFT_MODULE_DRIVE_MOTOR, + FRONT_LEFT_MODULE_STEER_MOTOR, + FRONT_LEFT_MODULE_STEER_ENCODER, + FRONT_LEFT_MODULE_STEER_OFFSET, + FRONT_LEFT_DRIVE_MOTOR_CONFIG, + FRONT_LEFT_STEER_MOTOR_CONFIG); + } + return m_frontLeftConstants; + } + + private SwerveModuleConfig m_frontRightConstants; + + public SwerveModuleConfig getFrontRightConstants() { + if (m_frontRightConstants == null) { + m_frontRightConstants = + new SwerveModuleConfig( + FRONT_RIGHT_MODULE_DRIVE_MOTOR, + FRONT_RIGHT_MODULE_STEER_MOTOR, + FRONT_RIGHT_MODULE_STEER_ENCODER, + FRONT_RIGHT_MODULE_STEER_OFFSET, + FRONT_RIGHT_DRIVE_MOTOR_CONFIG, + FRONT_RIGHT_STEER_MOTOR_CONFIG); + } + return m_frontRightConstants; + } + + private SwerveModuleConfig m_backLeftConstants; + + public SwerveModuleConfig getBackLeftConstants() { + if (m_backLeftConstants == null) { + m_backLeftConstants = + new SwerveModuleConfig( + BACK_LEFT_MODULE_DRIVE_MOTOR, + BACK_LEFT_MODULE_STEER_MOTOR, + BACK_LEFT_MODULE_STEER_ENCODER, + BACK_LEFT_MODULE_STEER_OFFSET, + BACK_LEFT_DRIVE_MOTOR_CONFIG, + BACK_LEFT_STEER_MOTOR_CONFIG); + } + return m_backLeftConstants; + } + + private SwerveModuleConfig m_backRightConstants; + + public SwerveModuleConfig getBackRightConstants() { + if (m_backRightConstants == null) { + m_backRightConstants = + new SwerveModuleConfig( + BACK_RIGHT_MODULE_DRIVE_MOTOR, + BACK_RIGHT_MODULE_STEER_MOTOR, + BACK_RIGHT_MODULE_STEER_ENCODER, + BACK_RIGHT_MODULE_STEER_OFFSET, + BACK_RIGHT_DRIVE_MOTOR_CONFIG, + BACK_RIGHT_STEER_MOTOR_CONFIG); + } + return m_backRightConstants; + } + + private SwerveDriveKinematics m_swerveKinematics; + + public synchronized SwerveDriveKinematics getSwerveDriveKinematics() { + + if (m_swerveKinematics == null) { + m_swerveKinematics = + new SwerveDriveKinematics( + new Translation2d(TRACKWIDTH_METERS / 2.0, WHEELBASE_METERS / 2.0), + new Translation2d(TRACKWIDTH_METERS / 2.0, -WHEELBASE_METERS / 2.0), + new Translation2d(-TRACKWIDTH_METERS / 2.0, WHEELBASE_METERS / 2.0), + new Translation2d(-TRACKWIDTH_METERS / 2.0, -WHEELBASE_METERS / 2.0)); + } + return m_swerveKinematics; + } } } diff --git a/src/main/java/com/team973/frc2025/subsystems/Drive.java b/src/main/java/com/team973/frc2025/subsystems/Drive.java index 1bf83ee..e8fcb79 100644 --- a/src/main/java/com/team973/frc2025/subsystems/Drive.java +++ b/src/main/java/com/team973/frc2025/subsystems/Drive.java @@ -1,7 +1,7 @@ package com.team973.frc2025.subsystems; +import com.team973.frc2025.RobotConfig; import com.team973.frc2025.shared.RobotInfo; -import com.team973.frc2025.shared.RobotInfo.DriveInfo; import com.team973.frc2025.subsystems.swerve.GreyPoseEstimator; import com.team973.frc2025.subsystems.swerve.MegaTagSupplier; import com.team973.frc2025.subsystems.swerve.OdometrySupplier; @@ -22,13 +22,13 @@ import java.util.concurrent.atomic.AtomicReference; public class Drive extends Subsystem.Stateless { - private final RobotInfo.DriveInfo m_driveInfo; + private final RobotInfo.DriveInfo m_driveInfo = RobotConfig.get().DRIVE_INFO; - private static final Translation2d[] MODULE_LOCATIONS = { - new Translation2d(DriveInfo.TRACKWIDTH_METERS / 2.0, DriveInfo.WHEELBASE_METERS / 2.0), - new Translation2d(DriveInfo.TRACKWIDTH_METERS / 2.0, -DriveInfo.WHEELBASE_METERS / 2.0), - new Translation2d(-DriveInfo.TRACKWIDTH_METERS / 2.0, DriveInfo.WHEELBASE_METERS / 2.0), - new Translation2d(-DriveInfo.TRACKWIDTH_METERS / 2.0, -DriveInfo.WHEELBASE_METERS / 2.0) + private final Translation2d[] MODULE_LOCATIONS = { + new Translation2d(m_driveInfo.TRACKWIDTH_METERS / 2.0, m_driveInfo.WHEELBASE_METERS / 2.0), + new Translation2d(m_driveInfo.TRACKWIDTH_METERS / 2.0, -m_driveInfo.WHEELBASE_METERS / 2.0), + new Translation2d(-m_driveInfo.TRACKWIDTH_METERS / 2.0, m_driveInfo.WHEELBASE_METERS / 2.0), + new Translation2d(-m_driveInfo.TRACKWIDTH_METERS / 2.0, -m_driveInfo.WHEELBASE_METERS / 2.0) }; private final Logger m_logger; @@ -66,9 +66,8 @@ public Drive( m_pigeon = pigeon; m_driveController = driveController; m_logger = logger; - m_driveInfo = RobotInfo.DRIVE_INFO; - System.out.println("Front_left_constants" + m_driveInfo.FRONT_LEFT_CONSTANTS); + System.out.println("Front_left_constants" + m_driveInfo.getFrontLeftConstants()); m_swerveModules = new SwerveModuleIO[] {frontLeft, frontRight, backLeft, backRight}; m_odometrySupplier = @@ -171,7 +170,7 @@ public void xOutModules() { /* Used by Auto */ public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, m_driveInfo.MAX_VELOCITY_METERS_PER_SECOND); + desiredStates, m_driveInfo.MAX_LINEAR_VELOCITY_METERS_PER_SECOND); double states[] = new double[8]; int index = 0; @@ -215,18 +214,6 @@ public void resetModules() { } } - public void enableBrakeMode() { - for (var mod : m_swerveModules) { - mod.driveBrake(); - } - } - - public void disableBrakeMode() { - for (var mod : m_swerveModules) { - mod.driveNeutral(); - } - } - public void log() { m_logger.log("X Meters Per Second", m_poseEstimator.getVelocityMetersPerSeconds().getX()); m_logger.log("Y Meters Per Second", m_poseEstimator.getVelocityMetersPerSeconds().getY()); @@ -281,7 +268,7 @@ public void update() { new ChassisSpeeds(twist_vel.dx / 0.03, twist_vel.dy / 0.03, twist_vel.dtheta / 0.03); SwerveModuleState[] swerveModuleStates = - m_driveInfo.SWERVE_KINEMATICS.toSwerveModuleStates(updated_chassis_speeds); + m_driveInfo.getSwerveDriveKinematics().toSwerveModuleStates(updated_chassis_speeds); setModuleStates(swerveModuleStates); } diff --git a/src/main/java/com/team973/frc2025/subsystems/composables/DriveWithJoysticks.java b/src/main/java/com/team973/frc2025/subsystems/composables/DriveWithJoysticks.java index 1d490d0..67fed0a 100644 --- a/src/main/java/com/team973/frc2025/subsystems/composables/DriveWithJoysticks.java +++ b/src/main/java/com/team973/frc2025/subsystems/composables/DriveWithJoysticks.java @@ -1,5 +1,6 @@ package com.team973.frc2025.subsystems.composables; +import com.team973.frc2025.RobotConfig; import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController.RotationControl; import com.team973.lib.util.DriveComposable; @@ -28,7 +29,7 @@ public class DriveWithJoysticks extends DriveComposable { private double m_rot = 0.0; public DriveWithJoysticks() { - m_driveInfo = RobotInfo.DRIVE_INFO; + m_driveInfo = RobotConfig.get().DRIVE_INFO; } public void setRotationControl(RotationControl rotationControl) { @@ -62,9 +63,9 @@ public void exit() {} @Override public ChassisSpeeds getOutput(Pose2d currentPose, Rotation2d angularVelocity) { final double xSpeed = - -MathUtil.applyDeadband(m_xAxis, 0.1) * m_driveInfo.MAX_VELOCITY_METERS_PER_SECOND; + -MathUtil.applyDeadband(m_xAxis, 0.1) * m_driveInfo.MAX_LINEAR_VELOCITY_METERS_PER_SECOND; final double ySpeed = - -MathUtil.applyDeadband(m_yAxis, 0.1) * m_driveInfo.MAX_VELOCITY_METERS_PER_SECOND; + -MathUtil.applyDeadband(m_yAxis, 0.1) * m_driveInfo.MAX_LINEAR_VELOCITY_METERS_PER_SECOND; Rotation2d currentYaw = currentPose.getRotation(); double rot = diff --git a/src/main/java/com/team973/frc2025/subsystems/swerve/GreyPoseEstimator.java b/src/main/java/com/team973/frc2025/subsystems/swerve/GreyPoseEstimator.java index 77e762d..c6351d2 100644 --- a/src/main/java/com/team973/frc2025/subsystems/swerve/GreyPoseEstimator.java +++ b/src/main/java/com/team973/frc2025/subsystems/swerve/GreyPoseEstimator.java @@ -1,5 +1,6 @@ package com.team973.frc2025.subsystems.swerve; +import com.team973.frc2025.RobotConfig; import com.team973.frc2025.shared.CrashTracker; import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; @@ -32,7 +33,7 @@ public GreyPoseEstimator( DriveController m_DriveController, OdometrySupplier odometrySupplier, Logger logger) { - m_driveInfo = RobotInfo.DRIVE_INFO; + m_driveInfo = RobotConfig.get().DRIVE_INFO; m_pigeon = pigeon; m_driveController = m_DriveController; @@ -76,7 +77,7 @@ public void observeOdometryData( if (m_poseEstimator == null) { m_poseEstimator = new SwerveDrivePoseEstimator( - m_driveInfo.SWERVE_KINEMATICS, + m_driveInfo.getSwerveDriveKinematics(), gyroAngle, modulePositions, new Pose2d(0, 0, gyroAngle)); diff --git a/src/main/java/com/team973/frc2025/subsystems/swerve/OdometrySupplier.java b/src/main/java/com/team973/frc2025/subsystems/swerve/OdometrySupplier.java index a06becd..0847d7e 100644 --- a/src/main/java/com/team973/frc2025/subsystems/swerve/OdometrySupplier.java +++ b/src/main/java/com/team973/frc2025/subsystems/swerve/OdometrySupplier.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.team973.frc2025.RobotConfig; import com.team973.frc2025.shared.RobotInfo; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.util.Logger; @@ -54,7 +55,7 @@ public OdometrySupplier(GreyPigeonIO pigeon, SwerveModuleIO[] swerveModules, Log m_thread.setName("swerve.OdometryPoseSupplier"); m_thread.setDaemon(false); - m_driveInfo = RobotInfo.DRIVE_INFO; + m_driveInfo = RobotConfig.get().DRIVE_INFO; m_pigeon = pigeon; m_swerveModules = swerveModules; @@ -93,7 +94,7 @@ public OdometrySupplier(GreyPigeonIO pigeon, SwerveModuleIO[] swerveModules, Log BaseStatusSignal.getLatencyCompensatedValue(m_yawGetter, m_angularVelocity); m_swerveOdometry = new SwerveDriveOdometry( - m_driveInfo.SWERVE_KINEMATICS, + m_driveInfo.getSwerveDriveKinematics(), Rotation2d.fromDegrees(yawDegrees.magnitude()), getPositions()); m_lastPoseMeters = getPoseMeters(); diff --git a/src/main/java/com/team973/frc2025/subsystems/swerve/SwerveModule.java b/src/main/java/com/team973/frc2025/subsystems/swerve/SwerveModule.java index 6d35e11..fdcd4d9 100644 --- a/src/main/java/com/team973/frc2025/subsystems/swerve/SwerveModule.java +++ b/src/main/java/com/team973/frc2025/subsystems/swerve/SwerveModule.java @@ -3,10 +3,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.team973.frc2025.RobotConfig; import com.team973.frc2025.shared.RobotInfo; import com.team973.lib.devices.GreyCANCoder; import com.team973.lib.devices.GreyTalonFX; @@ -39,8 +37,6 @@ public class SwerveModule implements SwerveModuleIO { private SwerveModuleState m_lastState; - private final TalonFXConfiguration m_driveMotorConfig; - private final StatusSignal m_driveMotorPositionStatusSignal; private final StatusSignal m_driveMotorVelocityStatusSignal; private final StatusSignal m_angleMotorPositionStatusSignal; @@ -52,7 +48,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfig moduleConfig, Logger lo this.moduleNumber = moduleNumber; m_logger = logger; m_angleOffset = Rotation2d.fromDegrees(moduleConfig.angleOffset); - m_driveInfo = RobotInfo.DRIVE_INFO; + m_driveInfo = RobotConfig.get().DRIVE_INFO; m_driveMechanism = new LinearMechanism(m_driveInfo.DRIVE_GEAR_RATIO, m_driveInfo.WHEEL_DIAMETER_METERS); @@ -72,7 +68,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfig moduleConfig, Logger lo moduleConfig.angleMotorID, RobotInfo.CANIVORE_CANBUS, logger.subLogger("Angle Motor", 0.1)); - configAngleMotor(); + m_angleMotor.setConfig(moduleConfig.angleMotorConfig.getConfig()); /* Drive Motor Config */ m_driveMotor = @@ -80,8 +76,8 @@ public SwerveModule(int moduleNumber, SwerveModuleConfig moduleConfig, Logger lo moduleConfig.driveMotorID, RobotInfo.CANIVORE_CANBUS, logger.subLogger("Drive Motor", 0.1)); - m_driveMotorConfig = m_driveMotor.getCurrentConfig(); - configDriveMotor(); + m_driveMotor.setConfig(moduleConfig.driveMotorConfig.getConfig()); + m_driveMotor.setPosition(0.0); BaseStatusSignal.waitForAll(0.5, m_angleEncoder.getAbsolutePosition()); resetToAbsolute(); @@ -108,54 +104,9 @@ public SwerveModule(int moduleNumber, SwerveModuleConfig moduleConfig, Logger lo private void configAngleEncoder() { var encoderConfig = new CANcoderConfiguration(); encoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - // encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; m_angleEncoder.getConfigurator().apply(encoderConfig); } - private void configAngleMotor() { - var motorConfig = m_angleMotor.getCurrentConfig(); - - motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - - motorConfig.Slot0.kP = m_driveInfo.ANGLE_KP; - motorConfig.Slot0.kI = m_driveInfo.ANGLE_KI; - motorConfig.Slot0.kD = m_driveInfo.ANGLE_KD; - motorConfig.Slot0.kS = m_driveInfo.ANGLE_KF; - - motorConfig.CurrentLimits.StatorCurrentLimit = 100.0; - motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; - - motorConfig.CurrentLimits.SupplyCurrentLimit = 60.0; - motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - - motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.02; - - m_angleMotor.setConfig(motorConfig); - - resetToAbsolute(); - } - - private void configDriveMotor() { - m_driveMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - m_driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - m_driveMotorConfig.Slot0.kP = m_driveInfo.DRIVE_KP; - m_driveMotorConfig.Slot0.kI = m_driveInfo.DRIVE_KI; - m_driveMotorConfig.Slot0.kD = m_driveInfo.DRIVE_KD; - m_driveMotorConfig.Slot0.kV = m_driveInfo.DRIVE_KF; - - m_driveMotorConfig.CurrentLimits.StatorCurrentLimit = 100.0; - m_driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; - m_driveMotorConfig.CurrentLimits.SupplyCurrentLimit = 60.0; - m_driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - - m_driveMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.02; - - m_driveMotor.setConfig(m_driveMotorConfig); - m_driveMotor.setPosition(0.0); - } - @Override public int getModuleNumber() { return moduleNumber; @@ -262,7 +213,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreJitter if (!ignoreJitter) { desiredState.angle = (Math.abs(desiredState.speedMetersPerSecond) - <= (m_driveInfo.MAX_VELOCITY_METERS_PER_SECOND * 0.01)) + <= (m_driveInfo.MAX_LINEAR_VELOCITY_METERS_PER_SECOND * 0.01)) ? m_lastState.angle : desiredState.angle; } @@ -276,16 +227,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreJitter m_lastState = desiredState; } - public void driveBrake() { - m_driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_driveMotor.setConfig(m_driveMotorConfig); - } - - public void driveNeutral() { - m_driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - m_driveMotor.setConfig(m_driveMotorConfig); - } - public void log() { m_driveMotor.log(); m_angleMotor.log(); diff --git a/src/main/java/com/team973/lib/devices/GreyTalonFX.java b/src/main/java/com/team973/lib/devices/GreyTalonFX.java index 4c4b85c..19b5421 100644 --- a/src/main/java/com/team973/lib/devices/GreyTalonFX.java +++ b/src/main/java/com/team973/lib/devices/GreyTalonFX.java @@ -633,7 +633,30 @@ public void logFault(String faultName, boolean value) { SmartDashboard.putBoolean("TalonFX/" + m_deviceID + " Faults/" + faultName, value); } - public static class GreyTalonFXConfig { + public static class Config { + public double KS = 0.0; + public double KV = 0.0; + public double KA = 0.0; + public double KP = 0.0; + public double KI = 0.0; + public double KD = 0.0; + + public double STATOR_CURRENT_LIMIT = 60.0; + public boolean STATOR_CURRENT_LIMIT_ENABLE = true; + public double SUPPLY_CURRENT_LIMIT = 40.0; + public boolean SUPPLY_CURRENT_LIMIT_ENABLE = true; + + public double PEAK_FORWARD_VOLTAGE = 12.0; + public double PEAK_REVERSE_VOLTAGE = -12.0; + + public double MOTION_MAGIC_CRUISE_VELOCITY = 10.0; + public double MOTION_MAGIC_ACCELERATION = 10.0; + public double MOTION_MAGIC_JERK = 0.0; + + public final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.0; + + public InvertedValue MOTOR_ROTATION_DIRECTION = InvertedValue.CounterClockwise_Positive; + public NeutralModeValue MOTOR_MODE = NeutralModeValue.Coast; public TalonFXConfiguration getConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -642,17 +665,22 @@ public TalonFXConfiguration getConfig() { config.CurrentLimits.StatorCurrentLimitEnable = STATOR_CURRENT_LIMIT_ENABLE; config.CurrentLimits.SupplyCurrentLimit = SUPPLY_CURRENT_LIMIT; config.CurrentLimits.SupplyCurrentLimitEnable = SUPPLY_CURRENT_LIMIT_ENABLE; - config.Voltage.PeakForwardVoltage = PEAK_FORDWARD_VOLTAGE; + config.Voltage.PeakForwardVoltage = PEAK_FORWARD_VOLTAGE; config.Voltage.PeakReverseVoltage = PEAK_REVERSE_VOLTAGE; + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; + config.Slot0.kS = KS; + config.Slot0.kV = KV; + config.Slot0.kA = KA; + config.Slot0.kP = KP; + config.Slot0.kI = KI; + config.Slot0.kD = KD; + config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; + config.MotorOutput.Inverted = MOTOR_ROTATION_DIRECTION; + config.MotorOutput.NeutralMode = MOTOR_MODE; + return config; } - - public double STATOR_CURRENT_LIMIT = 60.0; - public boolean STATOR_CURRENT_LIMIT_ENABLE = true; - public double SUPPLY_CURRENT_LIMIT = 40.0; - public boolean SUPPLY_CURRENT_LIMIT_ENABLE = true; - - public double PEAK_FORDWARD_VOLTAGE = 12.0; - public double PEAK_REVERSE_VOLTAGE = -12.0; } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManager.java b/src/main/java/com/team973/lib/util/SubsystemManager.java index 16ef22e..49fda2b 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManager.java +++ b/src/main/java/com/team973/lib/util/SubsystemManager.java @@ -1,14 +1,20 @@ package com.team973.lib.util; import com.team973.frc2025.Robot; +import com.team973.frc2025.RobotConfig; +import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; import com.team973.lib.devices.GreyPigeonIO; public abstract class SubsystemManager { private final Logger m_logger; + protected final RobotInfo m_robotInfo; + + private DriveController m_driveController; protected SubsystemManager(Logger logger) { m_logger = logger; + m_robotInfo = RobotConfig.get(); } protected Logger getLogger() { @@ -25,7 +31,19 @@ public static SubsystemManager init(Logger logger) { public abstract GreyPigeonIO getPigeon(); - public abstract DriveController getDriveController(); + protected abstract DriveController initDriveController(); + + public DriveController getDriveController() { + if (m_driveController == null) { + if (m_robotInfo.FEATURE_FLAGS.ENABLE_DRIVE) { + m_driveController = initDriveController(); + } else { + m_driveController = null; + } + } + + return m_driveController; + } public void log() {} } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java index 8dd2b0c..de67ed3 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java @@ -8,38 +8,34 @@ public class SubsystemManagerReal extends SubsystemManager { private final GreyPigeonIO m_pigeon; - private final DriveController m_driveController; public SubsystemManagerReal(Logger logger) { super(logger); m_pigeon = new GreyPigeon( - logger.subLogger("pigeon"), RobotInfo.DRIVE_INFO.PIGEON_ID, RobotInfo.CANIVORE_CANBUS); - - Logger driveLogger = logger.subLogger("drive", 0.05); - - m_driveController = - new DriveController( - logger, - new SwerveModule( - 0, RobotInfo.DRIVE_INFO.FRONT_LEFT_CONSTANTS, driveLogger.subLogger("swerve/mod0")), - new SwerveModule( - 1, - RobotInfo.DRIVE_INFO.FRONT_RIGHT_CONSTANTS, - driveLogger.subLogger("swerve/mod1")), - new SwerveModule( - 2, RobotInfo.DRIVE_INFO.BACK_LEFT_CONSTANTS, driveLogger.subLogger("swerve/mod2")), - new SwerveModule( - 3, RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, driveLogger.subLogger("swerve/mod3")), - m_pigeon); + logger.subLogger("pigeon"), + m_robotInfo.DRIVE_INFO.PIGEON_ID, + RobotInfo.CANIVORE_CANBUS); } public GreyPigeonIO getPigeon() { return m_pigeon; } - public DriveController getDriveController() { - return m_driveController; + public DriveController initDriveController() { + Logger driveLogger = getLogger().subLogger("drive"); + RobotInfo.DriveInfo driveInfo = m_robotInfo.DRIVE_INFO; + + return new DriveController( + driveLogger, + new SwerveModule( + 0, driveInfo.getFrontLeftConstants(), driveLogger.subLogger("swerve/mod0")), + new SwerveModule( + 1, driveInfo.getFrontRightConstants(), driveLogger.subLogger("swerve/mod1")), + new SwerveModule(2, driveInfo.getBackLeftConstants(), driveLogger.subLogger("swerve/mod2")), + new SwerveModule( + 3, driveInfo.getBackRightConstants(), driveLogger.subLogger("swerve/mod3")), + m_pigeon); } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java index 1d22df3..70df8c6 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java @@ -1,69 +1,88 @@ package com.team973.lib.util; +import static edu.wpi.first.units.Units.Inches; + import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; import com.team973.frc2025.subsystems.swerve.SwerveModuleSim; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.devices.GreyPigeonSim; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; public class SubsystemManagerSim extends SubsystemManager { private final SwerveDriveSimulation m_swerveDriveSimulation; private final GreyPigeonIO m_pigeon; - private final DriveController m_driveController; public SubsystemManagerSim(Logger logger) { super(logger); - m_swerveDriveSimulation = - new SwerveDriveSimulation( - RobotInfo.DRIVE_INFO.DRIVE_TRAIN_SIMULATION_CONFIG, - RobotInfo.DRIVE_INFO.SIM_STARTING_POSE); + DriveTrainSimulationConfig driveTrainSimConfig = + DriveTrainSimulationConfig.Default() + // Specify gyro type (for realistic gyro drifting and error simulation) + .withGyro(COTS.ofPigeon2()) + // Specify swerve module (for realistic swerve dynamics) + .withSwerveModule( + COTS.ofMark4( + DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 + DCMotor.getKrakenX60(1), // Steer motor is a Kraken + COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels + 3)) // L3 Gear ratio + // Configures the track length and track width (spacing between swerve modules) + .withTrackLengthTrackWidth(Inches.of(26), Inches.of(26)) + // Configures the bumper size (dimensions of the robot bumper) + .withBumperSize(Inches.of(34), Inches.of(34)); + + var simStartingPose = new Pose2d(7.18, 5.7, Rotation2d.fromDegrees(180)); + + m_swerveDriveSimulation = new SwerveDriveSimulation(driveTrainSimConfig, simStartingPose); SimulatedArena.getInstance().addDriveTrainSimulation(m_swerveDriveSimulation); m_pigeon = new GreyPigeonSim( logger.subLogger("pigeon"), - RobotInfo.DRIVE_INFO.PIGEON_ID, + m_robotInfo.DRIVE_INFO.PIGEON_ID, RobotInfo.CANIVORE_CANBUS, m_swerveDriveSimulation.getGyroSimulation()); - - Logger driveLogger = logger.subLogger("drive", 0.05); - - m_driveController = - new DriveController( - logger, - new SwerveModuleSim( - 0, - m_swerveDriveSimulation.getModules()[0], - RobotInfo.DRIVE_INFO.FRONT_LEFT_CONSTANTS, - driveLogger.subLogger("swerve/mod0")), - new SwerveModuleSim( - 1, - m_swerveDriveSimulation.getModules()[1], - RobotInfo.DRIVE_INFO.FRONT_RIGHT_CONSTANTS, - driveLogger.subLogger("swerve/mod1")), - new SwerveModuleSim( - 2, - m_swerveDriveSimulation.getModules()[2], - RobotInfo.DRIVE_INFO.BACK_LEFT_CONSTANTS, - driveLogger.subLogger("swerve/mod2")), - new SwerveModuleSim( - 3, - m_swerveDriveSimulation.getModules()[3], - RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, - driveLogger.subLogger("swerve/mod3")), - m_pigeon); } public GreyPigeonIO getPigeon() { return m_pigeon; } - public DriveController getDriveController() { - return m_driveController; + public DriveController initDriveController() { + Logger driveLogger = getLogger().subLogger("drive"); + RobotInfo.DriveInfo driveInfo = m_robotInfo.DRIVE_INFO; + + return new DriveController( + driveLogger, + new SwerveModuleSim( + 0, + m_swerveDriveSimulation.getModules()[0], + driveInfo.getFrontLeftConstants(), + driveLogger.subLogger("swerve/mod0")), + new SwerveModuleSim( + 1, + m_swerveDriveSimulation.getModules()[1], + driveInfo.getFrontRightConstants(), + driveLogger.subLogger("swerve/mod1")), + new SwerveModuleSim( + 2, + m_swerveDriveSimulation.getModules()[2], + driveInfo.getBackLeftConstants(), + driveLogger.subLogger("swerve/mod2")), + new SwerveModuleSim( + 3, + m_swerveDriveSimulation.getModules()[3], + driveInfo.getBackRightConstants(), + driveLogger.subLogger("swerve/mod3")), + m_pigeon); } @Override diff --git a/src/main/java/com/team973/lib/util/SwerveModuleConfig.java b/src/main/java/com/team973/lib/util/SwerveModuleConfig.java index e97a818..c9b83f4 100644 --- a/src/main/java/com/team973/lib/util/SwerveModuleConfig.java +++ b/src/main/java/com/team973/lib/util/SwerveModuleConfig.java @@ -1,16 +1,28 @@ package com.team973.lib.util; +import com.team973.lib.devices.GreyTalonFX; + public class SwerveModuleConfig { public final int driveMotorID; public final int angleMotorID; public final int cancoderID; public final double angleOffset; + public final GreyTalonFX.Config driveMotorConfig; + public final GreyTalonFX.Config angleMotorConfig; + public SwerveModuleConfig( - int driveMotorID, int angleMotorID, int cancoderID, double angleOffset) { + int driveMotorID, + int angleMotorID, + int cancoderID, + double angleOffset, + GreyTalonFX.Config driveMotorConfig, + GreyTalonFX.Config angleMotorConfig) { this.driveMotorID = driveMotorID; this.angleMotorID = angleMotorID; this.cancoderID = cancoderID; this.angleOffset = angleOffset; + this.driveMotorConfig = driveMotorConfig; + this.angleMotorConfig = angleMotorConfig; } } diff --git a/src/main/java/com/team973/lib/util/SwerveSimUtil.java b/src/main/java/com/team973/lib/util/SwerveSimUtil.java index aa37a8c..d9278ca 100644 --- a/src/main/java/com/team973/lib/util/SwerveSimUtil.java +++ b/src/main/java/com/team973/lib/util/SwerveSimUtil.java @@ -2,6 +2,12 @@ public class SwerveSimUtil { public static SwerveModuleConfig sanitizeModuleConfig(SwerveModuleConfig config) { - return new SwerveModuleConfig(config.driveMotorID, config.angleMotorID, config.cancoderID, 0.0); + return new SwerveModuleConfig( + config.driveMotorID, + config.angleMotorID, + config.cancoderID, + 0.0, + config.driveMotorConfig, + config.angleMotorConfig); } }