/*
 RC PulseIn Joystick Servo Control
 By: Nick Poole
 SparkFun Electronics
 Date: 5
 License: CC-BY SA 3.0 - Creative commons share-alike 3.0
 use this code however you'd like, just keep this license and
 attribute. Let me know if you make hugely, awesome, great changes.
 */

const int ch1_pin = 10 ;
const int ch2_pin = 11 ;

int ch1; // Here's where we'll keep our channel values
int ch2;
int ch3;

int move; // Forward/Back speed
int turn; // Turning Factor

int pwm_a = 4;  //PWM control for motor outputs
int pwm_b = 7;  //PWM control for motor outputs
int dir_a = 5;  //direction control for motor outputs
int dir_b = 6;  //direction control for motor outputs

int threshold = 30 ;


int middleThreshold (int RCvalue, int threshold) {  // 

  if ( (RCvalue < 1500 + threshold) && (RCvalue > 1500 - threshold) ) { RCvalue = 1500;}
  return RCvalue ;
  
}

void setup() {

pinMode(ch1_pin, INPUT); // Set our input pins as such
pinMode(ch2_pin, INPUT);

Serial.begin(9600); // Pour a bowl of Serial (for debugging)

  pinMode(pwm_a, OUTPUT);  //Set control pins to be outputs
  pinMode(pwm_b, OUTPUT);
  pinMode(dir_a, OUTPUT);
  pinMode(dir_b, OUTPUT);

  analogWrite(pwm_a, 0);  
  analogWrite(pwm_b, 0);
}


void loop() {
  
  ch1 = pulseIn(ch1_pin, HIGH, 25000); // Read the pulse width of  
  ch2 = pulseIn(ch2_pin, HIGH, 25000); // each channel

  ch1 = middleThreshold(ch1, threshold);
  ch2 = middleThreshold(ch2, threshold);
  

  move = map(ch2, 1000,2000, -255, 255); //center over zero
  move = constrain(move, -255, 255); //only pass values whose absolutes are
                                   //valid pwm values

  /*What we're doing here is determining whether we want to move
  forward or backward*/
  if(move>0){digitalWrite(dir_a, 1);digitalWrite(dir_b, 1);};
  if(move<0){digitalWrite(dir_a, 0);digitalWrite(dir_b, 0); move=abs(move);};
  
  /*Here we're determining whether a left or a right turn is being 
  executed*/
  turn = map(ch1,1000,2000,-255,255);
  turn = constrain(turn, -255, 255);
  
  /*This is where we do some mixing, by subtracting our "turn" 
  variable from the appropriate motor's speed we can execute
  a turn in either direction*/
  if(turn>=0){analogWrite(pwm_b, move-turn); analogWrite(pwm_a, move);};
  if(turn<0){turn=abs(turn); analogWrite(pwm_a, move-turn); analogWrite(pwm_b, move);};
  
  
  Serial.print("move:"); //Serial debugging stuff
  Serial.println(move);
  
  Serial.print("turn:"); //Serial debugging stuff
  Serial.println(turn);
  
  Serial.print("move-turn:"); //Serial debugging stuff
  Serial.println(move-turn);
  
  Serial.println(); //Serial debugging stuff
  Serial.println();
  Serial.println();

}