Hey, everyone. New to the FRC coding community.
Migrating over from FTC and the world of Roadrunner and Odometry, I was wondering if anyone has any sample code for a two-wheel odometry setup for trajectory following purposes.
Currently, our team is following a trajectory following code example provided by WPILIb under their Mecanum Controller Command class. While it does work most of the time, due to weight distribution issues on our robot, the odometry tracking carried out by the wheels becomes very inaccurate and our position is all screwed up.
Now, on our robot, we do have a two wheel odometry setup. Thing is, I don’t know how to convert the distance recorded by those wheels into something we can use. I know the ChasisSpeed class is an option and we have tried something like this:
speedChasis = ChasisSpeeds(encxvelocity, encyvelocity, angle_rate);
MecanumWheelSpeeds speeds = mecanumkinematics.toWheelSpeeds(speedChasis);
mecanumOdometry.update(imu.getRotation2d, speeds);
But it hasn’t been working out nicely and we have problems with heading corrections and overshooting how far it corrects. As such, does anyone have any leads to where I can find some sample code? Am I on the right track? Is this how I use the ChassisSpeed class to find our distance traveled? etc?