Articulated Rover Using ArduPilot + Arduino

const int throttlePin = 42;  // ArduPilot Output 1
const int steeringPin = 43;  // ArduPilot Output 3

const int LPWM[4] = {6, 8, 10, 12};
const int RPWM[4] = {7, 9, 11, 13};
const int L_EN[4] = {44, 46, 48, 50};
const int R_EN[4] = {45, 47, 49, 51};

float speedScale = 0.0;

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

  for (int i = 0; i < 4; i++) {
    pinMode(LPWM[i], OUTPUT);
    pinMode(RPWM[i], OUTPUT);
    pinMode(L_EN[i], OUTPUT);
    pinMode(R_EN[i], OUTPUT);
    digitalWrite(L_EN[i], HIGH);
    digitalWrite(R_EN[i], HIGH);
  }

  pinMode(throttlePin, INPUT);
  pinMode(steeringPin, INPUT);
}

void loop() {
  int throttlePWM = pulseIn(throttlePin, HIGH, 25000);
  int steeringPWM = pulseIn(steeringPin, HIGH, 25000);

  Serial.print("Throttle PWM: ");
  Serial.print(throttlePWM);
  Serial.print(" | Steering PWM: ");
  Serial.println(steeringPWM);

  int throttle = map(throttlePWM, 1000, 2000, -255, 255) * speedScale;
  int steering = map(steeringPWM, 1000, 2000, -255, 255) * speedScale;

  int leftSpeed = throttle + steering;
  int rightSpeed = throttle - steering;

  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  driveMotor(0, leftSpeed);   // Front Left
  driveMotor(2, leftSpeed);   // Back Left
  driveMotor(1, rightSpeed);  // Front Right
  driveMotor(3, rightSpeed);  // Back Right

  delay(20);
}

void driveMotor(int index, int speed) {
  if (index == 3) speed = -speed; // Back Right fix

  if (speed >= 0) {
    digitalWrite(RPWM[index], LOW);  // Force LOW
    analogWrite(LPWM[index], speed);
  } else {
    digitalWrite(LPWM[index], LOW);  // Force LOW
    analogWrite(RPWM[index], -speed);
  }
}