/* Schrankensteuerung V0.5DCC Stepmotorversion 23.10.2015
 *  -- Version mit Schrittmotoren und Nachwippen
 *  Die Schrankensteuerung benötigt die 'MobaTools' - library ab der Version 0.5
 *  und die NmraDcc Lib in der Version ohne Nutzung des Timer0
 *  
 *  V0.5DCC Version mit Ansteuerung über DCC. Der Schranke wird eine Lokadresse zugeordnet.
 *  F0 bestimmt den Zustand der Schranke (Auf/Zu)
 *  F1-F4 aktivieren die Justierung der Schrankenbäume 1-4. Im jeweiligen Zustand (Auf/Zu) kann dann
 *  die Lage der Schranke über den Fahrregler eingestellt werden. Mit dem Ausschalten der Funktion
 *  wird die dann aktive Position des Schrankenbaums abgespeichert.
 *  
 *  V0.4S Version mit Schrittmotoren und Nachwippen. Der Referenzpunkt des Schrittmotors wird über
 *  eine Segmentscheibe und Lichtschranke realisiert, und muss im Bewegungsbereich der Schranke liegen.
 *   
 *  Für eine bessere Übersicht und Erweiterbarkeit ist das Programm logisch in einzelen Blöcke aufgeteilt.
 *  Diese Blöcke sind im loop hintereinander angeordnet, arbeiten aber weitgehend unabhängig. Damit dies
 *  möglich ist, dürfen innerhalb der Blöcke keine Warteschleifen/Delays werwendet werden, die den 
 *  Programmablauf temporär anhalten.
 *  
 *  1.Block: Einschaltlogik. 
 *  Hier wird bestimmt, ob die Schranke geschlossen oder geöffnet werden soll. Derzeit ist dies einfach ein
 *  Schaltereingang, der abgefragt wird. Soll die Schranke später automatisch durch die Züge gesteuert werden
 *  muss dies in diesem Block eingerichtet werden. Ergebnis der Einschaltlogik ist ein Flag 'schrankeSchliessen'
 *  
 *  2. Block Ablaufsteuerung Schrankenlogik
 *  zentraler Block, der den Ablauf des Schrankenschliessens bzw -öffnens steuert. Der Block agiert abhängig
 *  von dem Flag 'schrankeSchliessen' und dem momentanen Zustand der Schrankenlogik
 *  Hier werden auch die Flags gesetzt, mit denen die Glocke (glAktiv) und der Wechselblinker (wbAktiv)
 *  ein- bzw ausgeschaltet werden.
 *  
 *  3. Block Ansteuerung der Glocke
 *  abhängig vom Flag 'glAktiv' wird der Impulsausgang für die Glocke ein- bzw ausgeschaltet. Je nach ange-
 *  schlossenem Audio-Modul muss gegebenenfalls auch darauf geachtet werden, dass der letzte Glockenschlag 
 *  nicht abgeschnitten wird.
 *  
 *  4. Block Wechselblinker
 *  abhängig vom Flag 'wbAktiv' wird der Wechselblinker ein- bw ausgeschaltet. Beim Einschalten sind kurz beide 
 *  Blinker gleichzeitig an, bevor sie dann abwechselnd blinken.
 *  
 */
 
#include <MobaTools.h>      
#include <NmraDcc.h>        // es wird die Variante ohne Timer0 - Nutzung benötigt
 
#define DCC_ADDR       9999 // bis 99 wird kurze Adressierung verwendet, ab 100 die lange
#define DEBUG ;             // Wenn dieser Wert gesetzt ist, werden Debug ausgaben auf dem ser. Monitor ausgegeben
 
// gegebenenfalls anzupassende Werte (weitere Werte können im Abschnitt 'Portzuordnungen und Konstante' angepasst werden)
#define GLOCKE_ZYK  1000    // Glockenzyklus und Impulslänge muss an das angeschlossene Soundmodul
#define GLOCKE_PULS 200     // angepasst werden.
 
////////////////////////////////////////////////////////////////////////
//////////////// Portzuordnungen und Konstante /////////////////////////
// 1. Einschaltlogik --------------------------------------------------
// in dieser Version wird die Schranke über DCC-Signale gesteuert
const byte DccInputP = 2 ;                  // muss ein IRQ-fähiger Port sein (2 oder 3 auf Uno+komp. )
 
// 2. Ablaufsteuerung --------------------------------------------------
// Antriebsmotore ( für 2 Schrankenbäume, bei 4 Schrankenbäumen müssen die Arrays entsprechend erweitert werden )
const byte StepPort[] = {SPI_1,SPI_2};      // Step-Motore über SPI-Schieberegister angeschlossen
const byte refLsP[]    = {A3,A2};           // Anschluß Lichtschranke für Referenzpunkt des Schrittmotors
const byte SCHRANKENZAHL = sizeof( refLsP );   // Zahl der Schrankenbäume 
const int8_t RichtungZu[] = { 1, 1 };       // Drehrichtung des Motors beim Schliessen der Schranke ( +1 oder -1 )
const byte refLsZu[] = { HIGH, HIGH };      // LS-Eingang im Bereich der geschlossenen Schranke
#define ENDPOS_INIT    20                   // Initiale Winkellage (+/- vom Refpunkt) für die Endpositionen. 
                                            // Die Werte können über die Justierung angepasst werden
const int steps360 = 4096;                  // Schritte für eine Umdrehung
const byte schrTempo[] = { 20,21 };
const int schrVerzZU[] = { 10, 500 };       // Verzögerung für die Schrankenbewegung (ms)
                                            // damit laufen die Schranken nicht exakt gleichzeitig los.
                                            // bei 4 Schrankenbäumen kann dies genutzt werden, um die in Auto-Fahrtrichtung
                                            // hinteren Schranken später schliessen zu lassen
 
const int vorlaufZu     = 6000;             // Vorlaufzeit: Glocke und Wechselblinker aktiv, Schranke noch ruhend
const int nachlaufZu    = 2000;             // Nachlaufzeit: Schranke zu, Glocke nochaktiv
 
// Definitionen für das Nachwippen
#define WIPPBEREICH 5
byte wippIx[SCHRANKENZAHL];                 // Index in die Tabelle für die Nachwippbewegungen
int  wippPos[] = {-20,0,-10,0,-5,0};        // Wipppunkte in 1/10 °, gerechnet von Endpunkt
byte wippSpeed[] = {12,0,6,0,3,0};          // Tempo an den Wippunkten. 0 = keine Änderung
const byte wippPnts = sizeof(wippPos)/sizeof(int); // Zahl der Wipppunkte
 
// 3. Glocke -------------------------------------------------------------
const byte glockeP      = 4;                // Impulsausgang zur Ansteuerung einer Glocke
const int glZykl        = GLOCKE_ZYK;       // Glockenrythmus
const int glImp         = GLOCKE_PULS;      // Impulslänge am Glockenausgang
 
// 4. Wechselblinker ----------------------------------------------------
const byte led1P        = 6;        // Ausgänge für den Wechselblinker ( Ports müssen PWM-fähig sein )
const byte led2P        = 5;
const int wbZykl        = 1100;     // Zykluszeit des Wechselblinkers
const int wbSoft        = 300;      // Auf/Abblendzeit der Lampen
 
// sonst. ----------------------------------------------------------------
const byte InitJustageP  = 7;        // Pin LOW:  Justage der Endlagen möglich
                                    // Pin LOW beim Programmstart: Alle Werte initiieren
byte justageMode;
 
////////////////////// globale Variable //////////////////////////////////////
// 1. Einschaltlogik ----------------------------------------------------
bool schrankeSchliessen;            // Wird über DCC, F0 gesetzt
 
NmraDcc DCC;
// 2. Ablaufsteuerung ---------------------------------------------------
// Aufteilung der CV-Variablen (Standardadressen werden in NmraDcc.h definiert)
// CV-Adressen für die Speicherung der Endlagen ( in Grad vom Refpunkt , ohne Vorzeichen )
const word  CvPosZu = 47;      // max Winkel 250 Grad vom Refpunkt, 255 gilt als ungültig (leeres EEPROM)
const word  CvPosAuf = CvPosZu+(SCHRANKENZAHL) ;
 
// Variable im RAM
int positionZu[SCHRANKENZAHL];      // Endlage, über Justiervorgang einstellbar
int positionAuf[SCHRANKENZAHL];     // Endlage, über Justiervorgang einstellbar
byte justageAktiv[SCHRANKENZAHL];   // über DCC F1...F4 ein/ausschalten
 
// Aufrufparameter für das Unterprogramm 'Schrankenbewegung':
#define SB_INIT         0       // Servoansteuerung initiieren
#define SB_START_AUF    1       // Schranke öffnen starten
#define SB_START_ZU     2       // Schranke schliessen starten 
#define SB_ENDE         3       // Bewegung überwachen und Ende erkennen. Ggfs. Endlagen justieren
 
Stepper4 Schranke[SCHRANKENZAHL] = { steps360,steps360 };     // Für die Schrankenmotore
EggTimer SchrankeT[SCHRANKENZAHL];  // Schrankenspezifische Zeiten
EggTimer VorlaufT;                  // Timer der Ablaufsteuerun
 
// Zustand, in dem sich die Ablaufsteuerung gerade befindet
byte bueZustand;        // Aktueller Zustand 
#define     OFFEN               50
#define     VORLAUF_ZU          51   // Wechselblinker und Glocke, aber noch keine Bewegung 
#define     SCHRANKE_SCHLIESST  52   // Bewegung Schrankenbaum zu
#define     NACHLAUF_ZU         53   // Beide Schrankenbäume in Endpos, Glocke läutet noch.
#define     GESCHLOSSEN         54   // Schranke geschlossen
#define     SCHRANKE_OEFFNET    56   // Bewegung Schrankenbaum auf
 
// 3. Glocke -------------------------------------------------------------
EggTimer glockeT;
byte glAktiv = false;   // Flag ob Glocke aktiv ist
 
// 4. Wechselblinker ------------------------------------------------------
SoftLed Wblinker[2];    // 2 Leds für den Wechselblinker
EggTimer BlinkerT;
byte wbAktiv = false;   // Flag ob Wechselblinker aktiv ist
byte ledState = LOW;    // Status Wechselblinker
// Zustand Wechselblinker
byte wblZustand = 0;
#define   WBL_AUS     0
#define   WBL_START   1   // Beim Start sind kurz beide Lampen an
#define   WBL_BLINKT  2   
 
// sonst. -----------------------------------------------------------------
// für debugging
#ifdef DEBUG
    #define DB_PRINT( ... ) {sprintf( dbgbuf,"Dbg: " __VA_ARGS__ ) ; Serial.println( dbgbuf );}
    byte debug;
    char dbgbuf[80];
#else
    #define DB_PRINT ;
#endif
 
//###################### Ende der Definitionen ##############################
//###########################################################################
 
void setup()
{
    byte i;
    #ifdef DEBUG
    Serial.begin(115200); //Debugging
    long waitMax = millis()+5000;
    while( !Serial && millis()<waitMax); // nur bei Arduino-Micro oder Leonardo notwendig
    Serial.println( "start of Program" );
    #endif
 
    
    pinMode( InitJustageP, INPUT_PULLUP );
    
    // 1. Einschaltlogik ----------------------------------------------------
    // Initiieren des DCC-Decoders
    DCC.pin(digitalPinToInterrupt(DccInputP), DccInputP, 1); // Dcc-Signal mit Pullup
    DCC.init( MAN_ID_DIY, 01, FLAGS_MY_ADDRESS_ONLY, 0 );    // Multi-Funktionsdecoder, nur eigene Adresse
 
    // 2. Ablaufsteuerung ---------------------------------------------------
    // Eingänge für die Referenzpunkt-Lichtschranken
    for ( i=0; i<SCHRANKENZAHL; i++ ) {
        pinMode( refLsP[i], INPUT );
    }
    ////////////////// Grundinitiierung  ////////////////////////////////////
    // Testen ob gültige Werte im EEProm. Wenn nicht, Grundinitiierung mit default-Werten
    // Initiiert wird auch, wenn der Justageschalter beim Start geschlossen ist
    if (  DCC.getCV( CvPosZu ) == 255 || digitalRead( InitJustageP ) == LOW  ){
        // Grundinitiierung
        //DB_PRINT( "CV Initiierung" );
        // Endlagen auf die Default-werte setzen
        for ( i=0; i<SCHRANKENZAHL; i++ ) {
            DCC.setCV( CvPosZu+i, ENDPOS_INIT );
            DCC.setCV( CvPosAuf+i, ENDPOS_INIT );
        }
        // DCC-Adresse setzen ( Multifunktionsdecoder )
        if ( DCC_ADDR > 99 ) {
            // lange Adresse
            DCC.setCV( 17, DCC_ADDR >> 8 | 0b11000000 );
            DCC.setCV( 18, DCC_ADDR & 0xff );
            DCC.setCV( 29, DCC.getCV(29) | 0b00100000 ); // lange Adressierung
        } else {
            // kurze Addresse:
            DCC.setCV( 1, DCC_ADDR );
            DCC.setCV( 29, DCC.getCV(29) & ~0b00100000 ); // kurze Adressierung
        }
    }
    
    /////////////////////////////////////
    // Positionswerte aus EEProm lesen
    for ( i=0; i<SCHRANKENZAHL; i++ ) {
        positionZu[i]  = DCC.getCV( CvPosZu+i) * RichtungZu[i] ;
        positionAuf[i] = DCC.getCV( CvPosAuf+i ) * -(RichtungZu[i]);
        DB_PRINT( "Schr.%d - PosZu=%d PosAuf=%d", i, positionZu[i], positionAuf[i] );
    }
 
    #ifdef DEBUG
    // CV-Werte ausgeben
    DB_PRINT( "CV1:%d, CV29:%x, CV7=%d, CV8=%d", DCC.getCV(1), DCC.getCV(29), DCC.getCV(7), DCC.getCV(8) );
    DB_PRINT( "CV17:%d, CV18:%d" ,DCC.getCV(17), DCC.getCV(18) );
    #endif
    
    //////////////////////////////////////
    /////// Antriebs-Initiierung ////////////
    Schrankenbewegung( SB_INIT );
 
    // Anfangsstellung der Schranke setzen
    // Die Stellung des Schrankenbaums 1 definiert den Zustand der gesamten Schrankenanlage
    if ( digitalRead( refLsP[0]) == refLsZu[0] ) {
        bueZustand = GESCHLOSSEN;
        schrankeSchliessen = true;
        Schrankenbewegung( SB_START_ZU );
        DB_PRINT( "Init = ZU" );
        while ( !Schrankenbewegung( SB_ENDE ) );
        wbAktiv = true;
    } else {
        bueZustand = OFFEN;
        schrankeSchliessen = false;
        Schrankenbewegung( SB_START_AUF );
        DB_PRINT( "Init = AUF" );
        while ( !Schrankenbewegung( SB_ENDE ) );
    }
    
    // 3. Glocke -------------------------------------------------------------
    pinMode( glockeP, OUTPUT );
    
    // 4. Wechselblinker ------------------------------------------------------
    Wblinker[0].attach(led1P); // Portzuordnung für den WEchselblinker
    Wblinker[1].attach(led2P);
    Wblinker[0].riseTime(wbSoft); // Weiches Auf/Abblenden der Lampen
    Wblinker[1].riseTime(wbSoft);
    
    // sonst. -----------------------------------------------------------------
    
} // End Setup
//############################### ENDE SETUP ##################################
//#############################################################################
 
void loop()
{
    // 1. Einschaltlogik ----------------------------------------------------
    //////////////  Eingang zur Steuerung des Bahnübergangs /////////////////
    DCC.process();
    // schrankeSchliessen wird in notifyDccFunc gesetzt
    justageMode = ( digitalRead( InitJustageP ) == LOW );
    
    // 2. Ablaufsteuerung ---------------------------------------------------
    
    //////////// Ablaufsteuerung des Bue - Haupt-Zustandsautomat ///////////////////
    switch ( bueZustand ) {
      case OFFEN:
        // Schranke ist geöffnet, warten auf Eingang
        if ( schrankeSchliessen ) {
            // Schranke soll sich schliessen, Glocke und Wechselblinker startet.
            wbAktiv = true;     // Wechselblinker einschalten
            glAktiv = true;     // Glocke einschalten. Diesen Befehl auskommentieren wenn die Glocke erst
                                // mit der Schrankenbewegung starten soll
            if ( justageMode ) VorlaufT.setTime( 100 ); // keine Verzögerung während Justage
            else VorlaufT.setTime( vorlaufZu );
            bueZustand = VORLAUF_ZU;
            //DB_PRINT("Zustandswechsel: %d", bueZustand ); 
        }
        break; //----------------------------------------------------------
      case VORLAUF_ZU:
        // Warten bis die Vorlaufzeit abgelaufen ist, dann die Schrankenbewegung starten
        if ( !VorlaufT.running() ) {
            // Vorlaufzeit abgelaufen, Schrankenbewegung starten.
            // spätestens hier muss auch die Glocke aktiviert werden
            glAktiv = true;  // wurde sie schon aktivert, machts auch nichts ;-)
            wbAktiv = true;
            Schrankenbewegung(SB_START_ZU);    // Überwachung initiieren
            bueZustand = SCHRANKE_SCHLIESST;
        }
        break; //----------------------------------------------------------
      case SCHRANKE_SCHLIESST:
        // Schrankenbaum schliesst sich.
        if ( ( Schrankenbewegung( SB_ENDE ) ) ) {
            // beide Schrankenbäume haben ihre Endposition erreicht 
            if ( justageMode ) VorlaufT.setTime( nachlaufZu );
            else VorlaufT.setTime(  100 ); // keine Verzögerung während Justage
            bueZustand = NACHLAUF_ZU;       
       }
        break; //----------------------------------------------------------
      case NACHLAUF_ZU:
        // Schrankenbaum geschlossen, kurzer Nachlauf für Glocke.
        if ( !VorlaufT.running() ) {
            glAktiv = false;
            bueZustand = GESCHLOSSEN; 
       }
        break; //----------------------------------------------------------
      case GESCHLOSSEN:
        // Schranke ist zu, warten auf Eingang
        if ( schrankeSchliessen == false ) {
            // Schranke soll sich öffnen, Bewegung einleiten
            Schrankenbewegung(SB_START_AUF);    // Überwachung initiieren
            wbAktiv = false;        // Wechselblinker ausschalten
            bueZustand = SCHRANKE_OEFFNET;
        }
        break; //----------------------------------------------------------
      case SCHRANKE_OEFFNET:
        // Schrankenbaum öffnet sich, warten bis offen
        if ( schrankeSchliessen == true ) {
            // Notfall: beim Öffnen der Schranke kommt wieder der Befehl Schranke schliessen
            bueZustand = VORLAUF_ZU; // Da der Vorlauftimer nicht läuft, schliesst die Schranke sofort
            
        }
        if (  Schrankenbewegung( SB_ENDE ) ) {
            // beide Schrankenbäume haben ihre Endposition erreicht 
            bueZustand = OFFEN;       
        }
        break; //----------------------------------------------------------
    } ////////////// Ende Zustandsautomat Bahnübergang /////////////////////
 
    // 3. Glocke -------------------------------------------------------------
    ////////////////// Glockenimpuls erzeugen ////////////////////////////////
    if ( glAktiv ) {
        if ( !glockeT.running() ) {
            // Glockentimer abgelaufen, Impuls erzeugen
            if ( digitalRead( glockeP ) == HIGH ) {
                // Port ist gesetzt, abschalten
                digitalWrite( glockeP, LOW );
                glockeT.setTime( glZykl - glImp );
            } else {
                // Port ist aus, einschalten
                digitalWrite( glockeP, HIGH );
                glockeT.setTime( glImp );
            }
        }
    } else {
        // Glocke inaktiv, Ausgang abschalten wenn Timer nicht mehr läuft
        if ( !glockeT.running() ) {
            // Die Timerabfrage stellt sicher, dass auch der letzte Impuls immer in
            // voller Länge ausgegeben wird
            digitalWrite( glockeP, LOW );
        }
    }
    
    // 4. Wechselblinker ------------------------------------------------------
    /////////////// Wechselblinker (Zustandsautomat ) //////////////////
    switch (wblZustand) {
      case WBL_AUS:
        // Beide Lampen sind aus, warten auf einschalten
        if ( wbAktiv ) {
            // Beide Leds einschalten, Timer für gemeinsames Startleuchten
            Wblinker[0].on();
            Wblinker[1].on();
            BlinkerT.setTime( wbSoft/2 );
            wblZustand = WBL_START;
        }
        break;
      case WBL_START:
        // Startphase: Nach Zeitablauf erste Led wieder aus
        if ( !BlinkerT.running() ) {
            // Übergang zur normalen Blinkphase
            ledState = HIGH;
            Wblinker[1].off();
            BlinkerT.setTime(wbSoft);
            wblZustand = WBL_BLINKT;
        }
        break;
      case WBL_BLINKT:
        if ( !BlinkerT.running() ) {
            BlinkerT.setTime(wbZykl/2);
            if ( ledState == LOW ) {
                Wblinker[0].on();
                Wblinker[1].off();
                ledState = HIGH;
            } else {
                ledState = LOW;
                Wblinker[1].on();
                Wblinker[0].off();
            }
        }
        if ( !wbAktiv ) {
            // Wechselblinker abschalten
            Wblinker[0].off();
            Wblinker[1].off();
            wblZustand = WBL_AUS;
        }
        break;
            
    } /////////// Ende switch Wechselblinker ////////////////////////
} // End Loop
//#################### ENDE LOOP ##################################################
//#################################################################################
 

byte Schrankenbewegung( byte mode ) {
    // Bewegungsvorgang der Schranken überwachen
    // Das Unterprogramm wird im Loop währende der Bewegung zyklisch aufgerufen
    // Der Funktionswert ist 'true', wenn die Bewegung aller Schranken abgeschlossen ist,
    // sonst immer 'false'
    // mode:SB_INIT         Grundinitiierung
    //      SB_START_ZU     Schliessen der Schranken einleiten
    //      SB_START_AUF    Öffnen der Schranken einleiten
    //      SB_ENDE         Bewegung überwachen, meldet 'true' wenn alle Bewegungen abgeschlossen
    //                      sind
    // -------------------------------------------------------------------------
    //
    static enum { WAIT, NORMAL, WIPPEN, FIND_REF, STOP  } ssZustand[SCHRANKENZAHL] ;
    static int startPos[SCHRANKENZAHL] ;  // Position der Schranke zu Bewegungsbeginn
    static byte gotRef[SCHRANKENZAHL]   ; // Flag ob Referenzpunkt bekannt ist
    static enum { AUF,ZU } richtung;   
    
    byte bewegung=0, sn, refLS;
    int tmp;
    for( sn=0; sn<SCHRANKENZAHL; sn++ ) {
        // für alle Schranken durchlaufen
        switch ( mode ) {
          case SB_INIT: // Initiierung der Schranken
            //DB_PRINT("SB_INIT(%d)",sn);
            Schranke[sn].attach(StepPort[sn] ); // Schrittmotore an SPI_x
            Schranke[sn].setSpeed( schrTempo[sn] );
            gotRef[sn]=false;
            bewegung = 1;
            break; //---------------------------------------------
          case SB_START_ZU:  // Schliessen der Schranken einleiten
            //DB_PRINT("SB_START_ZU(%d)",sn);
            richtung =ZU;
            Schranke[sn].setSpeed( schrTempo[sn]);
            SchrankeT[sn].setTime( schrVerzZU[sn] ); //Wartezeit bis Schrankenbewegung
            //DB_PRINT( "Start Wartezeit %d mit %d ms", sn, schrVerzZU[sn] );
            ssZustand[sn] = WAIT;
            bewegung = 1;
            break; //---------------------------------------------
          case SB_START_AUF: // Öffnen der Schranken einleiten
            richtung = AUF;
            Schranke[sn].setSpeed( schrTempo[sn]);
            if ( digitalRead( refLsP[sn] ) == refLsZu[sn] ) {
                // Bewegung AUF nur starten, wenn sich die Schranke im Bereich ZU befindet 
                // Zur Sicherheit dreht der Motor max 1/2 Umdrehung. In diesem Bereich muss er die
                // Referenzpunkt LS erreichen, wo auf den endgültigen Endpunkt umgeschaltet wird
                Schranke[sn].doSteps( -(RichtungZu[sn]) * steps360/2);
                //DB_PRINT( "doSteps(%d):%d R=%d S=%d",sn, RICHTUNG_AUF * steps360/2 , RICHTUNG_AUF, steps360);
                ssZustand[sn] = FIND_REF;
                //DB_PRINT("SB_START_AUF(%d)->FIND_REF",sn);
            } else if ( gotRef[sn] ) {
                // steht noch im Bereich 'AUF', könnte Notumkehr sein. Da ref bekannt ist, direkt
                // auf Position Auf
                Schranke[sn].write(positionAuf[sn]);
                ssZustand[sn] = NORMAL;
                //DB_PRINT("SB_START_AUF(%d)->NORMAL(write)",sn);
            } else {
                // nichts tun
                ssZustand[sn] = STOP;   // keine Aktion
                //DB_PRINT("SB_START_AUF(%d)->JUSTAGE_ENDE(nichts)",sn);
            }
            startPos[sn] = Schranke[sn].read();
            //DB_PRINT( "Schranke %d, Position: %d, Richtung= %d", sn, startPos[sn], richtung );
            bewegung = 1;
            break; //---------------------------------------------
          case SB_ENDE:     // Bewegung überwachen, auf Ende prüfen
            // Schrankenbewegung
            switch ( ssZustand[sn] ) {
              case WAIT: // Verzögerungszeit bis zum Schrankenstart abwarten
                         // Wird nur beim Schliessen der Schranke durchlaufen
                if ( SchrankeT[sn].running() == false ) {
                    // Zeit abgelaufen, Bewegung starten
                    //Schranke[sn].write( positionZu[sn]);
                    if ( digitalRead( refLsP[sn]) != refLsZu[sn]  ) {
                        // Bewegung ZU nur starten, wenn sich die Schranke im Bereich AUF befindet 
                        // Zur Sicherheit dreht der Motor max 1/3 Umdrehung. In diesem Bereich muss er die
                        // Referenzpunkt LS erreichen, wo auf den endgültigen Endpunkt umgeschaltet wird
                        Schranke[sn].doSteps( RichtungZu[sn] * steps360/2);
                        //DB_PRINT( "doSteps(%d):%d", sn, RICHTUNG_ZU * steps360/2 );
                        ssZustand[sn] = FIND_REF;
                        //DB_PRINT("WAIT(%d)->FIND_REF",sn);
                    } else if ( gotRef[sn] ) {
                        // steht noch im Bereich 'ZU', könnte Notumkehr sein. Da ref bekannt ist, direkt
                        // auf Position Zu
                        Schranke[sn].write(positionZu[sn]);
                        //DB_PRINT("WAIT(%d)->NORMAL(write)",sn);
                        ssZustand[sn] = NORMAL;
                    } else {
                        // nichts tun
                        //DB_PRINT("WAIT(%d)->JUSTAGE_ENDE(nichts)",sn);
                        ssZustand[sn] = STOP;   // keine Aktion
                    }
                    startPos[sn] = Schranke[sn].read();
                    //DB_PRINT( "Schranke %d, Position: %d, Richtung= %d", sn, startPos[sn], richtung ); 
                }
                bewegung = 1;                    
                break; //......................................
              case FIND_REF: // Bewegungsablauf bis zur LS überwachen. An der LS wird der Refpunkt gesetzt und die
                bewegung += Schranke[sn].moving();
                // Eigentliche Endposition angesteuert.
                refLS = digitalRead( refLsP[sn] );
                //if ( sn == 0 ) DB_PRINT( "refLS=%d", refLS );
                if ( richtung == ZU && refLS == refLsZu[sn] ) {
                    //Lichtschranke beim Schliessen erreicht)
                    Schranke[sn].setZero();
                    gotRef[sn] = true;
                    Schranke[sn].write( positionZu[sn]);
                    //DB_PRINT( "FIND_REF(%d)->NORMAL(zu)",sn );
                    ssZustand[sn] = NORMAL;
                }
                if ( richtung == AUF && refLS != refLsZu[sn] ) {
                    //Lichtschranke beim Öffnen erreicht)
                    Schranke[sn].setZero();
                    gotRef[sn] = true;
                    Schranke[sn].write( positionAuf[sn]);
                    //DB_PRINT( "FIND_REF(%d)->NORMAL(auf)",sn );
                    ssZustand[sn] = NORMAL;
                }
                break;
              case NORMAL:
                tmp = Schranke[sn].moving();
                bewegung += tmp;
                if (tmp < WIPPBEREICH && !justageMode ) {
                    //DB_PRINT( "NORMAL(%d) -> WIPPEN", sn );
                    ssZustand[sn] = WIPPEN;
                    wippIx[sn] = 0; // Zähler für die Wippbewegungen
                }
                break; // .....................................
              case WIPPEN:
                tmp = Schranke[sn].moving();
                bewegung += tmp;
                if ( tmp == 0 )  { 
                    // keine Bewegung mehr aber noch Wippen ?
                    if ( wippIx[sn] < wippPnts ) {
                    // nächsten Wipppunkt anfahren
                    tmp = 10* ((richtung == AUF) ? positionAuf[sn] : positionZu[sn]); // in 1/10° umrechnen
                    tmp = (tmp>0) ? tmp + wippPos[wippIx[sn]] : tmp - wippPos[wippIx[sn]]; 
                    if ( wippSpeed[wippIx[sn]] ) Schranke[sn].setSpeed( wippSpeed[wippIx[sn]]);
                    Schranke[sn].write( tmp, 10 );
                    //DB_PRINT( "Wip.write( %d , 10 )", tmp );
                    wippIx[sn]++;
                    bewegung +=1; // Es ist noch nicht zu Ende!
                    }
                }
                break; //......................................
              case STOP:
                // keine Aktion, nur auf Schrankenstop warten
                bewegung += Schranke[sn].moving();
    
                break; //......................................
           } // ..... Ende switch 'Schrankenzustand' ......
    
            break; //---------------------------------------------
          default:
            // falscher Programmaufruf, keine Reaktion
            ;
        } // --- Ende Switch 'mode' --------
    } // ........Ende forschleife der Schranken........
 
    if ( bewegung == 0 ) DB_PRINT( "Endpositionen erreicht");
    return ( bewegung == 0 );
}
  
//###################################################################################
//###################### DCC - Funktionen ###########################################
 
// Geschwindigkeitstelegramm =========================================================
// wird hier zur Endlagenjustierung genutzt. Der Geschwindikeitswert wird
// als Positionsdifferenz ( in Grad ) zur derzeitigen Position ausgewertet.
// Die Endlagenjustierung ist nur aktiv, 
// - wenn der Justiereingang auf LOW steht, und
// - wenn der Bue-Zustand OFFEN oder GESCHLOSSEN ist (dies bestimmt auch welche Endlage eingestellt wird)
// - mindestens eine der DCC-Funktionen F1 ... F4 ( jeweils für Schrankenbaum 1...4 ) aktiv ist.
// Wird die DCC Funktion abgeschaltet, so wird die momentane Position des Schrankenbaums als Endlage gespeichert.
// ( im Funktionstelegramm, s.u. )
//
void notifyDccSpeed( uint16_t Addr, uint8_t Speed, uint8_t ForwardDir, uint8_t MaxSpeed ){
    // Geschwindigkeitstelegramm nur zur Justage auswerten 
    int8_t vSpeed;
    Speed--;
    // Maximal +/- 32 Grad für die Justierung
    if ( MaxSpeed > 28 ) Speed /= 4;
    
    vSpeed = ForwardDir? -Speed:Speed;
    static int8_t lastSpeed;
    if ( vSpeed != lastSpeed ){
        // Geschwindigkeitswert hat sich verändert, gegebenenfalls Schrankenposition aktualisiern
        if (  (bueZustand == OFFEN || bueZustand == GESCHLOSSEN) && justageAktiv) {
            // im Zustand OFFEN oder GESCHLOSSEN ist Justage möglich
            for ( byte i= 0; i<SCHRANKENZAHL; i++ ) {
                // Justagezustand prüfen und Schrankenbaum gegebenenfalls bewegen
                if( justageAktiv[i]  ) {
                    // Schrankenbaum entsprechend vSpeed positionieren. Der über DCC vorgegebene
                    // Wert wird nur übernommen, solange sich der Schrankenbaum noch im richtigen
                    // Bereich der LS befindet.
                    Schranke[i].setSpeed( schrTempo[i]/2 );
                    if ( bueZustand == OFFEN ) {
                        if ( digitalRead( refLsP[i] ) != refLsZu[i] ) {
                            // noch im korrekten Bereich, Position übernehmen
                            Schranke[i].write( positionAuf[i]+vSpeed );
                        } else if ( (vSpeed * RichtungZu[i]) < 0 ) {
                            // wir sind im falschen Bereich, aber Richtung geht zum richtigen Bereich
                            Schranke[i].write( positionAuf[i]+vSpeed );
                        }
                    } else if ( bueZustand == GESCHLOSSEN ) {
                        if ( digitalRead( refLsP[i] ) == refLsZu[i] ) {
                            Schranke[i].write( positionZu[i]+vSpeed );
                        } else if ( (vSpeed * RichtungZu[i]) > 0 ) {
                            // wir sind im falschen Bereich, aber Richtung geht zum richtigen Bereich
                            Schranke[i].write( positionZu[i]+vSpeed );
                        }
                    }
                }
            }
        }
        DB_PRINT( " Adresse: %u, Speed %d, Max:%d", Addr, vSpeed, MaxSpeed );
        lastSpeed = vSpeed;
        
    }
   
}
 
// Funktionstelegramm ======================================================================
// F0: bestimmt den Zustand der Schranke (auf/zu)
// F1..F4 bestimmt, bei welchem Schrankenbaum die Endlage justiert wird. Mit dem Ausschalten der Funktion
// wird die Endlage übernommen und gespeichert.
void notifyDccFunc( uint16_t Addr, FN_GROUP FuncNum, uint8_t FuncState){
    // Funktionstelegramm  mit eigener Adresse erhalten
    static byte FGrOffset[] = {0,1,5,9,13,21}; // Offset Funktionsgruppe -> Funktionsnummer
    static uint8_t lastFuncNum, lastFuncState[6]; 
    uint8_t tmp, i, Fnr, State;
    //DB_PRINT( " Adresse: %u, F%d, Status %xnr", Addr, FuncNum, FuncState ); 
    if ( lastFuncState[FuncNum] != FuncState ) {
        // Funktionsnummer bestimmen
        tmp = FuncState ^ lastFuncState[FuncNum];
        // Sonderfall Funktionsnummer 0:
        if ( FuncNum == FN_0_4 && ( tmp&FN_BIT_00 ) ) {
            // Ist funktion 0 ( = Schranke auf/zu )
            schrankeSchliessen = ( (FuncState & FN_BIT_00) != 0);
            DB_PRINT( " Adresse: %u, F0, Status %x", Addr, schrankeSchliessen );
            tmp = tmp & ~FN_BIT_00;
        }
        for ( i= 0; i<8 ; i++ ) {
            if ( (1<<i) & tmp ) {
                // geändertes Bit gefunden
                Fnr = FGrOffset[FuncNum] + i;
                State = ( (FuncState&(1<<i)) !=0 );
                if( Fnr > 0 && Fnr <= SCHRANKENZAHL ){
                    // Funktionen F1 ... setzen den Justagemodus der Schranken
                    justageAktiv[Fnr-1] = State;
                    if ( State == 0 && justageAktiv  ) {
                        // Funktion wird ausgeschaltet, Motorposition als Endlage übernehmen, wenn LS
                        // im richtigen Bereich
                        if ( bueZustand == OFFEN && digitalRead( refLsP[Fnr-1] ) != refLsZu[Fnr-1]) {
                            positionAuf[Fnr-1] = Schranke[Fnr-1].read();
                            DCC.setCV( CvPosAuf+Fnr-1 , abs( positionAuf[Fnr-1] ) );
                        } else if ( bueZustand == GESCHLOSSEN  && digitalRead( refLsP[Fnr-1] ) == refLsZu[Fnr-1]) {
                            positionZu[Fnr-1] = Schranke[Fnr-1].read();
                            DCC.setCV( CvPosZu+Fnr-1 , abs( positionZu[Fnr-1] ) );
                        }
                    }
                }
                DB_PRINT( " Adresse: %u, F%d, Status %xnr", Addr, Fnr, State ); 
            }
        }
        //DB_PRINT( " Adresse: %u, Funktion %u, Status %xnr", Addr, FuncNum, FuncState );
        lastFuncState[FuncNum]=FuncState;
    }
}