jetzt kannst mühsam Register für Register wieder aktivieren.
Aber was anderes, eine Reihe von Kontrollfragen für dich: schau dir bitte mal alle deine int an. Müssen das wirklich alles int sein? kannst da ein paar auf int8_t oder uint8_t umrüsten? Dein int Hoehe = 155; z.B. wird ja wohl kaum negativ werden.
Dann noch: welche dieser Werte ändern sich nie? Dann setz' ein const davor.
warum brauchst du überhaupt diese Zwischenvariablen, warum kannst die Berechnung nicht direkt bei der Registerzuordnung machen?
Kannst mal testweise das ganze auf einem Mega laufen lassen?