Code:
for (i=0; i<4;i++)
{
        if ( max < servo_weg(i) ) {
               max = servo_weg(i);
               i_max = i;
        }
}
viel einfacher wird es nicht gehen...

Du könntest u.U. die Servos "sortieren", wenn sich deren Weglänge selten verändert, dürfte sich das lohnen.