Fixes servo to allow [0, 180]
Fixes compass heading calculation