// The "define" command enables me to write something (for example, "turn1") and have the program read that as a different symbol (such as "3").
// It's not necessary, but it makes my job easier by not requiring me to remember which pins control which functions.
// Every one of these numbers is a pin on the arduino that I will be using for input or output.
#define turn1 3
#define speed2 5
#define turnSwitch3 10
#define servoPinR 6
#define servoPinL 9

// We now include the servo library. Although we are not driving any servos in this project, the ESCs accept Servo-style PWM signals.
#include <Servo.h>

// Then we define our "servos," which are really the ESCs that control the right and left motors.
Servo servoR;
Servo servoL;

// This is simply setting up all of the variables we will be using later on. The only variable that already has a value is servoSpeed, which starts off at 60 (motors are at rest).
int speedSignal;
int servoSpeed = 60;
int angleSignal;
int servoAngle;
int direction;
int servoRspeed;
int servoLspeed;

void setup() {
  // Before we can read inputs or send outputs to our pins, we must define each as an input or output pin. Here you'll see the terms we defined earlier being used in place of pin numbers.
  pinMode(turn1, INPUT);
  pinMode(speed2, INPUT);
  pinMode(turnSwitch3, INPUT);
  
  // We must also let the arduino know which pins to send the PWM signals to, which were also defined previously.
  servoR.attach(servoPinR);
  servoL.attach(servoPinL);
}

// Everything in this loop will happen repeatedly many times a second.
void loop() {

  
  // The receiver sends the arduino a pulse that tells us how much the controller is being turned. To interpret the meaning of this pulse, we use a pulseIn() command.
  // The first term is the pin, and the second is the type of pulse we're looking for (we're looking for a HIGH signal as opposed to a LOW signal).
  // Lastly, the third term is the length of time we will check for the pulse in microseconds. Through testing, I found 30000 to be the lowest value that works without errors.
  int angleSignal = pulseIn(turn1, HIGH, 30000);
  
  // Now that we have the raw pulse data, we must turn it into something we can send to the ESC and control the motors with.
  // To convert this, we will use a map() function to get a value that's witin one to ten. Later on, we'll subtract this value from one of the motors to make it turn slower.
  // This will cause the prototype to turn in that direction due to the difference in speeds.
  int servoAngle = map(angleSignal, 1480, 1010, 0, 10);

  // The receiver also sends us data about the speed setting of the controller. We'll use the same pulseIn() function to read this data.
  int speedSignal = pulseIn(speed2, HIGH, 30000);

  // Through testing, I found that the receiver sent a signal with a length of >1950 microseconds when the trigger on the controller was fully on.
  // Therefore, when this signal is being sent, the speed of the motors increase continuously by 5 at a time.
  // However, the highest speed the motors can reach is a PWM signal of 110, which is why I capped the increase at that value.
  if (speedSignal > 1950 && servoSpeed <= 105) {
    servoSpeed += 5;
  }

  // I also found that the receiver sent a signal with a length of <1030 microseconds when in the neutral position.
  // Additionally, the motors stop turning when we send them a PWM signal of 60 or below.
  // Therefore, this code makes the speed decrease by 5 continuously when the trigger is neutral until the motors come to a stop at 60.
  if (speedSignal < 1030 && servoSpeed >= 65) {
    servoSpeed -= 5;
  }

  // For some reason, the controller I bought inputs a gradual signal when the turn switch is being pulled in one direction, but the signal is all-or-nothing in the other direction.
  // To fix this, I decided to use another button to put the prototype into 3 modes: forward, right, and left.
  // This variable stores the value of that button so I know which mode the prototype is in.
  int direction = pulseIn(turnSwitch3, HIGH, 30000);

  // These lines set the default speeds of both motors. Unless the prototype is in right-turn or left-turn mode, both motors will simply move at the same speed.
  servoRspeed = servoSpeed;
  servoLspeed = servoSpeed;

  // When the button is switched to left-turn mode, the receiver ouputs a value of <=1050 microseconds. When this happens, we make left motor slower to initiate a turn.
  // The amount we slow it down by is equal to the angle value which we mapped above. If the servo is in left-turn mode but not being turned, it will still move straight.
  if (direction <= 1050) {
    servoLspeed = servoSpeed - servoAngle;
  }

  // This does the same function, but for right-turn mode.
  if (direction >= 1950) {
    servoRspeed = servoSpeed - servoAngle;
  }

  // Now that we have our values for the speed of each motor, we simply tell the ESCs these values and make the motors move.
  servoR.write(servoRspeed);
  servoL.write(servoLspeed);

  // Finally, we add a little delay before starting again so that the motors have time to accelerate to their new speeds.
  delay(500);