#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);
  inputA1 = analogRead(A1);

  ch1 = map(inputA0, 0, 1023, 1000, 2000);    // scale to RC receiver range 1000..2000
  ch2 = map(inputA1, 0, 1023, 2000, 1000);    // 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) {
  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);
  }
}