Part 1. Using Input from the Joysticks

The first step in the programming process is to convert input from the two joysticks into a set of four wheel speeds and four wheel angles. These speeds and angles will be passed into a controller object for each wheel that we will design later.

This mathematical transformation requires the distance between each wheel axle on the length and width. Measure these and write them down.

Create a class called SwerveDrive, do not worry about writing constructors or data members for right now. The method that we will first implement is called drive, which should accept x1, y1, and x2 parameters of type double. The first two parameters are the x and y axis from the strafing joystick and the last parameter is the x axis from the rotation joystick.

public void drive (double x1, double y1, double x2) {

}

The next step is to create constants for the width and length recorded earlier. Just above the method declaration of drive add the following two lines of code.

public final double L = LENGTH_YOU_WROTE;
public final double W = WIDTH_YOU_WROTE;

Now we can start with the math for swerve in the drive method. Declare double r to be equal to Math.sqrt ((L * L) + (W * W)). Also set y1 equal to y1 * -1. Using these numbers we can compute variables a, b, c, and d as follows.

public void drive (double x1, double y1, double x2) {
    double r = Math.sqrt ((L * L) + (W * W));
    y1 *= -1;

    double a = x1 - x2 * (L / r);
    double b = x1 + x2 * (L / r);
    double c = y1 - x2 * (W / r);
    double d = y1 + x2 * (W / r);
}

Using these four variables the next step is to compute the speeds for each of the motors. It is important to note that these speeds are between 0 and 1, which may require necessary transformations depending on the speed range your motor requires.

public void drive (double x1, double y1, double x2) {
    double r = Math.sqrt ((L * L) + (W * W));
    y1 *= -1;

    double a = x1 - x2 * (L / r);
    double b = x1 + x2 * (L / r);
    double c = y1 - x2 * (W / r);
    double d = y1 + x2 * (W / r);


    double backRightSpeed = Math.sqrt ((a * a) + (d * d));
    double backLeftSpeed = Math.sqrt ((a * a) + (c * c));
    double frontRightSpeed = Math.sqrt ((b * b) + (d * d));
    double frontLeftSpeed = Math.sqrt ((b * b) + (c * c));
}

Once this is done, the wheel angles must also be computed for the four wheels. These will be in a range of -1 to 1, if you wish to turn this range into real degrees then simply multiply by 180.

public void drive (double x1, double y1, double x2) {
    double r = Math.sqrt ((L * L) + (W * W));
    y1 *= -1;

    double a = x1 - x2 * (L / r);
    double b = x1 + x2 * (L / r);
    double c = y1 - x2 * (W / r);
    double d = y1 + x2 * (W / r);

    double backRightSpeed = Math.sqrt ((a * a) + (d * d));
    double backLeftSpeed = Math.sqrt ((a * a) + (c * c));
    double frontRightSpeed = Math.sqrt ((b * b) + (d * d));
    double frontLeftSpeed = Math.sqrt ((b * b) + (c * c));

    double backRightAngle = Math.atan2 (a, d) / Math.pi;
    double backLeftAngle = Math.atan2 (a, c) / Math.pi;
    double frontRightAngle = Math.atan2 (b, d) / Math.pi;
    double frontLeftAngle = Math.atan2 (b, c) / Math.pi;
}

These eight doubles are now fit to be passed into the wheel class we will create in Part2.