/*
 * Arduino Controlled Servo Robot (SERB) - Simple Routines
 * For more details visit: http://www.oomlout.com/serb 
 * 
 * Behaviour: A set of simple routines to help control your SERB.
 *            To use.
 *             1.Copy and paste the "PREAMBLE" portion to the top of your arduino sketch
 *             2.Copy and paste the "ROUTINES" portion to the body of your Arduino sketch
 *             3.add a call to "serbSetup();" to your "setup();" routine
 *             4.use the included routines (each routine is commented and quite easy to use)
 *
 * Wiring: Right Servo Signal - pin 9
 *         Left Servo Signal - pin 10 
 *
 * License: This work is licenced under the Creative Commons 
 *          Attribution-Share Alike 3.0 Unported License. To 
 *          view a copy of this licence, visit 
 *          http://creativecommons.org/licenses/by-sa/3.0/ 
 *          or send a letter to Creative Commons, 171 Second 
 *          Street, Suite 300, San Francisco, California 94105, 
 *          USA.
 *         
*/ 


 
//---------------------------------------------------------------------------------
// START OF ARDUINO CONTROLLED SERVO ROBOT (SERB) "PREAMBLE"
#include <Servo.h>

#define LEFTSERVOPIN  10
#define RIGHTSERVOPIN  9

Servo leftServo; 
Servo rightServo; 

int speed = 100; //sets the speed of the robot (both servos) a percentage between 0 and 100

// END OF ARDUINO CONTROLLED SERVO ROBOT (SERB) PREAMBLE
//---------------------------------------------------------------------------------


//------------------------------------------------------------------------------------------------------------
//START OF ARDUINO CONTROLLED SERVO ROBOT (SERB) "ROUTINES"

/*
 * sets up your arduino to address your SERB using the included routines
*/
void serbSetup(){
  setSpeed(speed);
  pinMode(LEFTSERVOPIN, OUTPUT);     //sets the left servo signal pin to output
  pinMode(RIGHTSERVOPIN, OUTPUT);    //sets the right servo signal pin to output
  leftServo.attach(LEFTSERVOPIN);    //attaches left servo
  rightServo.attach(RIGHTSERVOPIN);  //attaches right servo
  goStop();                          //stops the servos incase the internal timers are a little off
}


/*
 * sets the speed of the robot between 0-(stopped) and 100-(full speed)
 * NOTE: speed will not change the current speed you must change speed 
 * then call one of the go methods before changes occur.
*/ 
void setSpeed(int newSpeed){
  if(newSpeed >= 100) {newSpeed = 100;}     //if speed is greater than 100 make it 100
  if(newSpeed <= 0) {newSpeed = 0;}         //if speed is less than 0 make it 0 
  speed = newSpeed * 0.9;                   //scales the speed to be between 0 and 90
}

/*
 * sends the robot forwards
 */
void goForward(){
 leftServo.write(90 + speed);
 rightServo.write(90 - speed);
}

/*
 * sends the robot backwards
 */
void goBackward(){
 leftServo.write(90 - speed);
 rightServo.write(90 + speed);
}

/*
 * sends the robot right
 */
void goRight(){
 leftServo.write(90 + speed);
 rightServo.write(90 + speed);
}

/*
 * sends the robot left
 */
void goLeft(){
 leftServo.write(90 - speed);
 rightServo.write(90 - speed);
}

/*
 * stops the robot
 */
void goStop(){
 leftServo.write(90);
 rightServo.write(90);
} 
//END OF ARDUINO CONTROLLED SERVO ROBOT (SERB) ROUTINES
//------------------------------------------------------------------------------------------------------------