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*=0; yValues*=0; twistValues*=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*=0; yValues*=0; twistValues*=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*;
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.*******