Chief Delphi - Mecanum Drive Not Working

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- General Forum (http://www.chiefdelphi.com/forums/forumdisplay.php?f=16)

- - Mecanum Drive Not Working (http://www.chiefdelphi.com/forums/showthread.php?t=83152)

Mecanum Drive Not Working

So for some reason we can't get our mecanum drive working. Our code (in C++) looks like this:

Code:

[This just calculates the motor speeds which will be set later.

-myAbs(float) is just a function which finds the absolute value of a float.

-largest(float ...) is a function which finds the largest any number of inputted float values.

-these two functions have been tested and work correctly.]



   //Square Joystick inputs for better precision control

    x = mainStick.GetX(); x = x*x;

    y = mainStick.GetY(); y = y*y;

    twist = secondaryStick.GetX();

    //[0, 1] Throttle value which may be used to artificially decrease speed

    throttleFactor = -(mainStick.GetThrottle()-1)/2;

    //Set minimum threshold on Joystick inputs

    if(myAbs(x) + myAbs(y) + myAbs(twist) > JOY_THRESHOLD)

    {

            //Calculate raw motor speeds for vector drive

        Speed[NW] = y + x + twist;    Speed[NE] = y - x - twist;

        Speed[SW] = y - x + twist;    Speed[SE] = y + x - twist;

        

        //Calculate magnitude of largest motor speed

        topSpeed = largest(myAbs(Speed[NW]), myAbs(Speed[NE]), myAbs(Speed[SW]), myAbs(Speed[SE]));

        

        //If the calculated largest speed is greater than 1, scale everything down

        if(topSpeed > 1)

        {

            Speed[NW] /= topSpeed;    Speed[NE] /= topSpeed;

            Speed[SW] /= topSpeed;    Speed[SE] /= topSpeed;

        }



        //Give option artificially slow down with throttle while holding down a button

        if (mainStick.GetRawButton(THROTTLE_BUTTON))

        {

            Speed[NW] *= throttleFactor;    Speed[NE] *= throttleFactor;

            Speed[SW] *= throttleFactor;    Speed[SE] *= throttleFactor;

        }

    }

    else  //If Joystick inputs not within threshold

    {

        Speed[NW] = 0;    Speed[NE] = 0;

        Speed[SW] = 0;    Speed[SE] = 0;

    }

We later set the motors in a separate function:

Code:

    //Invert motors in code instead of electronically

    NWMotor.Set(Speed[NW]*NW_MOTOR_INVERT);

    NEMotor.Set(Speed[NE]*NE_MOTOR_INVERT);

    SWMotor.Set(Speed[SW]*SW_MOTOR_INVERT);

    SEMotor.Set(Speed[SE]*SE_MOTOR_INVERT);

The motors do move, but in weird ways (for example if we push the main joystick full forward, I believe only two motors on opposite corners moved).
Also, if we put default code on the robot it does seem to make all of the motors move, but we don't want to use default code because we feel like it's cheating and it would just be nice to get our own code working so we can mess or with it or change it if we want.
So... anybody got any ideas?