- 4x Spannung messen -> Winkelbestimmung der Gelenke
initADC()
getADC()
setChannel()

- 1x Zustand messen -> Bodenkontakt
if (PINx &(1<<Pxn)) {}


- Routine Datenerfassung und Vergleich -> bestimmte Winkel und Zustand Bodenkontakt / Ist & soll
switch case oder verschachtelte if's

- Datenauswertung und Anzeige -> Anzeige über LCD

initLCD();
printLCD(char* text, unsigned char zeile);
clearLCD([unsigned char Zeile]); //Zeile nur optional


- Richtungsbestimmung bzw. Ausführung von Anweisungen -> Bewegungsabläufe

kommt drauf an wie du dich bewegst, aber bei servolenkung UND doppelantrieb brauchste wohl PWM also

initTimer() //altervnativ initSERVO()
initMotor()
setMotorDir() // wie im asuro ^^
setMotorSpeed()

- Verarbeitung von Ereignissen -> Kollision / nichterreichte Zustände

interruptservice-routinen

ISR oder SIGNAL

- allgemeine Spannungsüberwachung -> Betriebsspannung

das geht mit dem ADC


so als anregung für die zusammenstellung deiner methoden ^^