#include <Servo.h>

Servo leftTrack;  // create servo object to control a servo
Servo rightTrack;  // create servo object to control a servo

int inputA0;
int inputA1;
int ch1;
int ch2;

void setup() {
  Serial.begin(115000);

  leftTrack.attach(8);    // attach left to pin 8
  rightTrack.attach(9);  // attach right to pin 9
}

void loop() {
  
  inputA0 = analogRead(A0);                    // Forward - Reverse
  inputA1 = analogRead(A1);                    // Left - Right

  ch1 = map(inputA0, 0, 1023, 2000, 1000);    // scale to RC receiver range 1000..2000
  ch2 = map(inputA1, 0, 1023, 1000, 2000);    // scale to RC receiver range 1000..2000

  Serial.print("Channel 1 = ");
  Serial.print(ch1);
  Serial.print("    Channel 2 = ");
  Serial.print(ch2);
  
  controlCrawler(ch1, ch2); 

  delay(1000);
}

void motor(int track, int speed) {
  
  int val;

  if (track == 1) {
    val = map(speed, -127, 127, 180 , 0);
    rightTrack.write(val);
    Serial.print("    Right Speed = ");
    Serial.print(speed);
  }
  
  if (track == 2) {
    val = map(speed, -127, 127, 0 , 180);
    leftTrack.write(val);
    Serial.print("    Left Speed = ");
    Serial.println(speed);
  }
} 

void controlCrawler(int ch1, int ch2) { 
  int speedForward;   
  int speedBackward; 
  int LeftPercentage; 
  int RightPercentage;  
  int speedLeftTrack = 0;
  int speedRightTrack = 0;
   
  if (ch1 > 1530) {
    // Move in forward direction
    speedForward = map(ch1, 1530, 2000, 0, 127);   
    if(ch2 > 1530) {
      // Turn right forward
      RightPercentage = map(ch2, 1530, 2000, 100, 0);
      speedRightTrack = speedForward * (RightPercentage / 100.0); 
      speedLeftTrack = speedForward;      
    } else if (ch2 < 1470) {
      // Turn left forward
      LeftPercentage = map(ch2, 1000, 1470, 0, 100);
      speedLeftTrack = speedForward * (LeftPercentage / 100.0); 
      speedRightTrack = speedForward;       
    } else {
    	// Go straight forward
    	speedLeftTrack = speedForward; 
      speedRightTrack = speedForward; 
    } 
  } else if (ch1 < 1470) {
    // Move in reverse direction  
    speedBackward = map(ch1, 1000, 1470, -127, 0);   
    if(ch2 > 1530) {
      // Turn right backwards
      RightPercentage = map(ch2, 1530, 2000, 100, 0);
      speedRightTrack = speedBackward * (RightPercentage / 100.0); 
      speedLeftTrack = speedBackward;      
    } else if (ch2 < 1470) {
      // Turn left backwards
      LeftPercentage = map(ch2, 1000, 1470, 0, 100);
      speedLeftTrack = speedBackward * (LeftPercentage / 100.0); 
      speedRightTrack = speedBackward;       
    } else {
    	// Go straight backwards
    	speedLeftTrack = speedBackward; 
      speedRightTrack = speedBackward; 
    }  
  } else {
    // Stop and don't move
    speedLeftTrack = 0; 
		speedLeftTrack = 0; 
  }
  	
  motor(1, speedRightTrack);
  motor(2, speedLeftTrack);

}

/*

void controlCrawler(int ch1, int ch2) { 
  int speedForward;   
  int speedReverse; 
  int speedLeftTrack;
  int speedRightTrack;
  int LeftPercentage; 
  int RightPercentage;  
   
  if (ch1 > 1530) {
    // Move in forward direction
    speedForward = map(ch1, 1530, 2000, 0, 127);   
    if(ch2 > 1530) {
      // Turn right forward
      RightPercentage = map(ch2, 1530, 2000, 100, 0);
      speedRightTrack = speedForward * (RightPercentage / 100.0); 
      speedLeftTrack = speedForward;      
    } else if (ch2 < 1470) {
      // Turn left forward
      LeftPercentage = map(ch2, 1000, 1470, 0, 100);
      speedLeftTrack = speedForward * (LeftPercentage / 100.0); 
      speedRightTrack = speedForward;       
    } else {
    	// Go straight forward
    	speedLeftTrack = speedForward; 
      speedRightTrack = speedForward; 
    } 
  } else if (ch1 < 1470) {
    // Move in reverse direction  
 
    speedLeftTrack = 0; 
		speedLeftTrack = 0; 
  
  } else {
    // Stop and don't move
    speedLeftTrack = 0; 
		speedLeftTrack = 0; 
  }
  	
  motor(1, speedRightTrack);
  motor(2, speedLeftTrack);

}

*/

/*

void controlCrawler(int ch1, int ch2) {
  if (ch1 > 1530 && ch2 > 1530) {
    int speedRight = map(ch1, 1530, 2000, 0, 127);
    int speedLeft = map(ch2, 1530, 2000, 0, 127);
    motor(1, min(speedRight, speedLeft));
    motor(2, min(speedRight, speedLeft));
  } else if (ch1 > 1530 && ch2 < 1460) {
    int speedRight = map(ch1, 1000, 2000, 0, 127);
    int speedLeft = map(ch2, 1000, 2000, -127, 0);
    motor(1, speedRight);
    motor(2, speedLeft);
  } else if (ch1 < 1460 && ch2 > 1530) {
    int speedRight = map(ch1, 1000, 2000, -127, 0);
    int speedLeft = map(ch2, 1000, 2000, 0, 127);
    motor(1, speedRight);
    motor(2, speedLeft);
  } else if (ch1 < 1460 && ch2 < 1460) {
    int speedRight = map(ch1, 1000, 1460, -127, 0);
    int speedLeft = map(ch2, 1000, 1460, -127, 0);
    motor(1, max(speedRight, speedLeft));
    motor(2, max(speedRight, speedLeft));
  } else {
    motor(1, 0);
    motor(2, 0);
  }
}

*/