Nur mal zur Info - Ein paar Grundlagen.
So ein Servo wird mit Impulsen auf der Impulsleitung angesteuert.
Die Impulslänge liegt dabei zwischen 1 und 2ms. Alle 20ms wird so ein Impuls an das Servo ausgegeben.
Ich mach das in "C" mit einem Timer der mir den Impuls und die Pause über Comparematch Interrupts generiert.
Arduino hab ich nicht, dort könnte es eine fertige Library geben.
Die gewünschte Impulslänge musst du so lange ausgeben, bis das Servo die Lage auch erreichen kann ( wurde weiter vorher schon angesprochen ).
Das mit dem Entprellen der Tasten wurde auch schon angesprochen.
Ich mach das meistens so, das ich bei einer Änderung nach 10...100ms "nachsehe" ob die Taste noch gedrückt ist.
Ist das der Fall, wird dies als Tastendruck akzeptiert. Das gleiche passiert auch beim Loslassen der Taste.
Hat das Getriebe nich auch nen Schalter der bei Neutral geschlossen ist ? Den könnte man abfragen.
Die relativ große Zeitspanne der Wartezeit ist von der Qualität der Taster abhängig und deshalb u.U. experimentell zu ermitteln.
Ein weiteres Problem hast Du anscheinend noch nicht auf dem Schirm und zwar das Kuppeln.
Ich denke mal, das bei jedem Schaltvorgang die Kupplung betätigt werden muß.
Ist das Gefährt schon mal in Fahrt kann man die Kupplung wohl reinknallen.
Beim Anfahren wirst Du dabei wohl den Motor abwürgen.
Also wirst du drehzahlabhängig eine Einkuppelkurve anlegen müssen um erstmal vom Fleck zu kommen.

Ich seh das so, das Du einen Drehzahlsensor, einen Geschwindigkeitssensor, 2 Taster, evtl. 1 bis 2 Anzeigen und 2 bis 3 Servos bedienen musst.

Im Prinzip kein Problem, das Hauptproblem seh ich da in der Entwicklung der " Kennfelder " um möglichst schnell voran zu kommen.
Das dürfte wohl die meiste Zeit verschlingen.