#include <Servo.h>
//#define debug
#include <debug.h>
#include <makros.h>
#include <RCReceive.h>
/*
  SlowServo.ino - Servoverlangsamung  - Version 0.1
 
 Copyright (c) 2012 Wilfried Klaas.  All right reserved.
 
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
/*
 Servoverlangsamung. 
 Der Empfänger wird vom Pin 2 gelesen, 
 der Servo ausgang sind auf Pin 9. Zum Einstellen der Werte dienen 2 Poties an den analogen Eingängen 0 und 1.
 
 Progbrammablauf:
 Der Nullpunkt wird zunächst durch Interpolation der ersten 10 Werte vom Empfänger festgestellt.
 Fehlerhafte Werte, also Werte ausserhalb von 900-2100 werden ignoriert. Werden mehr als 3 fehlerhafte Werte hintereinander gefunden, 
 wird der Servo in die Min Position gefahren. (Failsafe) 
 */

const byte PIN_RC = 2;

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

// Eingänge 
const byte PIN_MODE = 0;

// Eingänge (analogen Eingänge)
const byte PIN_MIN = 0;
const byte PIN_MAX = 1;

// Stellgeschwindigkeit
const byte DELTA = 10;
const byte DELAY = 0;

// ********** Variablen **********
RCReceive rcReceiver;
Servo servo;

// Zwischenspeicher
int myRcValue;
byte maxServo;
byte minServo;

void setup() {
  pinMode(SERVO, OUTPUT);     
  pinMode(PIN_MODE, INPUT_PULLUP);     

  analogReference(DEFAULT);

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

void loop() {
  // Lesen der aktuellen Impuls-Einstellungen von den Kanälen
  myRcValue = analogRead(PIN_MIN);
  minServo = map(myRcValue,0, 1024,0,180);

  myRcValue = analogRead(PIN_MAX);
  maxServo = map(myRcValue,0, 1024,0,180);

  // Aktuellen RC-Wert lesen
  rcReceiver.poll();

  // Fehlerfall
  if (rcReceiver.hasError()) {
    // failsafe ...
    servo.write(minServo);
  } 
  else if (rcReceiver.hasNP()) {
    doWork();
  } 
  else {
    if (!servo.attached()) {
      servo.attach(SERVO);
    }
    servo.write(minServo);
  }
}

void doWork() {
  byte sValue = rcReceiver.getValue();
  sValue = map(sValue, 0, 255, minServo ,maxServo);

  byte aValue = servo.read();
  if (sValue != aValue) {
    int hValue = aValue + DELTA;
    if (hValue > maxServo) {
      hValue = maxServo;
    }
    int lValue = aValue - DELTA;
    if (lValue < minServo) {
      lValue = minServo;
    }
    byte dValue = constrain(sValue, lValue, hValue);
    servo.write(dValue);
    if (DELAY > 0) {
      delay(DELAY);
    }
  }
}