I am working on odometry for my robot with PROS. My code is currently as follows:
void position_track() {
auto plus = (right_mtrs.getPosition() + left_mtrs.getPosition()) / 2 * M_PI * wheelDiam;
distanceMoved += plus;
right_mtrs.tarePosition();
left_mtrs.tarePosition();
}
void moveDistance(double i) {
while(distanceMoved < i) {
left_mtrs.moveVoltage(2400);
right_mtrs.moveVoltage(2400);
position_track();
pros::delay(20);
}
left_mtrs.moveVoltage(0);
right_mtrs.moveVoltage(0);
while(true) { position_track(); pros::delay(20); }
}
(Note that distanceMoved
and wheelDiam
are class variables.)
However, the odometry is not accurate. When I command it to move 12 in. (wheel diameter 3.25), it actually moves over twice that distance. However, this is rarely consistent — sometimes it will move as much as three times that distance. I have also tried to use libraries like Okapi, which all face the same issue.
The robot itself uses 6 motors (three on each side) and gears — 8 wheels touch the ground.
Please advise if there are any errors that I should fix.