Odometry equation producing wrongly scaled heading

I have begun working on a custom odometry coded for absolute position tracking using three passive odometry wheels (left, right and lateral)

The heading calculation code (PROS) is pretty simple, yet it seems to produce consistently scaled heading results, such that one rotation reads as approximately 300 degrees, while rotating the robot back to its original state consistently reads a heading of around 4 degrees. I do not believe that this issue is caused by any measured inaccuracy such as the tracking radius (the distance between the left and right tracking wheels), since the scaling ratio of 0.83 is well beyond the tolerance of human error or even the difference between the inner and outer edges of both tracking wheels combined.

The issue, therefore, must be a fundamental error in the code or odometry equations. Here is a snippet from the PROS code, calculating the absolute heading of the robot in real time.

float heading = 0;
float x = 0;
float y = 0;

void runOdom()
{

pros::Imu imu (14);

pros::ADIEncoder el (1, 2, false);
pros::ADIEncoder em (3, 4, false);
pros::ADIEncoder er (5, 6, false);

el.reset();
em.reset();
er.reset();
imu.reset();

pros::delay(150);

float elDelta = 0;
float erDelta = 0;
float emDelta = 0;
float elRaw = 0;
float erRaw = 0;
float emRaw = 0;
float elDeltaIN = 0;
float erDeltaIN = 0;
float emDeltaIN = 0;

float pi = 3.141593;

float eDistMM = 212; // Distance in mm between the centers of the left and right tracking wheels (+- 2 mm)
float eDistIN = eDistMM / 25.4;
float mDistMM = 214; // Distance between the center of the central lateral tracking wheel and the center of the robot (not relevant for this specific question)
float mDistIN = mDistMM / 25.4;
float eDiamIN = 2.25; // Diameter of each tracking omni
float eCircIN = eDiamIN * pi;

float rad = 0;
float circ = 0;

float rot = 0;

float ic = 0;
float ct = 0;

while(true)
{
	elDelta = el.get_value() - elRaw;
	erDelta = -er.get_value() - erRaw;
	emDelta = em.get_value() - emRaw;

	elDeltaIN = (elDelta / 360) * eCircIN;
	erDeltaIN = (erDelta / 360) * eCircIN;
	emDeltaIN = (emDelta / 360) * eCircIN;

	elRaw = el.get_value();
	erRaw = -er.get_value();
	emRaw = em.get_value();

	rot = (((erDeltaIN - elDeltaIN) / (eDistIN)))*(180/pi);

	if (std::abs(rot) > 0 && std::abs(rot) < 100)
	{
	heading -= rot;
	}
	else
	{
		printf("%f\n", std::abs(rot));
	}

// This is for calculating translation so no part of the main question, although it does not function correctly so any help about this would also be appreciated.
	ic = (elDeltaIN + erDeltaIN) / 2;
	ct = std::tan(rot * (pi / 180)) * -mDistIN;
	x += (ic * std::sin(rot));
	x += ((emDeltaIN - ct) * (std::sin((heading - 90) * (pi / 180))));
	y += (ic * std::cos(rot));
	y += ((emDeltaIN - ct) * (std::cos((heading - 90) * (pi / 180))));

	pros::lcd::set_text(0, "l " + std::to_string(elRaw));
	pros::lcd::set_text(1, "r " + std::to_string(erRaw));
	pros::lcd::set_text(2, "x " + std::to_string(x));
	pros::lcd::set_text(3, "y " + std::to_string(y));
	pros::lcd::set_text(4, "h " + std::to_string(heading));
	pros::lcd::set_text(5, "ct " + std::to_string(ct));
	pros::lcd::set_text(6, "emd " + std::to_string(emDeltaIN));


	pros::delay(5);

}

} 

My original post didn’t get saved so this is in less detail sorry.

Another question, would using the v5 inertial sensor be more accurate for heading than tracking wheels in general. Both are vulnerable to drift, however vibration is known to throw the imu off far more. What kind of accuracy do teams get with 3 wheel odometry?

Sorry to ‘bump’ this, but we have a competition fairly soon. Is anyone able to help?

@1408F, your equations look good.

I think this should be 2.75, which must be the major source of error.

As for 4 deg return to the origin discrepancy - the slop in the omni rollers and axles could play a role. What happens if you turn back a few degrees past the zero heading and then move forward to exactly zero position?

Finally, this is statistically unlikely to affect you tests but there is always a slight chance for encoder to tick between the two calls to get_value(). I would call the function only once per loop and then just move the value between the variables.

1 Like

2.25/2.75 = 0.818

math checks out

4 Likes