Refined conversion of position error to motor speed, much more stable, now. Also fixed bug with nextUpdate--was int instead of long int. Renamed a variety of parameters for more readability