V5 field centric mecanum code

There have been a few requests for field centric X-drive code in the past couple of weeks. I’ve linked to an old RobotC example, but I revised that code today for my demo drivetrain and pushed out to github. My drive uses 8 motors, but it should be easy to revise the code for four. Obviously, it needs a V5 Inertial sensor to run.

project is here.


Very clean and organized code. Thank you so much!

1 Like

VERY clean code.

It almost looks like you do this professionally…


What’s the point of the variable temp?

I assume you mean here.

    // rotate coordinate system - IMU positive angle is CW
    int temp   = ctrl_right * sin(theta) + ctrl_fwd * cos(theta);
    ctrl_right = ctrl_right * cos(theta) - ctrl_fwd * sin(theta);
    ctrl_fwd = temp;

in the first line. you cannot update ctrl_fwd as it’s used in the second line, so temp holds the value that will be used for ctrl_fwd after the next calculation is done.


/sigh - Title is right, body is wrong. Not an X drive but Mecanum drive. Nice code!

On the programming side they are the same, no?