Part 2. Driving the Motors

2.1 - Writing the WheelDrive Class

Now that we understand how swerve drive works and we have a framework for converting joystick input into swerve relative values, we can begin the process of using those values to operate the actual swerve modules. This is where some of the code becomes module dependent, so this book will do the best job possible of explaining the possible changes one would have to make.

It is also important to note that the book assumes the swerve modules will be operated alongside an angle encoder. This will involve setting up a PIDController and converting the angle values into voltage ranges.

In your IDE, add a new class to your project and call it WheelDrive. This class will be instantiated four times for each of the wheels on the drivetrain. This is where the angle and speed values generated from the SwerveDrive class will go.

The SwerveDrive class needs to represent the swerve modules that are on the real robot. This will consist of two motors for angle and speed (can be Jaguars or Talons, we will use Jaguars) as well as an angle encoder that we will use to setup the PIDController. Start by making private instance variables for the speedMotor, angleMotor, and pidController.

private Jaguar angleMotor;
private Jaguar speedMotor;
private PIDController pidController;

We will set these up in the constructor, taking in two integers for the motor ports, one integer for the encoder port, and using default values for the PIDController. We also need to write the relevant initialization code for the PIDController.

public WheelDrive (int angleMotor, int speedMotor, int encoder) {
    this.angleMotor = new Jaguar (angleMotor);
    this.speedMotor = new Jaguar (speedMotor);
    pidController = new PIDController (1, 0, 0, new AnalogInput (encoder), this.angleMotor);

    pidController.setOutputRange (-1, 1);
    pidController.setContinuous ();
    pidController.enable ();
}

We also need to set a constant for the max voltage the swerve modules take. For the AndyMark swerve modules we purchased, this was around 4.95 volts.

private final double MAX_VOLTS = 4.95;

Next we need to write the drive method that the SwerveDrive class calls. This will take in a double for the wheel speed and a double for the wheel angle.

public void drive (double speed, double angle) {

}

The first step in writing the drive method is to set the speed motor to the value of the speed parameter, running the speed motor.

public void drive (double speed, double angle) {
    speedMotor.set (speed);

}

From there we can convert the angle into a value between 0 and MAX_VOLTS. When we do this, we need to 'wrap' the setpoint around 0 and MAX_VOLTS.

public void drive (double speed, double angle) {
    speedMotor.set (speed);

    double setpoint = angle * (MAX_VOLTS * 0.5) + (MAX_VOLTS * 0.5); // Optimization offset can be calculated here.
    if (setpoint < 0) {
        setpoint = MAX_VOLTS + setpoint;
    }
    if (setpoint > MAX_VOLTS) {
        setpoint = setpoint - MAX_VOLTS;
    }

    pidController.setSetpoint (setpoint);
}

When we call pidController.setSetpoint, we are telling the angle motor to adjust accurately to the setpoint we calculated using MAX_VOLTS.

2.2 - Using the WheelDrive class

Now that we have created WheelDrive, it is time to use it! In the main project file (usually robot.java), add the following lines to create instance variables.

private WheelDrive backRight = new WheelDrive (0, 1, 0);
private WheelDrive backLeft = new WheelDrive (2, 3, 1);
private WheelDrive frontRight = new WheelDrive (4, 5, 2);
private WheelDrive frontLeft = new WheelDrive (6, 7, 3);

private SwerveDrive swerveDrive = new SwerveDrive (backRight, backLeft, frontRight, frontLeft);

Two things to take note of here. For one, the numbers being passed to the WheelDrive constructors are arbitrary, and should be the real IDs of the angle motor, speed motor, and angle encoder for each wheel on the robot. Also, you should be getting an error for the SwerveDrive instantiation as we have not written a constructor for the SwerveDrive class that takes four WheelDrive objects. Switch to SwerveDrive.java and we will do that now.

private WheelDrive backRight;
private WheelDrive backLeft;
private WheelDrive frontRight;
private WheelDrive frontLeft;

public SwerveDrive (WheelDrive backRight, WheelDrive backLeft, WheelDrive frontRight, WheelDrive frontLeft) {
    this.backRight = backRight;
    this.backLeft = backLeft;
    this.frontRight = frontRight;
    this.frontLeft = frontLeft;
}

Now that the SwerveDrive class is aware of the wheels, we can have the drive method call each wheel's drive method. At the bottom of the SwerveDrive.drive method, add the following lines.

    backRight.drive (backRightSpeed, backRightAngle);
    backLeft.drive (backLeftSpeed, backLeftAngle);
    frontRight.drive (frontRightSpeed, frontRightAngle);
    frontLeft.drive (frontLeftSpeed, frontLeftAngle);
}

The only thing left now to do is to call swerveDrive.drive from teleopPeriodic in Robot.java. Once this is done, the robot should swerve!

private Joystick joystick = new Joystick (0);

public void teleopPeriodic () {
    swerveDrive.drive (joystick.getRawAxis (1), joystick.getRawAxis (0), joystick.getRawAxis (4));
}