## Formulas and Checks for Encoder Derived Outputs

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.

where

enc_lt = SensorValue(left_encoder)  (RobotC notation)

enc_rt = SensorValue(right_encoder) (RobotC notation)

and

deg_per_cnt = 57.3*dist_per_cnt/dist_wheel_base

Test Procedure:

Simple two step procedure to verify that these are implemented ok:

First compute the distance a wheel should go in one revolution:

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.

Debug Outputs:

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 dist_10in=10.*(float)(enc_lt+enc_rt)/2.*dist_per_cnt

int psi_deg = (float)(enc_lt-enc_rt)*deg_per_cnt