/* Function: DISTANCE() and HEADING()
 * Author: 
 * Purpose: The quadrature "encoder" code that Kevin Watson wrote
 *   provides distance in encoder ticks.  This tends to be awkward
 *   for navigation.  The following functions convert these to
 *   distance in inches and heading in degrees.
 */
#include "user_routines.h"
#include "distance.h"

/*
 * Return the average of the left and right encoders.  Scale
 * this number to inches.
 * Note: DIST_PER_PULSE_LEFT and DIST_PER_PULSE_RIGHT
 *   must be defined in "user_routines.h".  This number is
 *   scaled up by a factor of 1000 to avoid roundoff error.
 */
long DISTANCE(long l, long r)
{
        long distance;
        distance=((l*DIST_PER_PULSE_LEFT)+(r*DIST_PER_PULSE_RIGHT));
        distance=distance/2000;
        return(distance);
}

/*
 * Return the difference of the left and right encoders.  Scale
 * this number to degrees.
 * Note: DEGREE_PER_PULSE_LEFT and DEGREE_PER_PULSE_RIGHT
 *   must be defined in "user_routines.h".  This number is
 *   scaled up by a factor of 1000 to avoid roundoff error.
 */
long HEADING(long l, long r)
{
        long distance;
        distance=((l*DEGREE_PER_PULSE_LEFT)-(r*DEGREE_PER_PULSE_RIGHT));
        distance=distance/1000;
        return(distance);
}