Holonomic Drive System Issues, Robot Drives Diagonally...

[Omgsar]

Our robot is acting a bit wonky. We have a holonomic drive system made with the omni wheels, and mechanically everything seems sound. So, all that is left is the code. Here is our code:

  public void operatorControl() {

        System.out.println("operatorControl");
        initAll(); //init all variables
        while (true && isOperatorControl() && isEnabled()) {
            getReadings(); //Get the readings for the drive
            spikeTrigger(); //Get the readings for the actuator on the arm and map them, comment out to debug
            driveRobot.mecanumDrive_Polar(leftMagnitude, leftDirection, rotationRate);
 

            Timer.delay(0.01);
        }

That code is just for our drive. Here is where we declare things and such:

  private void getReadings() {


        leftMagnitude = leftStick.getMagnitude();
        leftDirection = leftStick.getDirectionDegrees();
        rotationRate = rightStick.getAxis(Joystick.AxisType.kX);
        Timer.delay(0.01);

    }

Here are some more:

 Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Jaguar jagFrontLeft = new Jaguar(1);
    Jaguar jagFrontRight = new Jaguar(2);
    Jaguar jagBackLeft = new Jaguar(3);
    Jaguar jagBackRight = new Jaguar(4);
    Victor victorLeft = new Victor(5);
    Victor victorRight = new Victor(6);
   Relay spike1 = new Relay(7);
    Relay spike2 = new Relay(8);
    RobotDrive driveRobot = new RobotDrive(jagFrontLeft, jagBackLeft, jagFrontRight, jagBackRight);
    Gyro roboGyro = new Gyro(2);
    double leftMagnitude = 0;
    double leftDirection = 0;
    double rotationRate = 0;

Can anyone here spot anything that seems off to you? I have never programmed a holonomic drive, nor was I really prepared, but I wanted to give it a try. Any help would be appreciated.

Attached is a diagram of what our robot tends to do. The green arrow is the direction that it is supposed to travel, and the red arrow is the direction that it actually travels.

We are using the Logitech Attack3 Joysticks, and yes I have tried to read most of the documentation that is already present on this forum. Specifically the posts by "Ether," those posts really helped... The problem exists in the fact that I don't really understand the calculations and diagrams presented in them.

Thanks in advance.

2745


OH! and all of our PWM cables are plugged in properly/in the right slot