Fix servo angles and invert issue. Dirty hack!