How do I make my mecanum drive field centric?

To make a mecanum drive field centric, the translation joystick controls the robot's direction relative to the field rather than the robot's frame . This is achieved by rotating the x/y components of the joysticks counter to the robot's angle, which is provided by an Inertial Measurement Unit (IMU) .

The IMU, found in Control Hubs and older Expansion Hubs, needs to be initialized. It is recommended to use the IMU class as of SDK v8.1, as the BNO055 class is not recommended for new development .

The IMU can be initialized as follows:

// Retrieve the IMU from the hardware map
imu = hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
        RevHubOrientationOnRobot.LogoFacingDirection.UP,
        RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
// Without this, the REV Hub's orientation is assumed to be logo up / USB forward
imu.initialize(parameters);

The robot's angle needs to be read in every loop, and a button to reset the angle is important to counteract drift and address potential zero changes from disconnects .

An example of resetting the yaw and getting the robot's heading is:

// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
    imu.resetYaw();
}

double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

The translation joystick values then need to be counter-rotated by the robot's heading. The formula for rotating a 2D vector is used, where x₁ and y₁ are the original vector components, beta is the angle to rotate by, and x₂ and y₂ are the resultant components .

The rotation can be implemented as:

// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);

These rotated values are then used in the mecanum kinematics :

double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + 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;

Was this right?

Sources6

Mecanum TeleOp

Game Manual 0·CC BY-NC 4.0

Note: Some common COTS mecanum drivetrain kits are the goBILDA Strafer Chassis Kit (https://www.gobilda.com/strafer-chassis-kit-v5/) and the REV Mecanum Drivetrain Kit (https://www.revrobotics.com/rev-45-2470/).

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.

Open Game Manual 0

Mecanum TeleOp > Field Centric

Game Manual 0·CC BY-NC 4.0

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 counter to the robot's angle, which is given by the IMU.

There is an IMU inside of Control Hubs (and older models of Expansion Hubs). Unlike most other hardware, it is recommended to do more than hardwareMap.get() to begin using it. Note, this is configured when creating a new configuration by default as imu. See the FTC doc page covering the IMU interface and its parameters (https://ftc-docs.firstinspires.org/programming_resources/imu/imu.html) for more information. The way the IMU will be initialized here is:

// Retrieve the IMU from the hardware map
imu = hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
        RevHubOrientationOnRobot.LogoFacingDirection.UP,
        RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
// Without this, the REV Hub's orientation is assumed to be logo up / USB forward
imu.initialize(parameters);

The angle needs to be read every loop. In addition to this, while the IMU keeps a consistent zero position between OpModes (relevantly, including between autonomous and teleop), adding a bind to reset the angle is important to counteract drift and because the zero can change due to some types of disconnects.

Note: BNO055 objects will reset the IMU zero when initialize is called. The BNO055 class is not recommended for new development. The IMU class does not have this behavior, and is the appropriate replacement as of SDK v8.1.

// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
    imu.resetYaw();
}

double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

Then, the translation joystick values need to be counterrotated by the robot heading. The IMU returns heading, however we need to rotate the movement counter to the robot's rotation, so its negative is taken. The joystick values are a vector, and rotating a vector in 2D requires this formula (proved here (https://matthew-brett.github.io/teaching/rotation_2d.html)), where x₁ and y₁ are the components of the original vector, beta is the angle to rotate by, and x₂ and y₂ are the components of the resultant vector.

x₂=x₁cos β - y₁sin β \\ y₂=x₁sin β + y₁cos β

// Rotate the movement direction counter to the bot's rotation
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 denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + 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;

Open Game Manual 0

Holonomic Drivetrains > X-Drive

Game Manual 0·CC BY-NC 4.0

X-Drive is a holonomic omni-wheel based drivetrain. This type of drive involves mounting 4 omni wheels at the corner of the robot at a 45 degree angle.

One notable difference between X-Drive and mecanum is strafe speed. While, as mentioned in the mecanum section, the ratio of strafe speed to forward speed is noticeably less than 1, the ratio on an X-Drive is exactly 1 due to the rotational symmetry of the wheel placement. This means that an X-Drive bot's strafe speed and forward speed are equivalent. The drivetrains are slower, however, when strafing at 45° (approximately frac{sqrt{2}}{2} of its forward speed).

Even though X-drive has good turning and acceleration, the main downside to the drive is packaging/form factor. Packaging refers to how easy/convenient the drivetrain fits into the overall design of the robot.

Ideally, the drivetrain should take up as little space as possible to make it easier to design mechanisms around. Because the omni wheels are offset, packaging a X-Drive is more difficult than other types of holonomic drive like mecanum or H-Drive. Also because of the strange packaging, it is relatively difficult to cleanly transfer power from the motors to wheels, meaning that most X-Drives end up being direct-driven, which is bad for the lifespan of the motor gearbox.

Note: When using X-Drive, the robot moves forwards/backwards/straight side-to-side sqrt{2} times faster than a drivetrain with wheels in the normal orientation (with the same gear ratio and wheel size).

For an explanation of why exactly this is, see this analysis (https://www.chiefdelphi.com/t/paper-mecanum-and-omni-kinematic-and-force-analysis/106153).

Open Game Manual 0

Holonomic Drivetrains > Mecanum Drive > Mecanum Wheels Miniguide

Game Manual 0·CC BY-NC 4.0

One of the most important features of a mecanum wheel is the mechanism that allows the roller to spin---either bushings or ball bearings. Mecanums which use ball bearings strafe better than those which use bushings, as the rollers can spin with less friction.

Note: In the past, some teams have invested in 6 inch diameter mecanum wheels. These are generally much more expensive and generally do not provide significant performance benefits. We highly recommend sticking with wheels that are between 3 and 4 inches in diameter.

Open Game Manual 0

Kinematics > Mecanum Drive > Inverse Kinematics

Game Manual 0·CC BY-NC 4.0

The inverse kinematics of a mecanum drive relate the desired velocity of the robot to the velocity required on the wheels. These are as follows:

vfl = vf - vs - (2r_b · ω)

vbl = vf + vs - (2r_b · ω)

vbr = vf - vs + (2r_b · ω)

vfr = vf + vs + (2r_b · ω)

Open Game Manual 0

Holonomic Drivetrains > Mecanum Drive > Advantages

Game Manual 0·CC BY-NC 4.0

Fantastic maneuverability and agility due to strafing, can avoid defense very well
Good acceleration, can have high top speed
Very versatile drivetrain for nearly any game

Suffers in traction, as mecanum rollers have a lower coefficient of friction than traction wheels; cannot traverse terrain
Able to be pushed around on defense
Wheels must be powered independently, so there is no redundancy

CAD Examples of Mecanum Drive (Click to expand) goBILDA COTS Click here to explore this model in OnShape online CAD (https://cad.onshape.com/documents/45549489f570f3694569a2df/w/85ff26b9fca4988ebc4df3b4/e/df9bcea72fcdd7e4aee4134b)

REV COTS Click here to explore this model in OnShape online CAD (https://cad.onshape.com/documents/45549489f570f3694569a2df/w/85ff26b9fca4988ebc4df3b4/e/b8d4a6cfe4f26c5170f40a6c)

goBILDA Custom Click here to explore this model in OnShape online CAD (https://cad.onshape.com/documents/45549489f570f3694569a2df/w/85ff26b9fca4988ebc4df3b4/e/cc2df2fbce0e9ca393696b54)

REV Custom Click here to explore this model in OnShape online CAD (https://cad.onshape.com/documents/45549489f570f3694569a2df/w/85ff26b9fca4988ebc4df3b4/e/40c3266dab2e444011cce79c)

(Figure: Configuration for mecanum wheels, courtesy goBILDA)

Open Game Manual 0