#include <RCReceive.h>
#include <debug.h>
#include <makros.h>
#include <Servo.h>


/*
 Servorevers und Servowegbegrenzung. Der Empfänger wird vom Pin 2.
*/

// Hardwareanbindung für Arduino Hardware, Empfängerkanäle
const byte PIN_RC = 2;

// Ausgänge
const byte SERVO = 9; // PWM Kanal 
const byte LED = 13; // LED auf dem Board

// Wir halbieren mal den Servoweg...
const byte MAX_SERVO = 45;
const byte MIN_SERVO = 135;

// und wir reversieren oder auch nicht
const boolean REVERS = true;

// ------------------------------------------------------------
// AB HIER BITTE NIX MEHR ÄNDERN
// ------------------------------------------------------------

RCReceive rcReceiver;

Servo servo;

void setup() {
  // LED Kanal auf Ausgang, und dann deaktivieren
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);

  // Eingang für RC
  rcReceiver.attach(PIN_RC);

  // Servo definieren
  servo.attach(SERVO);
}

void loop() {

  // und gleich auch für's Steuerservo
  rcReceiver.poll();

  // Nullpunktsbestimmung ?
  if (rcReceiver.hasNP()) {
    doWork();
  }
}

/*
Modus Soft berechnung, Werte werden jeweils halbiert und dann addiert.
 */
void doWork() {
  // Werte holen
  int servo1 = rcReceiver.getValue();;

  // Nullpunkte festlegen
  servo1 = servo1 - rcReceiver.getNP();

  // die eigentliche Berechnung
  if (REVERS) {
    servo1 = -1 * servo1;
  }

  // Und damit das nicht aus dem Ruder läuft, etwas begrenzen
  servo1 = constrain(servo1, -128, 128);

  // Nullpunkte wieder hinzu
  servo1 = servo1 + 128;

  // Auf den Servo umsetzen und begrenzen
  servo1 = map(servo1, 0, 255, MIN_SERVO, MAX_SERVO);

  // und einstellen
  servo.write(servo1);
}