/*
  Analog input, analog output, serial output

  Reads an analog input pin, maps the result to a range from 0 to 255 and uses
  the result to set the pulse width modulation (PWM) of an output pin.
  Also prints the results to the Serial Monitor.

  The circuit:
  - potentiometer connected to analog pin 0.
    Center pin of the potentiometer goes to the analog pin.
    side pins of the potentiometer go to +5V and ground
  - LED connected from digital pin 9 to ground through 220 ohm resistor

  created 29 Dec. 2008
  modified 9 Apr 2012
  by Tom Igoe

  This example code is in the public domain.

  https://www.arduino.cc/en/Tutorial/BuiltInExamples/AnalogInOutSerial
*/
#include <Servo.h>

// These constants won't change. They're used to give names to the pins used:
const int analogInPin = A0;  // Analog input pin that the potentiometer is attached to
const int analogOutPin = 9;  // Analog output pin that the LED is attached to

Servo myservo;  // create servo object to control a servo

int pos = 0;    // variable to store the servo position

int sensorValue = 0;  // value read from the pot
int outputValue = 0;  // value output to the PWM (analog out)
int speedVal = 0;     // PWM value to output to the speed controller

String inputString = "";      // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete

void setup() {
  // initialize serial communications at 9600 bps:
  Serial.begin(9600);

  Serial.println("1 = Read Pot, 2 = Set ESC");

  // reserve 200 bytes for the inputString:
  inputString.reserve(200);
  
  // attaches the servo on pin 8 to the servo object
  myservo.attach(8);  
}

void loop() {
// print the string when a newline arrives:
  if (stringComplete) {
    Serial.println(inputString);

    if (inputString == "1\n") {
      Serial.println("Reading Pot");
      readAnalog();
    }

    if (inputString == "2\n") {
      Serial.println("Setting ESC");
      setSpeedController();
    }

      //readAnalog();
      //setSpeedController();

    // clear the string:
    inputString = "";
    stringComplete = false;
  }
}

void setSpeedController() {
  speedVal = map(sensorValue, 0, 1023, 0, 180);
  myservo.write(speedVal); 
}

void readAnalog() {
  // read the analog in value:
  sensorValue = analogRead(analogInPin);
  // map it to the range of the analog out:
  outputValue = map(sensorValue, 0, 1023, 0, 255);
  // change the analog out value:
  analogWrite(analogOutPin, outputValue);

  // print the results to the Serial Monitor:
  Serial.print("sensor = ");
  Serial.print(sensorValue);
  Serial.print("\t output = ");
  Serial.println(outputValue);

  // wait 2 milliseconds before the next loop for the analog-to-digital
  // converter to settle after the last reading:
  delay(2);
}

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}