Styrt upp koden, flyttat ut servofunktioner till servo-filer. Lagt till några instruktioner till ax12.