Ja, mit den Zuständen kommt das prinzipiell hin. Dar Code wird dadurch natürlich länglicher, und die Bib-Funktionen kann man kaum verwenden, sondern man muss sebst codieren.

Übrigens ist es besser sprechende Bezeichner zu verwenden als magische Zahlen wie 7, also zB
Code:
enum
{
   ZUSTAND_1,
   ZUSTAND_2,
   ...
   ZUSTAND_N
}

// oder 

#define STATE_A 0
#define STATE_B 1
#define STATE_C 2
...

   if (STATE_A)
   ...
   else if (STATE_B)
   ...

// oder
   switch (state)
   {
      case ZUSTAND_1:
          ...
          break;
      case ZUSTAND_2:
          ...
   }
Zu Deiner Messung: Du brauchst doch immer 2 Werte, die in eine Differenzbildung eingehen. Wenn einer davon nicht gemessen wird, dann wird die Differenz falsch. Oder ich steh aufm Schlauch...

Wenn die Berechnung wie gesagt in der ISR gemacht, wird dann kannst Du nix verpassen (ausser die zeitgerechte Verwendung der Drehzahl/Speed/was-auch-immer falls Du in ner TWI-Schleife gondelst, aber immerhin ist die Berechnung dann OK). Das würde Dir die Implementierung eigener TWI-Routinen sparen.