Help with mecanum Drive!

[charrisTTI]

Here are some sections of code from FRC 623's mecanum drive from 2011.

First, as mentioned above you need 4 motors for mecanum. Additionally, 623 used a three axis joy stick: x, y, and twist.

The following class implements a "low pass" filter on the joystick inputs (reduces sensitivity) and transforms x, y, and twist to magnitude, direction, and rotation required by the mecanumDrive_Polar method.

public class JoystickFRC623Drive extends Joystick
{
    // number of samples for averaging
    private static final int FILTER_SAMPLE_SIZE = 5;

    public double getCurrentAngle()
    {
        synchronized (this)
        {
            return currentAngle;
        }
    }

    public double getCurrentMagnitude()
    {
        synchronized (this)
        {
        return currentMagnitude;
        }
    }

    public double getCurrentTwist()
    {
        synchronized (this)
        {
        return currentTwist;
        }
    }
    
    int position;
    double [] xValues;
    double [] yValues;
    double [] twistValues;

    double currentMagnitude;
    double currentAngle;
    double currentTwist;

    // override to allow implementation of filtering
    public JoystickFRC623Drive( int port )
    {
        super(port);
        position = 0;
        xValues = new double [FILTER_SAMPLE_SIZE];
        yValues = new double [FILTER_SAMPLE_SIZE];
        twistValues = new double [FILTER_SAMPLE_SIZE];
        for(int i=0; i<FILTER_SAMPLE_SIZE; i++) {xValues[i]=0; yValues[i]=0; twistValues[i]=0;}

    }

    public void teleopInit()
    {
        // danny you may want to reset the arrays to zero here --steven
        for(int i=0; i<FILTER_SAMPLE_SIZE; i++) {xValues[i]=0; yValues[i]=0; twistValues[i]=0;}
        // call when teleop starts
    }

    public void teleopPeriodic()
    {
        // called each time new driver station data is available


        xValues[position] = super.getAxis(AxisType.kX);
        yValues[position] = super.getAxis(AxisType.kY);
        twistValues[position] = super.getAxis(AxisType.kTwist);
        position++;
        if (position>=FILTER_SAMPLE_SIZE) position = 0;

        double averageX = averageArray(xValues);
        double averageY = averageArray(yValues);
        double averageTwist = averageArray(twistValues);

        setMagnitudeAngleTwist(averageX, averageY, averageTwist);

    }

    public double averageArray(double [] array)
    {
        double sum = 0;
        for(int i=0; i<array.length; i++)
            sum+=array[i];
        return sum/array.length;
    }

    public void setMagnitudeAngleTwist(double averageX, double averageY, double averageTwist)
    {
        synchronized (this)
        {
            currentMagnitude = Math.sqrt(averageX*averageX+averageY*averageY);
            if (currentMagnitude>1.0) currentMagnitude = 1.0;

            currentAngle = Math.toDegrees(MathUtils.atan2(averageX, -averageY));
            currentTwist = averageTwist;
        }
    }
}

The following snippets of code show setup the hardware(We used CAN bus for motor control) and how to pass data from joystick to robot drive. Our full implementation has a gyro for stabilization when not intentionally using twist as well as a stabilized "idle" mode (robot not moving, but gyro engaged so that if robot is bumped it will attempt to stay in position) This was accomplished with a state machine implementation with the following states:

STATE_IDLE
STATE_DRIVER_XY
STATE_DRIVER_TWIST
STATE_IDLE_WITH_GYRO

    // drive motors
    public CANJaguar leftFrontMotor;
    public CANJaguar leftRearMotor;
    public CANJaguar rightFrontMotor;
    public CANJaguar rightRearMotor;

    // robot drive instance
    public RobotDrive robotDrive;

    // our joystick class
    public JoystickFRC623Drive driveJoystick;

    public void robotInit() {

    try {
            leftFrontMotor = new CANJaguar(12);
            rightFrontMotor = new CANJaguar(13);
            leftRearMotor = new CANJaguar(14);
            rightRearMotor = new CANJaguar(11);

        } catch (CANTimeoutException ex) {
            System.out.println("Stuck in CANTimeoutError");
            System.out.println(ex.getMessage());
            ex.printStackTrace();
            DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "testing");
            DriverStationLCD.getInstance().updateLCD();
        }

        driveJoystick = new JoystickFRC623Drive(1);

        robotDrive = new RobotDrive(leftFrontMotor, leftRearMotor, rightFrontMotor, rightRearMotor);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

        // create gyro instance
        steeringGyro = new Gyro(1, 2);
    }

    public void teleopInit() {
        // call teleop init for joysticks
        this.driveJoystick.teleopInit();
    }

    public void teleopPeriodic() {
        // call telop periodic for joysticks to preprocess inputs
        this.driveJoystick.teleopPeriodic();

        this.robotDrive.mecanumDrive_Polar(
                     this.driveJoystick.getCurrentMagnitude(),
                     this.driveJoystick.getCurrentAngle(),
                     this.driveJoystick.getCurrentTwist() );

}

If you would like to see state machine implementation let me know.