Fixed for new wiring layout, added compass

Compass doesn't work well, need to switch to gyro.