# Swerve - hammerheads5000/Team-Documentation GitHub Wiki

Swerve is a type of drivetrain where all wheels can rotate and move independently of one another. It allows moving the robot in any direction with any angular velocity while maintaining power and speed. This won't go in depth into the math and algorithms behind swerve drives, only how to implement them in code. To learn more about the dynamics, see this blog post.

# Characterization

First, you need to characterize the swerve drive. This means measuring the dimensions, gear ratios, etc., and setting PID constants. There are a lot of constants, so seeing them all at once can be overwhelming. For that reason, we will break it down into more manageable chunks.

## Dimensions

Measure the distance between the centers of swerve modules, as well as the wheel radius, and then make constants for them as such:

```private static final Measure<Distance> swerveWidth = Inches.of(24); // width between centers of swerve modules
// from left to right
private static final Measure<Distance> swerveLength = Inches.of(24); // length between centers of swerve modules
// from front to back

private static final Measure<Distance> wheelRadius = Inches.of(1.97);```

## Gear Ratios

Next, enter the gear ratios you are using (if you are unsure, check the page you ordered from, it should have them).

```private static final double driveMotorGearRatio = (8.14 / 1.0); // 8.14:1
private static final double steerMotorGearRatio = (150.0 / 7 / 1.0); // 150/7:1```

Also, while it is not strictly necessary, the coupling gear ratio can help the swerve drive be as accurate as possible. This is the ratio between the number of rotations of the drive motor to the number of azimuthal (steering) rotations of the wheel, typically the inverse of the first-stage gear ratio.

`private static final double couplingGearRatio = 50.0 / 14.0;`

## Slip Current

Slip current is the current at which the wheel will slip on the floor. We typically just use a value of ~400 A, because it can be hard to precisely measure.

`private static final Measure<Current> slipCurrent = Amps.of(400);`

## Gains

These characterize the behavior of the motors in moving the wheels correctly. They require quite a bit of tuning. Generally, for the steering, set the kV (output per unit velocity) by measuring it for best results, then increase kP until the wheels oscilate, then increase kD to stop the oscilations and kI to remove any residual error. Also, set the kS (output to overcome static friction) through testing as well. The drive gains require less tuning, with mainly just the kV and a relatively low kP.

Here are the gains for our Crescendo bot:

```private static final Slot0Configs steerMotorGains = new Slot0Configs()
.withKP(200.0) // output per unit error in position (rotations)
.withKI(50.0) // output per unit integrated error (rotations*s)
.withKD(50.0) // output per unit of error derivative (rps)
.withKS(10.0) // output to overcome static friction
.withKV(2.5); // output per unit of velocity (rps)
private static final Slot0Configs driveMotorGains = new Slot0Configs()
.withKP(1.0) // output per unit error in position (rps)
.withKI(0.0) // output per unit integrated error (rotations)
.withKD(0.0) // output per unit of error derivative (rps/s)
.withKS(0) // output to overcome static friction
.withKV(0.123) // output per unit of velocity (rps)
.withKA(0); // output per unit of acceleration (rps/s)```

## Output type

This determines how the motors are controlled. We use TorqueCurrentFOC, which controllers the current to the motors:

```private static final ClosedLoopOutputType steerMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC;
private static final ClosedLoopOutputType driveMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC;```