#include <Servo.h>

int controlMode = 2; // 1 = transmitter, 2 = override

Servo steeringServo;
byte steeringRxPin = 3;
byte steeringControlPin = 9;
int steeringNeutral = 90;
int steeringPwmValue;
int steeringRxValue;
int steeringOverrideValue = -1;

Servo throttleMotor;
byte throttleRxPin = 5;
byte throttleControlPin = 10;
int throttleNeutral = 90;
int throttlePwmValue;
int throttleRxValue;
int throttleOverrideValue = -1;
void setup() {
    pinMode(steeringRxPin, INPUT);
    pinMode(throttleRxPin, INPUT);
    throttleMotor.attach(throttleControlPin);
    steeringServo.attach(steeringControlPin);

    setThrottle(90); 
    setSteering(90);
    
    Serial.begin(9600);
}

void loop() {
  throttlePwmValue = pulseIn(throttleRxPin, HIGH);
  throttleRxValue = map(throttlePwmValue, 890, 2053, 0, 180);
  Serial.print("Throttle: ");
  Serial.println(throttleRxValue);
  steeringPwmValue = pulseIn(steeringRxPin, HIGH);
  steeringRxValue = map(steeringPwmValue, 990, 1993, 0, 180);
  Serial.print("Steering: ");
  Serial.println(steeringRxValue);

  if (controlMode == 1) {
    // Controlling with Transmitter
    setThrottle(throttleRxValue);
    setSteering(steeringRxValue);
  } else if (controlMode = 2) {
//    setThrottle(throttleOverrideValue);
//    }
  } else {
    
  }
  
  delay(2000);
}
void setThrottle(int speed) {
  Serial.print("Setting Throttle to: ");
  Serial.println(speed);
  
  throttleMotor.write(speed);
}

void setSteering(int speed) {
  Serial.println("Setting Steering to: ");
  Serial.println(speed);
  steeringServo.write(speed);
}