View Full Version : Help with mecanum Drive!

12-11-2011, 05:25 PM
Hey Andy from Team 3871 here, this year will be our first year using java and we curently are using macanum wheels... just wonder how to program them... dont quit understan the how to state the joystick axis..

drive.mecanumDrive_Cartesian(leftStick.getDirectio nDegrees(), leftStick.getMagnitude(), rightStick.gyroAngle();

Am i on the right track, and how do i go about wachdog...
missing something?

Here is everything i have so far...

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;

public class Team3871Robot extends SimpleRobot {

RobotDrive drive = new RobotDrive (1, 2);
Joystick leftStick = new Joystick(1);
Joystick rightStick = new Joystick(2);

public void autonomous() {

public void operatorControl() {
while (isOperatorControl() && isEnabled())
drive.mecanumDrive_Cartesian(leftStick.getDirectio nDegrees(), leftStick.getMagnitude(), rightStick.gyroAngle();

12-11-2011, 06:32 PM
Here is a java Doc for RobotDrive. (http://www.wbrobotics.com/javadoc/edu/wpi/first/wpilibj/RobotDrive.html)
Specifically, I would recommend looking at:

The constructors
mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle)
mecanumDrive_Polar(double magnitude, double direction, double rotation)
setInvertedMotor(RobotDrive.MotorType motor, boolean isInverted)

And here is one for the Joystick class (http://www.wbrobotics.com/javadoc/edu/wpi/first/wpilibj/Joystick.html)
Specifically, I would look at


I haven't coded mecanum, but I think you need 4 motors assigned to your RobotDrive instead of 2.

Also, try using the "search" button up top Where I found this similar thread (http://www.chiefdelphi.com/forums/showthread.php?t=97989)

12-12-2011, 09:31 AM
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 )
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);
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++)
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:


// 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");
DriverStationLCD.getInstance().println(DriverStati onLCD.Line.kMain6, 1, "testing");

driveJoystick = new JoystickFRC623Drive(1);

robotDrive = new RobotDrive(leftFrontMotor, leftRearMotor, rightFrontMotor, rightRearMotor);
robotDrive.setInvertedMotor(RobotDrive.MotorType.k FrontRight, true);
robotDrive.setInvertedMotor(RobotDrive.MotorType.k RearRight, true);

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

public void teleopInit() {
// call teleop init for joysticks

public void teleopPeriodic() {
// call telop periodic for joysticks to preprocess inputs

this.driveJoystick.getCurrentTwist() );


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