//
// This version of the simulation included code with converts the pot inputs
// into the channel 1 and 2 inputs that reflect the actual outputs from
// the custom configured Flysky FS-i6X transmitter. 
//

#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;

int uncorrectedCh1;
int uncorrectedCh2;

int newCh1;
int newCh2;


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

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


  Serial.print("Unannel 1 = ");
  Serial.print(uncorrectedCh1);
  Serial.print("    Unannel 2 = ");
  Serial.println(uncorrectedCh2);
    
  if ((uncorrectedCh2 >= 1460) || (uncorrectedCh2 <= 1530)) {
     newCh1 = uncorrectedCh1;
     newCh2 = uncorrectedCh1;  
  }
  
  if (uncorrectedCh2 > 1530) {
    if (uncorrectedCh1 > 1500) {
      newCh1 = uncorrectedCh1;
    } else {
      newCh1 = map(uncorrectedCh1, 1000, 1500, 1000, 2000);
    } 
  }


  if (uncorrectedCh2 < 1460) {
    if (uncorrectedCh1 < 1500) {
      newCh1 = map(uncorrectedCh2, 1000, 1500, 1000, 2000);
    } else {
      newCh1 = uncorrectedCh1;
    }   
  }
  
  /*
    
  if (uncorrectedCh2 > 1500) {
    
    if (uncorrectedCh1 > 1500) {
      newCh1 = uncorrectedCh1;
    } else {
      newCh1 = map(uncorrectedCh2, 1500, 1000, 2000, -2000);
    } 
    
    if (uncorrectedCh1 < 1500) {
      newCh1 = map(uncorrectedCh2, 1500, 2000, -2000, 2000);
    } else {
      newCh1 = uncorrectedCh1;
    } 
  } 

  */

  ch1 = newCh1;
  ch2 = newCh2;

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