Getting X & Y Velocities for a mecanum drive robot

So I’m working on pose estimation for a mecanum drive FTC but I’ve hit a wall. I’m having trouble getting velocities going in x and y directions in relation to the field. It’s not much but this is what I have so far

public double getXVeloctiy() {
    return 0;

public double getYVelocity() {
    return 0;

// Velocity is in inches/second
public double getLeftFrontVel() {
    // RPS as in revolutions per second
    return getLeftFrontRPS() * Constants.WHEEL_CIRCUMFERENCE;

public double getRightFrontVel() {
    return getRightFrontRPS() * Constants.WHEEL_CIRCUMFERENCE;

public double getLeftBackVel() {
    return getLeftBackRPS() * Constants.WHEEL_CIRCUMFERENCE;

public double getRightBackVel() {
    return getRightBackRPS() * Constants.WHEEL_CIRCUMFERENCE;

    public void updatePoseEstimation() {
    dt = runtime.seconds() - previousTimestamp;

    xVelocity = ?
    yVelocity = ?
    estimatedX += xVelocity;
    estimatedY += yVelocity;

    previousTimestamp = runtime.seconds();

I need help for what I should make variables xVelocity and yVelocity, so any help would be greatly appreciated.

See this whitepaper: Paper: Mecanum and Omni Kinematic and Force Analysis

I’ll also add that a lot of teams fight a lot of this problem using hardware. They use spring load unpowered omni’s to calculate the x and y velocities. This accounts for any slip in your mechanum drive. While this isn’t exactly the software answer I’m sure you were looking for, it’s a solution.

Yeah we don’t have odometers on my team

Here is our code from 2019 which calculated the pose of our drivetrain. We used the white paper linked above to calculate it. It was accurate within an inch or two if you didn’t accelerate too quickly


Wait where or how would you use MecanumForwardKinematics @jreneew2

If you looked at the file, you would see that the Delta is just a velocity within a certain snapshot of time, then you can look at the Periodic function in drivebasesubsystem.cpp to see how to obtain the robots position and current velocity.

1 Like

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.