/************************************************************************************************* 
                                      PROGRAMMINFO
************************************************************************************************** 
Funktion: NANO Roboterarmsteuerung
Vor dem Programm hochladen RTX und TXT abstecken!!!
NANO Shield 2022 V1 D0->TXT D1->RXT  
Mit HC05 verbinden

**************************************************************************************************
Version: 27.02.2022
**************************************************************************************************
Board: NANO V3
**************************************************************************************************
C++ Arduino IDE V1.8.13
**************************************************************************************************
Einstellungen:
https://dl.espressif.com/dl/package_esp32_index.json
http://dan.drown.org/stm32duino/package_STM32duino_index.json
https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
**************************************************************************************************/
#define MOTORANZAHL 6         // Gesamtzahl der Servomotoren

#include "Servo.h"
Servo Motor[MOTORANZAHL];     // erstelle ein Objekt für jeden Motor

byte Motorpin[MOTORANZAHL] = {2, 3, 4, 5, 6, 7};
byte Motorposition[MOTORANZAHL];
byte aktuellerMotor = 0, Position = 0, EingabeWinkel = 0;

void setup()
{
  Serial.begin(115200);
  for(byte i = 0; i < MOTORANZAHL; i++)
  {
    pinMode(Motorpin[i], OUTPUT);
    Motor[i].attach(Motorpin[i]);
  }
}

void loop()
{
  while(Serial.available()) {
    byte Eingabe = Serial.read();

    if(aktuellerMotor == 10)          // noch kein Motor gewählt
    {
      if(Eingabe >= '0' && Eingabe < MOTORANZAHL + '0')
      {
        aktuellerMotor = Eingabe - '0';
        continue;
      }
      if(Eingabe == 'i')
      {
        Serial.println("### aktuelle Positionswerte ###");
        for(byte i = 0; i < MOTORANZAHL; i++)
          Serial.println("Motor " + String(i) + ": " + String(Motor[i].read()));
        continue;
      }
    }

    if(aktuellerMotor < 10)           // Motor wurde bereits angewählt
    {
      if(Eingabe >= '0' && Eingabe <= '9')      // Eingabe ist eine Ziffer
      {
        EingabeWinkel = EingabeWinkel*10 + (Eingabe - '0');
        continue;
      }

      if(Eingabe == '+')              // Erhöhung des Winkels
      {
        if(EingabeWinkel == 0)
          EingabeWinkel = 1;
        Position = Motor[aktuellerMotor].read();
        if(180 - Position >= EingabeWinkel)
          Position += EingabeWinkel;
        else
          Position = 180;
        Motor[aktuellerMotor].write(Position);
        Serial.println("Motor " + String(aktuellerMotor) + " wurde auf Position " + String(Position) + " gestellt."); 
        EingabeWinkel = 0; aktuellerMotor = 10; 
        continue;
      } 
      if(Eingabe == '-')              // Verringerung des Winkels
      {
        if(EingabeWinkel == 0)
          EingabeWinkel = 1;    
        Position = Motor[aktuellerMotor].read();               
        if(Position >= EingabeWinkel)
          Position -= EingabeWinkel;
        else
          Position = 0;
        Motor[aktuellerMotor].write(Position);  
        Serial.println("Motor " + String(aktuellerMotor) + " wurde auf Position " + String(Position) + " gestellt.");  
        EingabeWinkel = 0; aktuellerMotor = 10;
        continue;
      }       
      
      if(EingabeWinkel <= 180)        // absoluter Eingabewinkel
      {
        Motor[aktuellerMotor].write(EingabeWinkel);  
        Serial.println("Motor " + String(aktuellerMotor) + " wurde auf Position " + String(EingabeWinkel) + " gestellt.");
        EingabeWinkel = 0; aktuellerMotor = 10;
        continue;
      }

      Serial.println("Ungueltige Eingabe!");
      EingabeWinkel = 0; aktuellerMotor = 10;   
    }
  }
}