For Autonomous operation, two variables derived from encoder position are very helpful. I usually prefer to work in engineering units rather than encoder counts. The two are:
dist = average of left and right encoder in inches, positive when moving forward.
psi_deg = difference between left and right encoders in degrees of heading , positive increase when turning right.
If enc_rt and enc_lt are in counts (360 per revolution) then in software we compute
dist = (enc_rt+ enc_lt)/2 * dist_per_cnt.
heading psi_deg = (enc_lt-enc_rt)*deg_per_cnt.
enc_lt = SensorValue(left_encoder) (RobotC notation)
enc_rt = SensorValue(right_encoder) (RobotC notation)
dist_per_cnt = 2*pi*radius_wheel/360 cnts
deg_per_cnt = 57.3*dist_per_cnt/dist_wheel_base
Simple two step procedure to verify that these are implemented ok:
First compute the distance a wheel should go in one revolution:
d_rev = 2*pi*radius_wheel
Second compute the angle the robot would turn if the a left or right wheel turned one revolution while the other was stationary.
psi_deg_rev = 57.3*d_rev/dist_between_wheels.
Now… put robot on blocks
Turn the right wheel one revolution forward: Check psi_deg = – psi_deg_rev. , dist = d_rev/2.
Next turn the left wheel one revolution forward: Check psi_deg = 0 , dist = d_rev.
For a typical robot with 4 in wheels with a wheel base of 14 inches
dist_rev = 12.6 in , psi_deg_rev = 57.3*12.56/14 = 51.4 deg
dist_per_cnt = 12.6/360= .035 , deg_per_cnt = 51.4/360 = .142
Integer vs Floating Point Implementation
The above formulas are ok if using a floating point implementation. If an integer implementation is used , I usually select a precision of 1 deg and .1 inches (deci inch). To remind myself of the output units in deci inchs I rename dist as dist_10in. So the calculations are done in floating point and then converted to integer. The new Cortex allows floating point.
int psi_deg = (float)(enc_lt-enc_rt)*deg_per_cnt