removed divisor of 5 from diff in motor_servo