/*
  Exceed RC RockCrawler Driver, on romeo for PC
  
*/
#include <Servo.h>
#include <stdlib.h>
//#include <Wire.h> 
//#include <LiquidCrystal_I2C.h>
//LiquidCrystal_I2C lcd(0x27,16,2);

const int strSvrF = 10;  // front steering servo
const int strSvrR = 3;  // rear steering servo
const int driveS = 6;    // throttle controller
const int xIn = A2;
const int yIn = A3;
const int zIn = A4;

byte inBuf[3];  //data input buffer, [0]=0x30: left, 0x40: center, 0x50:right
          //[1]= stereing value, [2]= direction speed
byte inbyte;

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

void setup(){
  //lcd.init();
  //lcd.backlight();
  //lcd.clear();
  
  pinMode(strSvrF, OUTPUT);
  pinMode(strSvrR, OUTPUT);
  pinMode(driveS, OUTPUT);
  pinMode(xIn, INPUT);
  pinMode(yIn, INPUT);
  pinMode(zIn, INPUT);
  frontS.attach(strSvrF);
  rearS.attach(strSvrR);
  thrtl.attach(driveS);
  inBuf[0] = 0x40;
  inBuf[1] = 0x00;
  inBuf[2] = 0x50;

  center_str();  
  Serial.begin(38400);
  //38400 for pc with Xbee mod's
}

void loop(){
  int i;
    if(Serial.available()==3){
       for(i=0;i<3;i++){
          inBuf[i] = Serial.read();
       }
      
    }
  
  switch(inBuf[0]){
    case 0x30:
       steer_l(inBuf[1]);
       break;
    case 0x40:
       center_str();
       break;
    case 0x50:
       steer_r(inBuf[1]);
       break;
    default:
       center_str(); 
  }
  
  drive(inBuf[2]);
  
  //lcd.print(inBuf);
 
  
  
}

void check_tip(){
    
}


void center_str(){
  frontS.write(90);
  rearS.write(90);
}

void drive( int val){
  if((val<84)&&(val>76))
    val = 80;
   
  if(val<0)
     val = 0;
  if(val>180)
     val = 180;
    
  thrtl.write(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);
  
}

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);
  
}

/*
  int vl = analogRead(A0);
  if(vl<=512){
  vl = map(vl, 0, 512, 100, 0);
  steer_l(vl);
  }
  else if(vl>512){
    vl = map(vl, 513, 1023, 0 , 100);
    steer_r(vl);
  }
*/