Swerve Article

Docs/3_Specifics/3.3_Swerve.md

@@ -0,0 +1,145 @@
+# Swerve
+
+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
+
+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 [Odometry Thread](#odometry-thread)). I'll talk about the `IOSim` implementation in [Maple Sim](#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 *in what way* 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.
+
+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 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 [Odometry Thread](#odometry-thread)
+
+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++) {
+    // 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);
+}
+```
+
+## 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. 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
+
+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.