ATmega32 sterowanie silnikami krokowymi - robot kartezjański

[rupert123]

Witam serdecznie. Realizuję projekt robota kartezjańskiego. Doszedłem do etapu programowania i ze względu na brak doświadczenia w tym zakresie przysparza mi ono sporo problemów. Udało mi się napisać funkcje do bazowania i zadawania ruchu o charakterystyce trapezowej dla pojedynczego silnika, teraz chciałbym tak zmodyfikować te funkcje, aby za pomocą jakiejś zmiennej przekazywać do nich, który z czterech silników mają obsługiwać. Bardzo proszę o jakąś podpowiedź, jak do tej modyfikacji podejść. Sterowanie odbywa się za pomocą mikrokontrolera ATmega32 i sterowników A4988. Poniżej wklejam funkcję realizującą ruch trapezowy i procedurę obsługi przerwania.

 void trapezoidMove (uint16_t howManySteps)
   {
           uint8_t delay = MAX_DELAY;
           uint16_t stepsTaken = 0;
           TIMSK |= (1<<OCIE0);

           if (howManySteps > (RAMP_STEPS*2))
           {
                   while(stepsTaken < RAMP_STEPS)
                   {
                           takeSteps(1,delay);
                           delay -= ACCELERATION;
                           stepsTaken++;
                   }

                   delay = MIN_DELAY;
                   takeSteps((howManySteps - 2*RAMP_STEPS), delay);
                   stepsTaken += (howManySteps - 2*RAMP_STEPS);

                   while(stepsTaken < howManySteps)
                   {
                           takeSteps(1,delay);
                           delay += ACCELERATION;
                           stepsTaken++;
                   }
           }

           else
           {
                   while(stepsTaken <= howManySteps/2)
                   {
                           takeSteps(1,delay);
                           delay -= ACCELERATION;
                           stepsTaken++;
                   }
                   delay += ACCELERATION;
                   while (stepsTaken < howManySteps)
                   {
                           takeSteps(1,delay);
                           delay += ACCELERATION;
                           stepsTaken++;
                   }
           }
   }

 ISR(TIMER0_COMP_vect)  //funkcja obslugi przerwania
   {
           PORTD ^= (1<<STEP);
           stepCounter++;
   }

Z góry bardzo dziękuję za wszystkie sugestie i pomoc.