Added compass compensation and wrap around

added 90 to the compass reading and wrapped it around between 0 and 360