Drehzahlabhängig schalten mit Display

#include <LiquidCrystal.h>                    // LCD Library einbinden

LiquidCrystal mylcd(8, 9, 4, 5, 6, 7);        // Display Anschlußpins definieren

const int Eingang = 2;                        // Pin 2 ist Eingang für Sensor

const int SchalterVor = 13;
const int SchalterRueck = 10 ;
const int RelaisVor = 3;
const int RelaisRueck = 11;
const int auswertung = 12;

int vor;
int rueck;
int rVor;
int rRueck;
int GeschwindigkeitOk;

void setup() {
 pinMode(Eingang, INPUT);                    // Eingangspin auf Eingang stellen
  digitalWrite(Eingang, HIGH);                // und Pullup-Widerstand einschalten

  mylcd.begin(16, 2);                         // Display hat 2 Zeilen a 16 Zeichen
  mylcd.print("Drehzahlsensor");              // Überschrift ausgeben
  attachInterrupt(0, readmicros, RISING );    // Interrupt 0 auf Routine readmillis setzen
                                              // reagiert auf steigende Flanken an Pin 2

     pinMode(RelaisVor, OUTPUT);
     pinMode(SchalterVor, INPUT);
     pinMode(RelaisRueck, OUTPUT);
     pinMode(SchalterRueck, INPUT);
     pinMode(auswertung, INPUT);

}
                                              // ein paar variablen definieren
volatile unsigned long dauer=0;               // microsekunden seit dem letzten Interrupt
volatile unsigned long last=0;                // Zählerwert beim letzten Interrup
long drehzahl;                                // selbstredend
char buf[17];                                 // Pufferstring für sprintf
void loop() {         //ist die Geschwindigkeit ok, dann soll wenn entweder der Vorwärts- oder der Rückwärtsgang
                      //drin ist, das jeweilige Relais anziehen. Fällt der jeweilige Schalter für die Fahrtrichtung
                      //ab, soll auch da jeweilie Relais abfallen.


  if (drehzahl <= 80)                        // auf gewünschte drehzahl einstellen,
                                              // bei der noch geschaltet werden darf
  {
    GeschwindigkeitOk = HIGH;
  }
  if (drehzahl >= 85)
  {
    GeschwindigkeitOk = LOW;
  }

   vor = digitalRead(SchalterVor);            // Schalter fürs vorwärtsfahren
     if (vor == LOW)
     {
       digitalWrite (RelaisVor, HIGH);        // RelaisVor soll aus sein!!

     }
      if (vor==HIGH && GeschwindigkeitOk==HIGH && rueck!=HIGH)
      {
         rVor = LOW;
         digitalWrite(RelaisVor, rVor);       // RelaisVor soll an sein!!
        }
   rueck = digitalRead(SchalterRueck);        // Schalter fürs rückwärtsfahren
     if (rueck == LOW)
     {
        digitalWrite (RelaisRueck, HIGH);     // RelaisRueck soll aus sein!!

     }
      if (rueck==HIGH && GeschwindigkeitOk==HIGH && vor!=HIGH)
      {
         rRueck = LOW;
         digitalWrite(RelaisRueck, rRueck);   //RelaisRueck soll an sein!!
        }
         drehzahl = 60000000 / 32 / dauer ;        // Drehzahl ausrechnen und (32 Zähne)
  sprintf(buf, "p/min %4lu   ", drehzahl);    // als 4stellig formatierte Zahl in den Puffer schreiben
  mylcd.setCursor(0, 1);                      // cursor an den Anfang der 2. Zeile (fängt mit 0 an)
  mylcd.print(buf);                           // Puffer ausgeben
}
 void readmicros() {                          // Interrupt-Routine
  detachInterrupt(0);                         // Interrupt ausschalten damit er uns nicht beißt
  unsigned long m = micros();                 // Microsekundenzähler auslesen
  unsigned long v = m - last;                 // Differenz zum letzten Durchlauf berechnen
  if (v > 5000) {                             // ignorieren wenn <= 5ms (Kontaktpreller)
    dauer = v;                                // Wert in dauer übernehmen
    last = m;                                 // und wieder den letzten Wert merken
  }
  attachInterrupt(0, readmicros, RISING );    // Interrupt wieder einschalten.
}