#include <MINDSi.h>
#include <Servo.h>

/***************************************************
/MINDS-i Application. mymindsi.com
/
/Encoder Application
/
/This Encoder Application code is a basic example 
/of how to incorporate your Encoder in the Arduino 
/code.  In this code your rover will drive forward 
/for a certain number of revolutions of the 
/driveshaft, stop and drive back in reverse the 
/same /number of revolutions. 
/
/***************************************************/
Servo drive, steer;

void setup() {
  startEncoder(); //begin counting encoder steps

  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
}

void loop() {
  while( getSteps() < 4000) { //move forward 4000 steps
    drive.write(100);
    delay(5);
  }

  drive.write(0); //brake and wait
  delay(500); 
  drive.write(95); //disarm brakes

  while( getSteps() > 0){ //move backwards until starting position
    drive.write(80);
    delay(5);
  }

}