From 9bea7e66fcbb422f9e8da0151c826c29ae9e69bf Mon Sep 17 00:00:00 2001 From: Hyperoperation Date: Mon, 8 Dec 2025 15:32:49 -0500 Subject: [PATCH] Add initial implementation of Swerve Drive system and configuration files --- .../swerve/modules/controllerproperties.json | 8 ++ .../swerve/modules/physicalproperties.json | 16 +++ .../deploy/swerve/modules/pidfproperties.json | 16 +++ .../deploy/swerve/modules/swervedrive.json | 14 ++ src/main/deploy/swerve/modules/x.json | 27 ++++ .../frc/robot/subsystems/SwerveDrive.java | 87 ++++++++++++ .../frc/robot/subsystems/SwerveModule.java | 127 ++++++++++++++++++ 7 files changed, 295 insertions(+) create mode 100644 src/main/deploy/swerve/modules/controllerproperties.json create mode 100644 src/main/deploy/swerve/modules/physicalproperties.json create mode 100644 src/main/deploy/swerve/modules/pidfproperties.json create mode 100644 src/main/deploy/swerve/modules/swervedrive.json create mode 100644 src/main/deploy/swerve/modules/x.json create mode 100644 src/main/java/frc/robot/subsystems/SwerveDrive.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java diff --git a/src/main/deploy/swerve/modules/controllerproperties.json b/src/main/deploy/swerve/modules/controllerproperties.json new file mode 100644 index 0000000..d6d2856 --- /dev/null +++ b/src/main/deploy/swerve/modules/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0.01 + } + } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 0000000..f42dd45 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "conversionFactors": { + "angle": {"gearRatio": 28.125}, + "drive": {"diameter": 4, "gearRatio": 6.75} + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "wheelGripCoefficientOfFriction": 1.19, + "optimalVoltage": 12 + } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 0000000..1e00204 --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.01, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } + } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/swervedrive.json b/src/main/deploy/swerve/modules/swervedrive.json new file mode 100644 index 0000000..bc043f1 --- /dev/null +++ b/src/main/deploy/swerve/modules/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "pigeon2", + "id": 13, + "canbus": "canivore" + }, + "invertedIMU": true, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] + } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/x.json b/src/main/deploy/swerve/modules/x.json new file mode 100644 index 0000000..ce6e4bd --- /dev/null +++ b/src/main/deploy/swerve/modules/x.json @@ -0,0 +1,27 @@ +{ + "drive": { + "type": "sparkmax", + "id": 2, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 1, + "canbus": null + }, + "encoder": { + "type": "cancoder", + "id": 10, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderInverted": false, + "absoluteEncoderOffset": -50.977, + "location": { + "front": 12, + "left": -12 + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..7e9b824 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -0,0 +1,87 @@ +// Import relevant classes. +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + + +// Example SwerveDrive class +public class SwerveDrive extends SubsystemBase +{ + + // Attributes + SwerveDriveKinematics kinematics; + SwerveDriveOdometry odometry; + Gyroscope gyro; // Psuedo-class representing a gyroscope. + SwerveModule[] swerveModules; // Psuedo-class representing swerve modules. + + // Constructor + public SwerveDrive() + { + + swerveModules = new SwerveModule[4]; // Psuedo-code; Create swerve modules here. + + // Create SwerveDriveKinematics object + // 12.5in from center of robot to center of wheel. + // 12.5in is converted to meters to work with object. + // Translation2d(x,y) == Translation2d(front, left) + kinematics = new SwerveDriveKinematics( + new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(12.5)), // Front Left + new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(-12.5)), // Front Right + new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(12.5)), // Back Left + new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(-12.5)) // Back Right + ); + + gyro = new Gyroscope(); // Psuedo-constructor for generating gyroscope. + + // Create the SwerveDriveOdometry given the current angle, the robot is at x=0, r=0, and heading=0 + odometry = new SwerveDriveOdometry( + kinematics, + gyro.getAngle(), // returns current gyro reading as a Rotation2d + new SwerveModulePosition[]{new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition}, + // Front-Left, Front-Right, Back-Left, Back-Right + new Pose2d(0,0,new Rotation2d()) // x=0, y=0, heading=0 + ); + + } + + // Simple drive function + public void drive() + { + // Create test ChassisSpeeds going X = 14in, Y=4in, and spins at 30deg per second. + ChassisSpeeds testSpeeds = new ChassisSpeeds(Units.inchesToMeters(14), Units.inchesToMeters(4), Units.degreesToRadians(30)); + + // Get the SwerveModuleStates for each module given the desired speeds. + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(testSpeeds); + // Output order is Front-Left, Front-Right, Back-Left, Back-Right + + swerveModules[0].setState(swerveModuleStates[0]); + swerveModules[1].setState(swerveModuleStates[1]); + swerveModules[2].setState(swerveModuleStates[2]); + swerveModules[3].setState(swerveModuleStates[3]); + } + + // Fetch the current swerve module positions. + public SwerveModulePosition[] getCurrentSwerveModulePositions() + { + return new SwerveModulePosition[]{ + new SwerveModulePosition(swerveModules[0].getDistance(), swerveModules[0].getAngle()), // Front-Left + new SwerveModulePosition(swerveModules[1].getDistance(), swerveModules[1].getAngle()), // Front-Right + new SwerveModulePosition(swerveModules[2].getDistance(), swerveModules[2].getAngle()), // Back-Left + new SwerveModulePosition(swerveModules[3].getDistance(), swerveModules[3].getAngle()) // Back-Right + }; + } + + @Override + public void periodic() + { + // Update the odometry every run. + odometry.update(gyro.getAngle(), getCurrentSwerveModulePositions()); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..e777a52 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,127 @@ +import com.revrobotics.CANSparkMax; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import edu.wpi.first.math.util.Units; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; + + +public class SwerveModule { + + private CANSparkMax driveMotor; + private CANSparkMax steerMotor; + private CANcoder absoluteEncoder; + private SparkMaxPIDController drivingPIDController; + private SparkMaxPIDController turningPIDController; + private RelativeEncoder driveEncoder; + private RelativeEncoder steerEncoder; + + public SwerveModule(int driveMotorCANID, int steerMotorCANID, int cancoderCANID) + { + driveMotor = new CANSparkMax(driveMotorCANID); + steerMotor = new CANSparkMax(steerMotorCANID); + absoluteEncoder = new CANcoder(cancoderCANID); + + // Get the PID Controllers + drivingPIDController = driveMotor.getPIDController(); + turningPIDController = steerMotor.getPIDController(); + + // Get the encoders + driveEncoder = driveMotor.getEncoder(): + steerEncoder = steerMotor.getEncoder(); + + // Reset everything to factory default + driveMotor.restoreFactoryDefaults(); + steerMotor.restoreFactoryDefaults(); + absoluteEncoder.getConfigurator().apply(new CANcoderConfiguration()); + + // Continue configuration here.. + + // CANcoder Configuration + CANcoderConfigurator cfg = encoder.getConfigurator(); + cfg.apply(new CANcoderConfiguration()); + MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); + cfg.refresh(magnetSensorConfiguration); + cfg.apply(magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)); + + // Steering Motor Configuration + steerMotor.setInverted(false); + turningPIDController.setFeedbackDevice(steerEncoder); + // Apply position and velocity conversion factors for the turning encoder. We + // want these in radians and radians per second to use with WPILib's swerve + // APIs. + steerEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor); + steerEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor); + // Enable PID wrap around for the turning motor. This will allow the PID + // controller to go through 0 to get to the setpoint i.e. going from 350 degrees + // to 10 degrees will go through 0 rather than the other direction which is a + // longer route. + turningPIDController.setPositionPIDWrappingEnabled(true); + turningPIDController.setPositionPIDWrappingMinInput(0); + turningPIDController.setPositionPIDWrappingMaxInput(90); + // Set the PID gains for the turning motor. Note these are example gains, and you + // may need to tune them for your own robot! + turningPIDController.setP(ModuleConstants.kTurningP); + turningPIDController.setI(ModuleConstants.kTurningI); + turningPIDController.setD(ModuleConstants.kTurningD); + turningPIDController.setFF(ModuleConstants.kTurningFF); + + // Drive Motor Configuration + driveMotor.setInverted(false); + drivingPIDController.setFeedbackDevice(driveEncoder); + // Apply position and velocity conversion factors for the driving encoder. The + // native units for position and velocity are rotations and RPM, respectively, + // but we want meters and meters per second to use with WPILib's swerve APIs. + driveEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor); + driveEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor); + // Set the PID gains for the driving motor. Note these are example gains, and you + // may need to tune them for your own robot! + drivingPIDController.setP(ModuleConstants.kDrivingP); + drivingPIDController.setI(ModuleConstants.kDrivingI); + drivingPIDController.setD(ModuleConstants.kDrivingD); + drivingPIDController.setFF(ModuleConstants.kDrivingFF); + + // Save the SPARK MAX configurations. If a SPARK MAX browns out during + // operation, it will maintain the above configurations. + driveMotor.burnFlash(); + steerMotor.burnFlash(); + + driveEncoder.setPosition(0); + steerEncoder.setPosition(encoder.getAbsolutePosition().refresh().getValue() * 360); + } + + + /** + Get the distance in meters. + */ + public double getDistance() + { + return driveEncoder.getPosition(); + } + + /** + Get the angle. + */ + public Rotation2d getAngle() + { + return Rotation2d.fromDegrees(steerEncoder.getPosition()); + } + + /** + Set the swerve module state. + @param state The swerve module state to set. + */ + public void setState(SwerveModuleState state) + { + turningPIDController.setReference(state.angle.getDegrees(), ControlType.kPosition); + drivingPIDController.setReference(state.speedMetersPerSecond, ControlType.kVelocity); + } + +} \ No newline at end of file