Fixed horrible bugs in Arduino code
Swapped around servo pins on arduino