// Elegoo Samrt Robot Car V3.0 - Test der Motorsteuerung per L 293D Modul
#include <Servo.h>
#include <IRremote.h>
Servo myservo;

const byte ENA = 5, IN1 = 7, IN2 = 8,  // Motor links
           ENB = 6, IN3 = 9, IN4 =11,  // Motor rechts
           Echo = A4, Trig = A5, ServoPin = 3, RECV_PIN = 12,
           vmax = 255; 

IRrecv irrecv(RECV_PIN);
decode_results results;

const unsigned long
  KEY_LINKS  = 16720605, KEY_HOCH   = 16736925,  KEY_RECHTS = 16761405,
  KEY_RUNTER = 16754775, KEY_OK =  16712445, 
  KEY_1 = 16738455,     KEY_2 = 16750695,   KEY_3 = 16756815,
  KEY_4 = 16724175,     KEY_5 = 16718055,   KEY_6 = 16743045,
  KEY_7 = 16716015,     KEY_8 = 16726215,   KEY_9 = 16734885,
  KEY_STERN = 16728765, KEY_0 = 16730805,   KEY_RAUTE = 16732845;           

byte v = 120;  // Geschwindigkeit

void setup() {
  myservo.attach(ServoPin);  // attach servo on pin 3 to servo object
  Serial.begin(9600); meldung("Start");
  analogWrite(ENA,0); analogWrite(ENB,0);
  pinMode(IN1, OUTPUT);  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);  pinMode(IN4, OUTPUT);
//pinMode(ENA, OUTPUT);  pinMode(ENB, OUTPUT);
  pinMode(Echo, INPUT);  pinMode(Trig, OUTPUT);  //US-Sensor
  irrecv.enableIRIn();      // Start receiving
}

void loop() {
  char zeichen = IR_Abfrage();    // Infrarot-Fernbedienung
  if (zeichen!=0) {
    Steuerung(zeichen);           // Befehl an die Steuerung
  } else {
    zeichen = Serielle_Abfrage(); // Befehle vom PC bzw. per Bluetooth
    if (zeichen!=0) Steuerung(zeichen); // Befehl an die Steuerung
  }
}

char Serielle_Abfrage() {
  byte zeichen=0;
  if (Serial.available() > 0)  {
    zeichen = Serial.read();
    if ((zeichen!=10)&&(zeichen!=13)) {
      meldung("Serieller Befehl:"+String(zeichen));
      return zeichen;
    }  
  } 
  return 0;
}

char IR_Abfrage() {
  char zeichen=0;
  if (irrecv.decode(&results)) {
    unsigned long val = results.value;
    String tastenname;
    switch (val) {
      case KEY_LINKS:  tastenname = "KEY_LINKS"; 
         
         break;
      case KEY_HOCH:   tastenname = "KEY_HOCH"; 
        v=v+20; if (v>255) v=255; meldung("Geschwindigkeit: "+String(v));
        break;
      case KEY_RECHTS: tastenname = "KEY_RECHTS"; break;
      case KEY_RUNTER: tastenname = "KEY_RUNTER"; 
        v=v-20; if (v<100) v=0; 
        meldung("Geschwindigkeit: "+String(v));
        meldung(" - Neue Geschwindigkeit wird erst mit neuem Fahrbefehl wirksam");
        break;
      case KEY_OK: tastenname = "KEY_OK"; break;
      case KEY_1: tastenname = "KEY_1"; zeichen='1'; break;
      case KEY_2: tastenname = "KEY_2"; break;
      case KEY_3: tastenname = "KEY_3"; break;
      case KEY_4: tastenname = "KEY_4"; break;
      case KEY_5: tastenname = "KEY_5"; break;
      case KEY_6: tastenname = "KEY_6"; break;
      case KEY_7: tastenname = "KEY_7"; break;
      case KEY_8: tastenname = "KEY_8"; break;
      case KEY_9: tastenname = "KEY_9"; break;
      case KEY_0: tastenname = "KEY_0"; zeichen='0'; break;
      case KEY_STERN: tastenname = "KEY_STERN"; break;
      case KEY_RAUTE: tastenname = "KEY_RAUTE"; break;
      default: tastenname = "unbekannt " + String(val);
    }
    if (val != 4294967295) {
      meldung("IR-Befehl: "+String(zeichen)+" per " + tastenname);
    }
    irrecv.resume();      // Receive the next value
    delay(150);
  }
  return zeichen;
}

void meldung(String mText) { // Info an PC bzw. Bluetooth-Gerät
  Serial.print("Meldung: ");
  Serial.println(mText);
}

int Distance_test() {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH); 
  Fdistance= Fdistance / 58;       
  return (int)Fdistance;
}  


void Steuerung(char zeichen) {
  if(zeichen == '0') { // Halt
    analogWrite(ENA, 0); digitalWrite(IN1, HIGH); digitalWrite(IN2, HIGH);
    analogWrite(ENB, 0); digitalWrite(IN3, HIGH); digitalWrite(IN4, HIGH);
    meldung("Halt");
  } else if(zeichen == '1') { // Langsam vorwaerts
    analogWrite(ENA, v); digitalWrite(IN1, HIGH); digitalWrite(IN2,  LOW);
    analogWrite(ENB, v); digitalWrite(IN3,  LOW); digitalWrite(IN4, HIGH);
    meldung("Langsam vorwaerts");
  } else if(zeichen == '2') { // Vollgas vorwaerts
    analogWrite(ENA, vmax); digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    analogWrite(ENB, vmax); digitalWrite(IN3,  LOW); digitalWrite(IN4, HIGH);
    meldung("Vollgas vorwaerts");
  } else if(zeichen == '3') { // Rechtskurve
    analogWrite(ENA, vmax); digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    analogWrite(ENB,    v); digitalWrite(IN3,  LOW); digitalWrite(IN4, HIGH);
    meldung("rechts vorwaerts");
  } else if(zeichen == 'e') { 
    meldung("Entfernung: "+String(Distance_test()));
  } else if(zeichen == 'r') { 
      myservo.write( 25);
      meldung("Servowinkel: "+String(15));
  } else if(zeichen == 'l') { 
      myservo.write(165);
      meldung("Servowinkel: "+String(165));
  } else if(zeichen == 'g') { 
      myservo.write( 90);
      meldung("Servowinkel: "+String(0));
  } else if(zeichen == ' ') { 
      myservo.detach();    
      analogWrite(ENA, 0); analogWrite(ENB, 0);
      meldung("+++ Alle Aktoren auf AUS +++");
  } else if(zeichen == 's') { 
      myservo.attach(ServoPin);    
      meldung("Servo wieder aktiv");
  }
}