# Programming Tutorial - Mecanum Drivetrain#

## Mecanum Physics#

Mecanum drive is a very popular drive train in FTC. Mecanum drive enables holonomic movement. This means that the drivetrain is able move in any direction while rotating: forwards, backwards, side to side, translating while rotating, etc. Here is a neat video demonstrating such movement.

Mecanum wheels have rollers at a 45° angle to the rest of the wheel. Since these are in contact with the ground instead of something solid like in a traction wheel, instead of the wheel creating a force parallel to the orientation of the wheel, it creates one 45° from parallel. Depending on how the wheels are driven, X or Y components of the force vectors can cancel which allows movement in any direction.

## Using Vectoring to Create Omnidirectional Movement#

A standard mecanum drive configuration possesses 4 mecanum wheels oriented in an “X” shape. This means that the rollers are angled towards the center when looking at it from above. This configuration allows one to add up the force vectors generated by the offset rollers and derive movement in any direction. It is important to note that because of friction, perfect movement isn’t possible in every direction, so a mecanum drivetrain will be able to drive slightly faster forwards/backwards than any other directions. Combining translation and rotation will also result in slower movement.

In the image above, vectors 1, 2, 3, and 4 are the force vectors created by the mecanum wheels when the chassis is instructed to drive towards the top of the image. All motors are driving forward. The blue and red lines are their X and Y components, respectively. Here are a few examples of how the wheels must be driven to achieve different movements:

Attention

It is strongly advised to not hardcode these movements in; there is a much better way described below that allows for true holonomic movement and is much more elegant.

## Deriving Mecanum Control Equations#

Before thinking about mecanum, envision a scenario where you have a 2 motor tank drivetrain which you want to control using the left stick Y axis for forward/backward movement, and the right stick X axis for pivot turning. The motors are configured so that positive is clockwise for the right motor when the body is facing away from you, and the left motor is the opposite. To control only forward/backward movement, you simply need to set the motor powers to the Y stick value (flip the sign since Y is reversed):

```
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
leftMotor.setPower(y);
rightMotor.setPower(y);
```

Although at first adding rotation might seem like a difficult task, it’s actually super simple. All you need to do is subtract the x value from the right side, and add it to the left:

```
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.right_stick_x;
leftMotor.setPower(y + x);
rightMotor.setPower(y - x);
```

Here, if the Y stick is pressed upwards, both of the motors will be fed a positive value, causing the robot to move forward. If it is pressed downwards, both of the motors will be fed a negative value, causing the robot to move backwards. A similar principle applies for rotation: if the X stick is pushed rightward, the left wheels will spin forward while the right spin backward, causing rotation. The opposite applies for pushing the stick left. If both sticks are pushed at the same time, say the Y stick is at 1 and the X stick is also at 1, the value of the left wheels will be \(1+1=2\) (which gets clipped to 1 in the SDK) and the right wheels will be \(1-1=0\), which causes a rightward curve.

Applying omnidirectional movement with mecanum wheels operates under the same principle as adding turning into the tank example. The left stick X values will be added or subtracted to each wheel depending on how that wheel needs to rotate to get the desired movement. The only difference between adding turning is that rather than wheels on the same side being the same sign, wheels diagonal to each other will be the same sign.

We want a positive X value to correlate to rightward strafing. If we refer back to the vectoring image, this means that the front left and back right need to rotate forward, while the back left and front right need to rotate backwards. So, we should add the x value to the front left and back right and subtract it from the back right and front left:

```
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;
frontLeftMotor.setPower(y + x + rx);
backLeftMotor.setPower(y - x + rx);
frontRightMotor.setPower(y - x - rx);
backRightMotor.setPower(y + x - rx);
```

Important

Motors in FTC spin counterclockwise when given positive power by default (except for NeveRest motors). In this case, you need to reverse the direction of the right drive motors so that they spin toward the same direction as the left drive motors when supplied with a positive power (for a drivetrain using NeveRests, reverse the right side instead). This can be done with `DcMotor.setDirection(DcMotor.Direction.REVERSE)`

.

This is the same as the tank example, except now with 4 motors and the strafing component added. Similarly to the tank example, the Y component is added to all wheels, and the right x (rx) is added to the left and subtracted from the right. Now, we have added another component that will allow us to strafe rightward. In doing that, however, we have actually allowed for strafing in any direction. If you think about it, pressing the joystick to the left will do the same thing in reverse, which is what is needed to strafe left. If it is pressed at 45 degrees, the x and y components of the joystick will be equal. This will cause two diagonal motors to cancel, allowing for diagonal movement. This same effect applies to every angle of the joystick.

Now that we have a functioning mecanum driving program, there are a few things that can be done to clean it up. The first of these would be multiplying the left X value by something to counteract imperfect strafing. Doing this will make the drive feel more accurate on non axis aligned directions, and make field centric driving more accurate. In this tutorial, we will use 1.1, but it’s really up to driver preference.

```
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
```

The other improvement we can make is scale the values into the range of -1 to 1.

Since the SDK simply clips (limits) the powers to that range, we can lose the ratio we are looking for unless we proactively put all the numbers back in that range while still maintaining our calculated ratio. For example, if we calculate values of 0.4, 0.1, 1.1, and 1.4, they will be clipped to 0.4, 0.1, 1.0, and 1.0, which is not the same ratio. Instead, we need to divide all of them by the largest power’s absolute value when it exceeds 1:

```
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
```

Make sure to set the powers on your motor and update this every loop in an opmode!

## Robot-Centric Final Sample Code#

```
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@TeleOp
public class MecanumTeleOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft");
DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft");
DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight");
DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight");
// Reverse the right side motors
// Reverse left motors if you are using NeveRests
motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
motorFrontLeft.setPower(frontLeftPower);
motorBackLeft.setPower(backLeftPower);
motorFrontRight.setPower(frontRightPower);
motorBackRight.setPower(backRightPower);
}
}
}
```

## Field Centric#

With field centric mecanum drive, the translation joystick controls the direction of the robot relative to the field, as opposed to the robot frame. This is preferred by some drivers, and make some evasive action easier, as one can spin while translating in a given direction easier. To do this, the x/y components of the joysticks are rotated by the robot’s angle, which is given by the IMU.

There is a BNO055 IMU inside of Control Hubs (and older models of Expansion Hubs). Unlike most other hardware, it requires more than `hardwareMap.get()`

to begin using it. Note, this is configured when creating a new configuration by default as `imu`

. There is an FTC SDK sample on how to use the BNO055. The way the BNO055 IMU will be initialized here is:

```
// Retrieve the IMU from the hardware map
BNO055IMU imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
// Technically this is the default, however specifying it is clearer
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
// Without this, data retrieving from the IMU throws an exception
imu.initialize(parameters);
```

The angle needs to be read every loop. The IMU returns heading with clockwise positive, while the following code needs counterclockwise positive, so its negative is taken.

```
// Read inverse IMU heading, as the IMU heading is CW positive
double botHeading = -imu.getAngularOrientation().firstAngle;
```

Then, the translation joystick values need to be rotated by the robot heading. The joystick values are a vector, and rotating a vector in 2D requires this formula (proved here), where \(x_1\) and \(y_1\) are the components of the original vector, \(\beta\) is the angle to rotate by, and \(x_2\) and \(y_2\) are the components of the resultant vector.

```
double rotX = x * Math.cos(botHeading) - y * Math.sin(botHeading);
double rotY = x * Math.sin(botHeading) + y * Math.cos(botHeading);
```

Then, these rotated values can be put into the mecanum kinematics shown earlier.

```
double frontLeftPower = rotY + rotX + turn;
double backLeftPower = rotY - rotX + turn;
double frontRightPower = rotY - rotX - turn;
double backRightPower = rotY + rotX - turn;
```

### Field-Centric Final Sample Code#

```
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@TeleOp
public class FieldCentricMecanumTeleOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft");
DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft");
DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight");
DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight");
// Reverse the right side motors
// Reverse left motors if you are using NeveRests
motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
// Retrieve the IMU from the hardware map
BNO055IMU imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
// Technically this is the default, however specifying it is clearer
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
// Without this, data retrieving from the IMU throws an exception
imu.initialize(parameters);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
// Read inverse IMU heading, as the IMU heading is CW positive
double botHeading = -imu.getAngularOrientation().firstAngle;
double rotX = x * Math.cos(botHeading) - y * Math.sin(botHeading);
double rotY = x * Math.sin(botHeading) + y * Math.cos(botHeading);
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
motorFrontLeft.setPower(frontLeftPower);
motorBackLeft.setPower(backLeftPower);
motorFrontRight.setPower(frontRightPower);
motorBackRight.setPower(backRightPower);
}
}
}
```