/*
 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 move; // Forward/Back speed
int turn; // Turning Factor
int speeds_pin[2] = {5, 6};
int dirs_pin[2] = {4, 7};
//int pwm_a = 5;  //PWM control for motor outputs
//int pwm_b = 6;  //PWM control for motor outputs
//int dir_a = 4;  //direction control for motor outputs
//int dir_b = 7;  //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)

  for(int i; i<2; i++){
    pinMode(speeds_pin[i], OUTPUT);
    pinMode(dirs_pin[i], OUTPUT);
    analogWrite(speeds_pin[i], 0) ;
  }
//  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() {

  //Read RC values and give wide center point
  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);
  
  //Refresh and scale raw move values
  move = map(ch2, 1000,2000, -255, 255); //center over zero
  move = constrain(move, -255, 255); //only pass values whose absolutes are
                                   //valid pwm values
  turn = map(ch1,1000,2000,-255,255);
  turn = constrain(turn, -255, 255);

  //calculate each motor speed
  int speeds[2] = {0, 0} ;
  int dirs[2] = {0, 0} ;
  
  if (turn>0) { speeds[0] = move; speeds[1] = move - turn; }
  else {speeds[0] = move-turn; speeds[1] = move ;}

  //apply to driver
  for(int i =0; i<2; i++) {
    dirs[i] = speeds[i] > 0 ? 1 : 0 ;
    speeds[i] = abs( speeds[i]);
    digitalWrite(dirs_pin[i], dirs[i]) ;
    analogWrite(speeds_pin[i], speeds[i]) ;
    
  }

//  analogWrite (
// 
//  
//  /*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);};
////  analogWrite(pwm_b, move); analogWrite(pwm_a, 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();

}