#include <Servo.h> 

/***************************************************
/ MINDS-I ESC with Servo Example
/
/ The ESC supplied with MINDS-I kits has a standard
/ control scheme. The ESC is controlled like a servo.
/ >90 = Foward
/ 90 = Nuetral
/ <90 = Reverse
/
/ 0 = Brake
/ *Brakes will only go into effect if the ESC was driving
/ foward when activated; otherwise, 0 will activate full reverse
/ 
/ The servo will always move to the most recent value sent with a .write()
/***************************************************/

Servo ESC;               // Creates a servo object for your electronic speed controller
Servo frontservo;        // Create servo object to control a servo
Servo rearservo;         // Create servo object to control a servo

void setup() {
  ESC.attach(2);         // Defines the pin the ESC will be plugged into. 
  
  ESC.write(90);         // Sets the output to nuetral
  delay(1000);           // Waits 1 second while in nuetral for the ESC to arm.
  frontservo.attach(3);  // Attaches the front servo on pin 3 to the servo object
  rearservo.attach(4);   // Attaches the rear servo on pin 4 to the servo object
}

void loop() {
  frontservo.write(89);  // Set servo at 90 degrees
  rearservo.write(97);   // Set servo at 90 degrees
  delay(500);            //Alows the servos time to straighten
  
  ESC.write(102);        // Drives forward for 1 second
  delay(1000); 
  
  ESC.write(0);          // Apply brake for 1 second
  delay(1000);
  
  ESC.write(91);         // Release brake
  delay(100);
}