/*
  Exceed RC RockCrawler Driver, on romeo board for Android
  
*/

#include <Servo.h>

/* flags for specific indicators in the incomming serial buffer */
const byte S_FLAG = 0x61;  // start flag, signifies the beginning of the data in the buffer
const byte E_FLAG = 0x13;  // end flag, signifies the end of the data in the buffer
const byte B_FLAG = 0x3B;  // separator flag, signifies the separation betweeen bytes of data

/* counters and indexes */
int i,j, n, mstart;

byte inBuf[7];    // the buffer of bytes to store the incoming serial data

/* pin locations */
const int strSvrF = 10; // front steering servo
const int strSvrR = 3;  // rear steering servo
const int driveS = 6;   // throttle controller

/* Steering and throttle servo objects */
Servo frontS;  //90 is center, higher for right, lower for left, ms, 30-150
Servo rearS;  //90 is center, 30-150
Servo thrtl;  // 80 is centered signal, <80: forward, >80: backward

/*-----------------------------------------------------------------------------------------------------*/
// setup gets called once at the start of the program, will not be called again untill the board is reset
void setup(){
  
  /* Pin Modes, input or output */
  pinMode(strSvrF, OUTPUT);
  pinMode(strSvrR, OUTPUT);
  pinMode(driveS, OUTPUT);
  
  /* Steering and Throttle Servo init's */
  frontS.attach(strSvrF);
  rearS.attach(strSvrR);
  thrtl.attach(driveS);
  
  /* Integer Var's, Used for counters and indexes, init to 0 */
  i = j = n = mstart = 0;
  
  /* Center the steering servos */
  center_str();
  
  /* init the serial port at 9600 baud for the bluetooth module */
  Serial.begin(9600);
  
  
}// end setup
/*-----------------------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------------------*/
// loop is a continual iteration, will loop forever
void loop(){
    /* if there is any serial data in the defined serial port */  
    if(Serial.available()>0){
      
      delay(20);        // delay a small amount to allow the serial buffer to receive all the bytes
           
      n=0;              // index for the buffer array
      
      /* read the serial data as a byte and store it in the array at the index n, iterate until no more data is available */
      while(Serial.available()>0){
        inBuf[n++] = Serial.read();  // read and store the serial data, increment the counter
      }//end while
      
      
        proc_cmd( mstart, (n-1), inBuf );  // process the commands in the buffer
      
      
      /* flush the default serial buffer, if debug is enabled then this is the USB serial port, if NOT enabled then this is the bluetooth port*/
      Serial.flush();
      
            
    }//end if

}// end loop
/*-----------------------------------------------------------------------------------------------------*/

/*__________ Funcions______________*/
/*-------------------------------------------------------------------*/
// processes the commands taken from the input buffer
void proc_cmd( int start, int m_end, byte inB[]){
  switch(inB[start+1]){
    case 0x30:
       steer_l((int)inB[start+3]);
       break;
    case 0x40:
       center_str();
       break;
    case 0x50:
       steer_r((int)inB[start+3]);
       break;
    default:
       center_str(); 
  }// end steering dir. and mag. switch
  
  drive_m((int)inB[start+5]);
  
}// end process command
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
// center the steering servos
void center_str(){
  frontS.write(90);
  rearS.write(90);
}// end center
/*-------------------------------------------------------------------*/


/*-------------------------------------------------------------------*/
// drive the bot with velocity val, 80->180: backwards | 80->0: forwards
void drive_m( int val){
  if((val<84)&&(val>76))
    val = 80;   
  if(val<0)
     val = 0;
  if(val>180)
     val = 180;    
  thrtl.write(val); 
}// end drive motors
/*-------------------------------------------------------------------*/


/*-------------------------------------------------------------------*/
// steer the bot left with val, map 0-100% to 30-150
void steer_l(int val){  
  if(val>100)
    val = 100;
  if(val<0)
    val = 0;

  val = map(val, 0, 100, 0, 60);

  frontS.write(90-val);
  rearS.write(90+val);
  
}// end steer left
/*-------------------------------------------------------------------*/

/*-------------------------------------------------------------------*/
// steer bot right with same map
void steer_r(int val){  
  if(val>100)
    val = 100;
  if(val<0)
    val = 0;
    
  val = map(val, 0, 100, 0, 60);

  frontS.write(90+val);
  rearS.write(90-val);
  
}// end steer right
/*-------------------------------------------------------------------*/