possible KiwiDrive example code for 4802

Kiwi.java

public class Kiwi
{
  SafePWM s1 = new SafePWM(0); // motor 1 pwm port 0
  SafePWM s2 = new SafePWM(1); // motor 2
  SafePWM s3 = new SafePWM(2); // motor 3
  
  private final double[][] inversion = { {2/3, 0, 1/3}, {-1/3, 1/sqrt(3), 1/3}, {-1/3, -1/sqrt(3), 1/3}};
  
  public Kiwi()
  {
  }
  
  /**
   * @input double x the x position of the vector joystick (-1 to 1)
   * @input double y the y position of the vector joystick (-1 to 1)
   * @input double p how fast to pivot (-1 to 1)
   */
  public void drive(double x, double y, double p)
  {
    double[] speeds;
    speeds[0] = (x*inversion[0][0] + y*inversion[0][1] + p*inversion[0][2]);
    speeds[1] = (x*inversion[1][0] + y*inversion[1][1] + p*inversion[1][2]);
    speeds[2] = (x*inversion[2][0] + y*inversion[2][1] + p*inversion[2][2]);
    
    s1.set(speeds[0]);
    s2.set(speeds[1]);
    s3.set(speeds[2]);
  }
}