From 2b2433bd2da825f6b53b4f43296b3d62420cbf4f Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 4 Dec 2025 18:17:54 -0800 Subject: [PATCH 1/7] Some rough draft --- Docs/3_Specifics/3.3_Swerve.md | 134 +++++++++++++++++++++++++++++++++ 1 file changed, 134 insertions(+) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index e69de29..807b91a 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -0,0 +1,134 @@ +# Swerve + +Swerve drives are high-performace, omnidirectional drivetrains. TODO: ADD MORE HERE + +## Layout of a Swerve Module + +On our robot (and most swerve drivetrains), each module consists of a **drive motor** and a **steer (or turn) motor**. The drive motor runs the wheel, moving the robot, while the steer motor rotates the wheel, directing its force in any direction. Multiple modules combine move the robot in any translational direction while rotatating. + +### The Module IO Interface + +As with any mechanism, the IO class is the lowest level of abstraction, interfacing directly with the motors. Our `ModuleIO` interface has methods that set the turn motor position, and the drive motor voltage and velocity. +```java +//---SNIP--- +public interface ModuleIO { + + @AutoLog + public static class ModuleIOInputs { + //---SNIP--- + } + + public void updateInputs(ModuleIOInputs inputs); + + // This method is used with open-loop control + public void setDriveVoltage(double volts, boolean withFoc); + + public default void setDriveVoltage(double volts) { + setDriveVoltage(volts, true); + } + + // This method is used with closed-loop control + public void setDriveVelocitySetpoint(double setpointMetersPerSecond); + + // This sets the turn motors setpoint + public void setTurnPositionSetpoint(Rotation2d setpoint); +} +``` +The `IOReal` implementation of this interface is fairly standard (except for one key difference which I'll talk about in TODO: INSERT SECTION ON ODOMETRY THREAD). I'll talk about the `IOSim` implementation in TODO: INSERT SECTION ON MAPLE SIM + +### The Module Class + +The `Module` class, in `Module.java` is one layer of abstraction above `ModuleIO`. Its job is to tell the IO *how* it should move the motors. In addition to some helpers, it has 3 main methods: + +1. `runVoltageSetpoint()` +```java +private void runVoltageSetpoint(double volts, Rotation2d targetAngle, boolean focEnabled) { + io.setTurnPositionSetpoint(targetAngle); + io.setDriveVoltage(volts * Math.cos(targetAngle.minus(inputs.turnPosition).getRadians()), focEnabled); +} +``` +This internal method runs the drive motor to the specified voltage, and the turn motor to the specified angle. +Note that the requested voltage is scaled by the cosine of the *error* of the turn angle, which avoids moving the drive motor too much when the turn is out of position (if its off by 0 radians, it will scale by 1, π/2 rad (90 deg), it wil scale by 0). + +2. `runOpenLoop()` +```java +public SwerveModuleState runOpenLoop(SwerveModuleState state, boolean focEnabled) { + state.optimize(getAngle()); + + double volts = + state.speedMetersPerSecond * 12 / SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(); + + runVoltageSetpoint(volts, state.angle, focEnabled); + + return state; +} +``` +This method runs the drive motor using feedforward velocity control. It's used for teleop driving, where velocity accuracy isn't required (the human driving the robot will correct for error). +> Note: `SwerveModuleState` stores an angle and velocity for a swerve module. + +```java +state.optimize(getAngle()) +``` +This line reduces the distance the turn motor has to move to reach the setpoint, because driving at 0° at x velocity is has the same vector as driving at 180° at -x velocity. + +```java +double volts = + state.speedMetersPerSecond * 12 / SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(); +``` +This line converts the requested speed to a voltage. It calculates the *proportion* of the maximum velocity, and requestes the same *proportion* of the maximum voltage (in this case 12). So if the maximum velocity is 10 m/s, and the requested velocity is 5 m/s, it would send 6V to the drive motor (5 m/s is half of 10 m/s, and 6V is half of 12V). + +The method returns the optimized state for logging. + +3. `runClosedLoop()` +```java +public SwerveModuleState runClosedLoop(SwerveModuleState state) { + state.optimize(getAngle()); + + io.setTurnPositionSetpoint(state.angle); + io.setDriveVelocitySetpoint(state.speedMetersPerSecond); + + return state; +} +``` +Unlike the previous two methods, this one runs the module using *feedback* velocity control, giving better accuracy to autonomous actions. We use this method for autonomous driving and autoaim. + +## Helpful WPIlib Classes + +- `SwerveModuleState`: This class stores the angle and velocity of one swerve wheel +- `SwerveModulePosition`: Similar to `SwerveModuleState`, but represents the distance travelled (position) by the module, not its velocity +- `ChassisSpeeds`: This class stores represents the velocity of a drivetrain. Internally it stores the linear velocity as x and y components, and the rotational velocity (aka omega). It also has helper methods to convert between robot relative and field relative velocities. +- `SwerveDriveKinematics`: This class uses information about the robot's swerve drive to calculate the `SwerveModuleStates` required to drive at the requested `ChassisSpeeds`. +- `SwerveDrivePoseEstimator`: This class uses odometry from the swerve modules, the gyro, and vision estimates, to estimate the position of the robot on the field. + +## The drive() method + +The drive method is the method that drives the robot (woah really). + +At a high level, all the drive method needs to do is: +1. Calculate the `SwerveModuleStates` required to drive at the requested `ChassisSpeeds` (this is what `SwerveDriveKinematics` is for). +2. Drive each module at its `SwerveModuleState`. + +In practice, it does a few extra things like logging, and has some performance enhancements. If you're interested, the full method is in `SwerveSubsystem.java`. + +## The updateOdomerty() method + +The `updateOdometry()` method is called periodically and updates the pose estimation with the position measured by the swerve modules and gyro. +> Note: The updateOdomerty() implementation I'll talk about here is *not* the same as the one in the actual codebase. I'll talk about the actual implentation along with TODO: LINK TO PHOENIX ODO THREAD SECTION + +The most basic implementation looks something like this. +```java +private void updateOdometrySimple() { + // We will store the measured module positions to this array before sending them to the estimator + SwerveModulePosition[] swerveModulePositions = new SwerveModulePositions[modules.length]; + + // Loop through all the modules + for (int i = 0; i < modules.length; i++) { + // Set store each module position in the swerveModulePositions array + swerveModulePositions[i] = modules[i].getPosition(); + } + + // This line actually updates the estimator with the measured positions and gyro rotation + estimator.update(gyroRotation, swerveModulePositions); +} +``` + From 7ef1ac720f3336b902e353466a8b54bef8d60e05 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 5 Dec 2025 17:41:03 -0800 Subject: [PATCH 2/7] Fix typo, add initial blurb on maple sim --- Docs/3_Specifics/3.3_Swerve.md | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 807b91a..47c3d2f 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -38,7 +38,7 @@ The `IOReal` implementation of this interface is fairly standard (except for one ### The Module Class -The `Module` class, in `Module.java` is one layer of abstraction above `ModuleIO`. Its job is to tell the IO *how* it should move the motors. In addition to some helpers, it has 3 main methods: +The `Module` class, in `Module.java` is one layer of abstraction above `ModuleIO`. Its job is to tell the IO *in what way* it should move the motors. In addition to some helpers, it has 3 main methods: 1. `runVoltageSetpoint()` ```java @@ -102,7 +102,7 @@ Unlike the previous two methods, this one runs the module using *feedback* veloc ## The drive() method -The drive method is the method that drives the robot (woah really). +The drive method is the method that drives the robot. At a high level, all the drive method needs to do is: 1. Calculate the `SwerveModuleStates` required to drive at the requested `ChassisSpeeds` (this is what `SwerveDriveKinematics` is for). @@ -110,7 +110,7 @@ At a high level, all the drive method needs to do is: In practice, it does a few extra things like logging, and has some performance enhancements. If you're interested, the full method is in `SwerveSubsystem.java`. -## The updateOdomerty() method +## The updateOdometry() method The `updateOdometry()` method is called periodically and updates the pose estimation with the position measured by the swerve modules and gyro. > Note: The updateOdomerty() implementation I'll talk about here is *not* the same as the one in the actual codebase. I'll talk about the actual implentation along with TODO: LINK TO PHOENIX ODO THREAD SECTION @@ -132,3 +132,7 @@ private void updateOdometrySimple() { } ``` +## Maple Sim + +Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. + From 3baef87092ca5ca1c40632fad9c1f0634d3768dc Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 9 Dec 2025 17:58:07 -0800 Subject: [PATCH 3/7] Odo thread blurb --- Docs/3_Specifics/3.3_Swerve.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 47c3d2f..5ae5f47 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -136,3 +136,10 @@ private void updateOdometrySimple() { Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. +## Odometry Thread + +The main robot loop, which runs the command scheduler and all the periodic methods, runs at 50 Hz. For that reason, we update almost all of our CTRE status signals at 50 Hz. However, with liscensed devices, we can actually poll the devices much faster. For most applications, this isn't necessary, however for odometry, more snapshots gives a more accurate estimation. To poll the drivetrain faster than the main loop, we use a **thread** (called in code `PhoenixOdometryThread`). In computing, a thread is like a piece of code which runs in parrallel to another. This thread stores all of the CTRE StatusSignals in a **record** called `RegisteredSignal` (a record is a type that stores multiple closely linked data in a single Object). The thread has a loop that polls each StatusSignal at 150 Hz, storing the odometry in another record called `Samples` (which stores all of the data from the motors at a timestamp). The Samples are added to a locked journal, which is read by the main thread in `updateOdometry()`. + +### Actual updateOdometry() implementation + +The actual updateOdometry() method gets all the unread samples from the odometry thread's journal, and loops through them, updating the estimator with the Samples's timestamp. Additionally, the updateOdometry() method has some additional null safety, inferring some information if they aren't present in a sample, for example judging rotation based on wheel odometry when the gyro has no data. From dfe6300e30a17b80cddcd31dbeec6aaf0900dbef Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 11 Dec 2025 21:25:16 -0800 Subject: [PATCH 4/7] Add maple sim docs link --- Docs/3_Specifics/3.3_Swerve.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 5ae5f47..15ec934 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -134,7 +134,7 @@ private void updateOdometrySimple() { ## Maple Sim -Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. +Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. I won't talk much about implementation here because the [MapleSim](https://shenzhen-robotics-alliance.github.io/maple-sim/) docs are good. ## Odometry Thread From 2b40ec226f38d01a537947f22c4fa564d7398a4f Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 11 Dec 2025 21:28:51 -0800 Subject: [PATCH 5/7] Add links between sections --- Docs/3_Specifics/3.3_Swerve.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 15ec934..2927efe 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -34,7 +34,7 @@ public interface ModuleIO { public void setTurnPositionSetpoint(Rotation2d setpoint); } ``` -The `IOReal` implementation of this interface is fairly standard (except for one key difference which I'll talk about in TODO: INSERT SECTION ON ODOMETRY THREAD). I'll talk about the `IOSim` implementation in TODO: INSERT SECTION ON MAPLE SIM +The `IOReal` implementation of this interface is fairly standard (except for one key difference which I'll talk about in [Odometry Thread](#odometry-thread)). I'll talk about the `IOSim` implementation in [Maple Sim](#maple-sim) ### The Module Class @@ -113,7 +113,7 @@ In practice, it does a few extra things like logging, and has some performance e ## The updateOdometry() method The `updateOdometry()` method is called periodically and updates the pose estimation with the position measured by the swerve modules and gyro. -> Note: The updateOdomerty() implementation I'll talk about here is *not* the same as the one in the actual codebase. I'll talk about the actual implentation along with TODO: LINK TO PHOENIX ODO THREAD SECTION +> Note: The updateOdomerty() implementation I'll talk about here is *not* the same as the one in the actual codebase. I'll talk about the actual implentation along with [Odometry Thread](#odometry-thread) The most basic implementation looks something like this. ```java @@ -134,7 +134,7 @@ private void updateOdometrySimple() { ## Maple Sim -Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. I won't talk much about implementation here because the [MapleSim](https://shenzhen-robotics-alliance.github.io/maple-sim/) docs are good. +Maple sim is a drivetrain, field, and gamepiece physics sim. It includes collision, skidding, and friction in its calculations making it more accurate that typical simulations. I won't talk much about implementation here because the [MapleSim](https://shenzhen-robotics-alliance.github.io/maple-sim/) docs are good. However, I think it's important to note that the `ModuleIOSim` uses the CTRE simulation instead of the WPIlib DC motor sim, so the sim code is very similar to the real code. ## Odometry Thread From e008748fb5c1306c3d20bc98368d144b98eeac65 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 11 Dec 2025 21:37:03 -0800 Subject: [PATCH 6/7] More intro --- Docs/3_Specifics/3.3_Swerve.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 2927efe..15942f2 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -1,6 +1,6 @@ # Swerve -Swerve drives are high-performace, omnidirectional drivetrains. TODO: ADD MORE HERE +Swerve drives are high-performace, omnidirectional drivetrains. Unlike tank drives or West Coast drives, swerves can move in any translational or rotational direction, and can translate and rotate simultaniously (somewhat). Therefore, a swerve drivebases are highly versitile, however they are more difficult to program and build than other drivetrain types. ## Layout of a Swerve Module From 93605f8c5ab5b1df02c227dae8cf981a66914fea Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 11 Dec 2025 21:40:28 -0800 Subject: [PATCH 7/7] fix typo --- Docs/3_Specifics/3.3_Swerve.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/3_Specifics/3.3_Swerve.md b/Docs/3_Specifics/3.3_Swerve.md index 15942f2..dea75a9 100644 --- a/Docs/3_Specifics/3.3_Swerve.md +++ b/Docs/3_Specifics/3.3_Swerve.md @@ -123,7 +123,7 @@ private void updateOdometrySimple() { // Loop through all the modules for (int i = 0; i < modules.length; i++) { - // Set store each module position in the swerveModulePositions array + // Store each module position in the swerveModulePositions array swerveModulePositions[i] = modules[i].getPosition(); }