#include <MINDSi.h>
#include <Servo.h>
#include <Wire.h>  //necessary only for I2C devices

/***************************************************
/ MINDS-i Application. mymindsi.com
/
/Compass Application
/
/This Compass Application code is a basic example 
/of how to incorporate your Compass in the Arduino 
/code.  In this code your rover will drive and turn 
/until it faces north.
/
/***************************************************/
Servo drive, steer;

void setup() {
  beginCompass();

  drive.attach(4); //set a pin for the ESC/steering servo to use
  steer.attach(5); 
    
  drive.write(90); //set the output for the ESC/servo
  steer.write(90);

  delay(1000); //delay 1 second for arming

  steer.write(135); //turn wheels hard
}

void loop() {
  if( fabs( getHeading() ) < .05 ) drive.write(90); //stop when near north
  else drive.write(100); //otherwise drive (turning)

}