// SB13 Klapptriebwerk

#include <Servo.h>

// Pinzuordungen
#define KLAPP 2
#define REGLER 5
#define RECEIVER_EINAUS A2
#define RECEIVER_GAS A0
#define ENDSCHALTER_AUS 9
#define ENDSCHALTER_EIN 7

// Pulsbreiten für Klappservo
#define AUSFAHREN 1100
#define EINFAHREN 1900
#define STOPP 1390

// Richtungsflag
#define RAUS 1
#define REIN 2
#define NO 0

// Globale Variablen
int richtung = NO; 
Servo klapp;
Servo regler;
unsigned long inputPulse;
unsigned long gasPulse;

void setup() {
  klapp.attach(KLAPP);
  regler.attach(REGLER);
  pinMode(RECEIVER_EINAUS, INPUT);
  pinMode(RECEIVER_GAS, INPUT);
  Serial.begin(9600);
  klapp.writeMicroseconds(STOPP);  // Auf jeden Fall mal abschalten
}

void loop() {
  gasPulse = pulseIn(RECEIVER_GAS, HIGH);
  inputPulse = pulseIn(RECEIVER_EINAUS, HIGH);

  // Auswertung des INputPulses vom Empfänger
  if (inputPulse > 1800) richtung = RAUS;
  else if (inputPulse < 1200) richtung = REIN;
  else richtung = NO;
  
  Serial.print(inputPulse);  // Debug only
  Serial.print("  "); 
  Serial.println(gasPulse);
  
  if ((richtung == RAUS) && (digitalRead(ENDSCHALTER_AUS) == HIGH)) {
    klapp.writeMicroseconds(AUSFAHREN);
  }
  if ((richtung == REIN) && (digitalRead(ENDSCHALTER_EIN) == HIGH)) {
    klapp.writeMicroseconds(EINFAHREN);
  }
  if ((digitalRead(ENDSCHALTER_EIN) == LOW) && (richtung == REIN)) {
    klapp.writeMicroseconds(STOPP);
  }
  if ((digitalRead(ENDSCHALTER_AUS) == LOW) && (richtung == RAUS)) { 
    klapp.writeMicroseconds(STOPP);
  }
  if (richtung == NO) klapp.writeMicroseconds(STOPP);

  // Reglerabschaltung, wenn nicht ausgefahren
  if (digitalRead(ENDSCHALTER_EIN) == LOW) {
     regler.writeMicroseconds(gasPulse);     
  } else {
    regler.writeMicroseconds(950);  // Kürzeste Pulsbreite
  }
  
}