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?


2 quick questions.

  1. Does placement of the inertial sensor matter as long as it is one of their official ways?(on the website it shows 6 ways)
  2. Would the code be any different for a X-Drive? I tried it and it seems to be not working. I could be dumb, though.

It is best to keep the inertial sensor’s tracking center as close to the center of the robot as possible (I was told that the further out the more likely you are to max out the g-force limit of the sensor)

Mecanum drive and x drive are code wise the same, try modifying the code until it works or find other sources to compare code to

1 Like

Ok. Thank You!

20 ch