We are now ready to PID tune the drive motors.

#### Drive motor characteristics

The first thing you need to do is determine the drive motor characteristics. The two important number that we need are the minimum power and the max speed. Do get these we will once again create a command that will drive the robot forward using a power that ramps from **0** to about **1.2**. The reason we go to **1.2** rather than the max of **1** is so that we can get a good measure of the maximum speed.

Create a new command **TestDriveRamp**. In the **initialize **function, set the steering position for all the motors to zero degrees so the robot will drive mostly straight. In the **execute **function log the power and the speed for all four of the drive motors. Then increment the power and set the new value. Then in the **isFinished **function return true when the power is greater than or equal to **1.2**.

When I do this, this is my result:

You need to glean two things from this graph. The first is the **minimum power** to get the motors to start. That value is the intersection of the linear portion of the graphs with the x axis. In this case you could estimate this from the graph to be about 0.3 for all of the motors. If you want a more precise answer, you can plot just the linear portions of the graphs and fit them to a line which will give you the intercept:

The second thing you need is the **maximum speed**. The max speed you set needs to be the slowest of the four motors. In my case it looks to be **2500**. Define these values as constants in your **DriveSubsystem **and set call the **setMinSpeed **functions (which you must create) for the four module’s **SwerveModule **instance:

1 2 3 4 5 6 7 8 9 10 11 12 13 |
private static final double k_frontLeftMinDrivePower = 0.29; private static final double k_backLeftMinDrivePower = 0.29; private static final double k_backRightMinDrivePower = 0.30; private static final double k_frontRightMinDrivePower = 0.30; private static final double k_maxDriveSpeed = 2500; ... m_frontLeft.setDriveMinPower(k_frontLeftMinDrivePower); m_backLeft.setDriveMinPower(k_backLeftMinDrivePower); m_backRight.setDriveMinPower(k_backRightMinDrivePower); m_frontRight.setDriveMinPower(k_frontRightMinDrivePower); |

Now you are ready to tune the **PID **values. You should create a **CalibrateDrive **command. Like you do for the **TestDriveRamp **command, in it’s **initialize** function you should set the angle for all of the wheels to **0** degrees so the robot will drive straight:

1 2 3 4 |
m_frontLeft.setSteeringPosition(0); m_backLeft.setSteeringPosition(0); m_backRight.setSteeringPosition(0); m_frontRight.setSteeringPosition(0); |

Next you can tune the **F**, **P **and **I** parameters (we generally don’t use the **D **parameter when tuning speed control). The method for doing this is similar to the one you used for the **Minibot**. First you set the **F **term (a good starting value would be **1.0/k_maxSpeed**). You then add a **P **term and increase it until the speed becomes unstable. A good starting point for the **P **term would be **0.0001**.

Finally you add the **I **term (along with an **IZone**) and increase that until the speed becomes unstable. A good starting point for the **I **term would be **0.0001**.

When I tuned my robot the following were my results:

I did my tuning at a speed of **2000 **and checked it at a speed of **1000**. My final numbers were as follows:

1 2 3 4 5 6 7 8 9 |
private static final double k_frontLeftDriveF = 1.05 / k_maxDriveSpeed; private static final double k_backLeftDriveF = 1.05 / k_maxDriveSpeed; private static final double k_backRightDriveF = 0.95 / k_maxDriveSpeed; private static final double k_frontRightDriveF = 0.95 / k_maxDriveSpeed; public static final double k_drivePTerm = 0.0010; public static final double k_driveITerm = 0.0004; public static final double k_driveIZone = 200; |

#### Calibrate drive encoder

Finally we need to compute the conversion factor from encoder units (**ticks**, **ticks/sec**) to real world units (**meters**, **meters/sec**). To do this create a command **CalibrateDistance**. Set up this command to drive the robot forward at a relative slow speed for a short distance and record the position of each of the drive wheels using the encoder’s getPosition function.

I did this for my robot and found that **1.345** meters equals **3579 **encoder ticks. This allows me to define the following constants in the **DriveSubsystem**:

1 2 3 |
private static final double k_maxDriveSpeed = 2500; private static final double k_ticksPerMeter = 3579 / 1.345; private static final double k_maxDriveSpeedMetersPerSecond = k_maxDriveSpeed / k_ticksPerMeter; // = 0.9395 m/s |

Which then allows me to create the following functions in the **SwerveModule**:

1 2 3 4 5 6 7 8 9 10 11 |
public void setDriveSpeedInMetersPerSecond(double speed) { setDriveSpeed(speed * DriveSubsystem.k_ticksPerMeter); } public double getDriveSpeedInMetersPerSecond() { return getDriveSpeed() / DriveSubsystem.k_ticksPerMeter; } public double getDrivePositionInMetersPerSecond() { return getDrivePosition() / DriveSubsystem.k_ticksPerMeter; } |