// Gianluca Trentadue - interfaccia Arduino per
// gestione servocomando RC per attuazioni generiche
// in sala prova motori
String version ="1.1";

#include <Servo.h>
Servo myservo;  // Crea l'oggetto myservo dalla classe Servo

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

int val = 90;
int msgval = 90;    // variabile del valore letto dalla seriale
int minval = 0;     // variabile finecorsa al minimo
int maxval = 180;   // variabile finecorsa al massimo

int i = 0;      // variabile di appoggio per il led del watchdog

String message;

void setup() {
  myservo.attach(9);  // uscita per il servo settatta sul pin 9

  pinMode(LED_BUILTIN, OUTPUT);  // inizializza il led giallo onboard
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);

  lcd.init();  // inizializza il display e scrive la prima riga
  lcd.backlight();
  
   Serial.begin(115200);
   Serial.setTimeout(100);

   Serial.println("G32 servo driver - fw version "+version);

}

void loop() {
  i++;

  //message = Serial.readString();
  message = Serial.readStringUntil('#');
  
  if(message.substring(0,6) == "SETPOS") {
    msgval = message.substring(7,10).toInt();
    Serial.println("ANSPOS "+String(msgval));
  }
  if(message.substring(0,6) == "SETDPO") {
    msgval = 90;
    Serial.println("ANSPOS "+String(msgval));
  }
  if(message.substring(0,6) == "GETPOS") {
    Serial.println("ANSPOS "+String(val));
  }
  if(message.substring(0,6) == "GETVER") {
    Serial.println("ANSVER v"+version);
  }  

  minval = map(analogRead(A0), 0, 1023, 0, 88);
  maxval = map(analogRead(A1), 0, 1023, 92, 180);

  if (msgval <= minval) {
    val = minval;
  }
  else if (msgval >= maxval) {
    val = maxval;
  }
  else {
    val = msgval;
  }
  
  myservo.write(val);                 // imposta la posizione del servo

  lcd.setCursor(0, 0);
  lcd.print("min:");
  lcd.setCursor(4, 0);
  if (minval < 10) {
    lcd.print(minval);
    lcd.print(" ");
  } else {
    lcd.print(minval);
  }

  lcd.setCursor(9, 0);
  lcd.print("max:");
  lcd.setCursor(13, 0);
  if (maxval < 100) {
    lcd.print(maxval);
    lcd.print(" ");
  } else {
    lcd.print(maxval);
  }

  lcd.setCursor(0, 1);
  lcd.print("pos:");
  lcd.setCursor(4, 1);
  if (val < 10) {
    lcd.print(val);
    lcd.print("  ");
  } else if (10 <= val <= 99) {
    lcd.print(val);
    lcd.print(" ");
  } else {
    lcd.print(val);
  }

  lcd.setCursor(9, 1);
  lcd.print("cmd:");
  lcd.setCursor(13, 1);
  if (msgval < 10) {
    lcd.print(msgval);
    lcd.print("  ");
  } else if (10 <= msgval <= 99) {
    lcd.print(msgval);
    lcd.print(" ");
  } else {
    lcd.print(msgval);
  }

  if (i <= 25) {
    digitalWrite(LED_BUILTIN, HIGH);
  } else {
    digitalWrite(LED_BUILTIN, LOW);
    if (i >= 50) {
      i = 1;
    }
  }
}