[Mike Shaul]

Here is some basic P control code, let me know if anyone catches an error!

#define GAIN       10    //This will need to be tuned

u8 p_control(u16 desired, u16 actual)
{
    s16 error, control;

    error = desired - actual;
    control = error * GAIN;
    if( control > 504 )
		control = 504;
    else if( control < -504 )
   		control = -504;
    
    control /= 4;   /* Convert to 8-bits */

    return (u8)(control +127);   /* PWM Output */
}