Rover 5 RC WIP

int throttleVal; // Here's where we'll keep our channel values
int steerVal;
//int ch3;

const String terminalReset = "\x1b[2J\x1b[1;1H";

const int steerChannel = PN_3;
const int throttleChannel = PN_2;

int leftThrottle;
int rightThrottle;

const int leftMotorDir = PL_3;
const int leftMotorPWM = PM_5;
const int rightMotorDir = PL_2;
const int rightMotorPWM = PM_4;

const int leftEncA = PQ_1;
const int leftEncB = PP_3;
const int leftEncX = PQ_0;
const int rightEncA = PQ_3;
const int rightEncB = PQ_2;
const int rightEncX = PP_4;

unsigned long leftEncTicks = 0;
unsigned long rightEncTicks = 0;

void setup() {

  pinMode(steerChannel, INPUT); // Set our input pins as such
  pinMode(throttleChannel, INPUT);
  //pinMode(7, INPUT);

  pinMode(leftMotorDir, OUTPUT);
  pinMode(leftMotorPWM, OUTPUT);
  pinMode(rightMotorDir, OUTPUT);
  pinMode(rightMotorPWM, OUTPUT);

  pinMode(leftEncA, INPUT);
  pinMode(leftEncB, INPUT);
  pinMode(leftEncX, INPUT);
  pinMode(rightEncA, INPUT);
  pinMode(rightEncB, INPUT);
  pinMode(rightEncX, INPUT);

  Serial.begin(115200); // Pour a bowl of Serial
  attachInterrupt(leftEncX, leftEncoderInterrupt, CHANGE);
  attachInterrupt(rightEncX, rightEncoderInterrupt, CHANGE);
}

void loop() {
  Serial.print(terminalReset);
  throttleVal = pulseIn(throttleChannel, HIGH, 25000); // Read the pulse width of
  steerVal = pulseIn(steerChannel, HIGH, 25000); // each channel
  //ch3 = pulseIn(7, HIGH, 25000);
  if (steerVal < 750) {
    Serial.println("***ERROR***");
    Serial.println("*No Signal*");

    throttleVal = 0;
    steerVal = 0;
  }
  else {
    steerVal = convertPulseToSpeed(steerVal, 20, true);
    throttleVal = convertPulseToSpeed(throttleVal, 20, false);
  }
  leftThrottle = throttleVal + steerVal;
  rightThrottle = throttleVal - steerVal;
  leftThrottle = constrain(leftThrottle, -500, 500);
  rightThrottle = constrain(rightThrottle, -500, 500);
  Serial.print("Throttle Channel:"); // Print the value of
  Serial.println(throttleVal);        // each channel
  Serial.print("Steering Channel:");
  Serial.println(steerVal);
  Serial.println();
  Serial.print("Left Throttle: ");Serial.println(leftThrottle);
  Serial.print("Right Throttle: ");Serial.println(rightThrottle);
  Serial.print('\n');
  Serial.print("LTicks: ");Serial.println(leftEncTicks);
  Serial.print("RTicks: ");Serial.println(rightEncTicks);
  //SAFETY MODE
  leftThrottle = 0;
  rightThrottle = 0;

  setValues(leftThrottle, rightThrottle);
  delay(100); // I put this here just to make the terminal
              // window happier
}

int convertPulseToSpeed(int pulseWidth, int deadzone, boolean invertAxis) {
  pulseWidth = constrain(pulseWidth, 1000, 2000);
  pulseWidth = map(pulseWidth, 1000, 2000, -500, 500);
  if (invertAxis) {
    pulseWidth = -pulseWidth;
  }
  if ((pulseWidth > -deadzone) && (pulseWidth < deadzone)) {
    pulseWidth = 0;
  }
  return pulseWidth;
}

void setValues(int leftSpeed, int rightSpeed) {
  leftSpeed = map(leftSpeed, -500, 500, -255, 255);
  if (leftSpeed == 0) {
    digitalWrite(leftMotorDir, HIGH);
    analogWrite(leftMotorPWM, 0);
  }
  else if (leftSpeed > 0) {
    digitalWrite(leftMotorDir, HIGH);
    analogWrite(leftMotorPWM, leftSpeed);
  }
  else {
    digitalWrite(leftMotorDir, LOW);
    analogWrite(leftMotorPWM, -leftSpeed);
  }

  rightSpeed = map(rightSpeed, -500, 500, -255, 255);
  if (rightSpeed == 0) {
    digitalWrite(rightMotorDir, HIGH);
    analogWrite(rightMotorPWM, 0);
  }
  else if (rightSpeed > 0) {
    digitalWrite(rightMotorDir, HIGH);
    analogWrite(rightMotorPWM, rightSpeed);
  }
  else {
    digitalWrite(rightMotorDir, LOW);
    analogWrite(rightMotorPWM, -rightSpeed);
  }
}

void leftEncoderInterrupt(void) {
  leftEncTicks++;
}
void rightEncoderInterrupt(void) {
  rightEncTicks++;
}