Your linear velocity should be the average of both wheel values. Assuming there's some wheel radius of WHEEL_RADIUS
, as you've stated, then you should get each wheel speed as:
left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
linear_velocity = 0.5f * (left_velocity + right_velocity);
Units matter and they're important! I define RPM_TO_RAD_PER_S
here because you need to convert from motor RPM to radians per second; that conversion is:
$$
\left(\frac{1 \mbox{ rev}}{1 \mbox{ minute}}\right) * \left( \frac{1 \mbox{ minute}}{60 \mbox{ seconds}} \right) * \left(\frac{2\pi \mbox{ radians}}{1 \mbox{ rev}}\right) \\
\left(\frac{1 \require{cancel}\bcancel{\mbox{ rev}}}{1 \require{cancel}\cancel{\mbox{ minute}}}\right) * \left( \frac{1 \require{cancel}\cancel{\mbox{ minute}}}{60 \boxed{\mbox{ seconds}}} \right) * \left(\frac{2\pi \boxed{\mbox{ radians}}}{1 \require{cancel}\bcancel{\mbox{ rev}}}\right) \\
\mbox{RPM_TO_RAD_PER_S} = \frac{2\pi}{60} \ = 0.1047
$$
The DIST_PER_RAD
is just the circumference, 2*pi*WHEEL_RADIUS
, divided by the number of radians in a complete revolution, which is just 2*pi
. This means that DIST_PER_RAD
is the same as the WHEEL_RADIUS
, but I think it's important to point out why the equation works out the way that it does. Convert RPM to rad/s, and then use that value with the wheel radius to get speed.
Important to note - left_rpm
and right_rpm
actually need to be in RPM, and DIST_PER_RAD
and/or WHEEL_RADIUS
needs to be in meters in order to get the output velocities in meters per second.
Moving on to the rotation rate -
Assume for a moment that rotation is positive CCW, viewed from above. If, hypothetically, the robot is rotating CCW, then it would mean that the right wheel is moving some distance further than the left wheel. The difference in distances traveled becomes the arc length scribed by the right wheel (again, relative to the left), and the wheel base, or distance between the wheels, becomes the hypotenuse or radius for the rotation.
Continuing with the assumption, positive is CCW, you would wind up with an equation like:
angular_rotation = (right_distance - left_distance)/WHEEL_BASE;
You can divide through by time to get:
angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
This is coupled with the linear velocity equation, and you wind up with:
angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
linear_velocity = 0.5f * (left_velocity + right_velocity);
You can solve for the right_velocity
in the angular_rate
equation:
angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
angular_rate*WHEEL_BASE + left_velocity = right_velocity;
And then plug that into the linear velocity equation:
linear_velocity = 0.5f * (left_velocity + angular_rate*WHEEL_BASE + left_velocity);
Then solve for the left_velocity
:
linear_velocity = 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE + 0.5f*left_velocity;
linear_velocity = 0.5f*left_velocity + 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity = left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity - 0.5f*angular_rate*WHEEL_BASE = left_velocity;
left_velocity = linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
And then plug this back into the expression for the right_velocity
:
angular_rate*WHEEL_BASE + left_velocity = right_velocity;
right_velocity = angular_rate*WHEEL_BASE + left_velocity;
right_velocity = angular_rate*WHEEL_BASE + linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = angular_rate*WHEEL_BASE - 0.5f*angular_rate*WHEEL_BASE + linear_velocity;
right_velocity = 0.5f*angular_rate*WHEEL_BASE + linear_velocity;
This then gives the equations for left_ and right_velocities in terms of linear velocity and angular rate:
left_velocity = linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = linear_velocity + 0.5f*angular_rate*WHEEL_BASE;
Now, recall the equations for the conversion from _rpm
to _velocity
:
left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
And you can see that:
left_rpm = left_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = right_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
Which, substituting in the _velocity terms, gives:
left_rpm = (linear_velocity - 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = (linear_velocity + 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
This looks similar to what you state from the start, but there's a conversion here from RPM to rad/s. If you wanted this in percentage, then you should use the percentage formula:
percentage = 100.0f * (value/maximum);
I would define a value to be something like:
RPM_TO_PERCENT = 100.0f/TOP_SPEED_RPM;
and then you can do:
left_percent = left_rpm * RPM_TO_PERCENT;
right_percent = right_rpm * RPM_TO_PERCENT;
You can define any other values you want, too, such as a value that would get you to a PWM output on a 0-255 scale, like:
RPM_TO_PWM = 255.0f/TOP_SPEED_RPM;
and then cast to int
on the equation:
left_pwm = <int>(left_rpm * RPM*PWM);
right_pwm = <int>(right_rpm * RPM*PWM);
You haven't posted any sample data, so I'm assuming this is the answer to your question, at least - that the source of your trouble is a units problem. Don't worry about defining too many variables! I don't think you would ever run into a problem where you're at a limit on how many variables you have defined. Defining the conversion constants like this makes things easier to read, debug, maintain/update, etc.