Basic sonar program (NOT tested), tweaked gyro