Sterowanie 5 serwomechanizmami

Witam,

Sprzęt:

Piszę do was bo naprawdę się nakombinowałem przeokrutnie a skutku nie widzę. Poza tym przypuszczam, że dla was problem będzie prosty.
Napisałem program, który steruje 5 serwami:

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define KEY1    (1<<PA0)				// definicja przycików
#define KEY2    (1<<PA1)
#define KEY3    (1<<PA3)

#define KEY1_DOWN ( !(PINA & KEY1) )	// definicja wciśniętego przycisku
#define KEY2_DOWN ( !(PINA & KEY2) )
#define KEY3_DOWN ( !(PINA & KEY3) )

#define SERVO1 (1<<PB0)					// definicja silników
#define SERVO2 (1<<PB1)
#define SERVO3 (1<<PB2)
#define SERVO4 (1<<PB3)
#define SERVO5 (1<<PB4)


#define SERVO_MID       1500			// definicja położenia silnika
#define SERVO_MIN       1000
#define SERVO_MAX       2000


uint16_t servo1, servo2, servo3, servo4, servo5;
volatile uint8_t key_flag;				// zmienna pomijana przez kompilator

int main(void)
{
	servo1 = servo2 = servo3 = servo4 = servo5 = SERVO_MID;		// ustawienie początkowe silników
	PORTA |= KEY1 | KEY2 | KEY3;				// wyjścia - przyciski
	DDRB |= SERVO1 | SERVO2 | SERVO3 | SERVO4 | SERVO5; 			// wyjścia do silników
	TCCR1A |= 1<<WGM11;
	TCCR1B |= 1<<WGM12 | 1<<WGM13 | 1<<CS10;//rejestry odpowiedzialne za PWM
	TIMSK1 |= 1<<OCIE1A;					// timer dla TCCR1B & TCCR1A

	ICR1 = 19999;//rozciagnij, zawez okres

	sei();//uruchomienie przerwań

	while(1)
	        {
	                if(key_flag) // jeśli flaga = 0, wykonaj program
	                {
	                        key_flag = 0;
	                        if (KEY1_DOWN) //klawisz 1 wcisniety
	                        {

	                                servo1 = SERVO_MAX;
	                                servo2 = SERVO_MIN;
	                                servo3 = SERVO_MAX;
	                                servo4 = SERVO_MIN;
	                                servo5 = SERVO_MAX;

	                        }
	                        if (KEY2_DOWN) //klawisz 2 wcisniety
	                        {
	                                servo1 = SERVO_MIN;
	                                servo2 = SERVO_MAX;
	                                servo3 = SERVO_MIN;
	                                servo4 = SERVO_MAX;
	                                servo5 = SERVO_MIN;

	                        }
	                        if (KEY3_DOWN) //klawisz 3 wcisniety
	                        {
	                                servo1 = SERVO_MID;
	                                servo2 = SERVO_MID;
	                                servo3 = SERVO_MID;
	                                servo4 = SERVO_MID;
	                                servo5 = SERVO_MID;

	                        }

	                }
	                if(TCNT1 >= servo1) PORTB &= ~SERVO1; //nakierowanie mechanizmu przerwania
	                if(TCNT1 >= servo2) PORTB &= ~SERVO2;
	                if(TCNT1 >= servo3) PORTB &= ~SERVO3;
	                if(TCNT1 >= servo4) PORTB &= ~SERVO4;
	                if(TCNT1 >= servo5) PORTB &= ~SERVO5;

	        }
}
ISR (TIMER1_COMPA_vect)
{
        PORTB |= SERVO1 | SERVO2 | SERVO3 | SERVO4 | SERVO5;
        key_flag = 1;						 // jeśli flaga = 1, program sie wykonał
}

 

I teraz problem. Zachodzi tutaj zjawisko hazardu. Serwa startują jednocześnie lub losowo. Chciałbym żeby startowały po kolei, od SERVO1 do SERVO5.

Na pewno chodzi o tą część programu:

 
ISR (TIMER1_COMPA_vect)
{
        PORTB |= SERVO1 | SERVO2 | SERVO3 | SERVO4 | SERVO5;
        key_flag = 1;						 // jeśli flaga = 1, program sie wykonał
}

Tylko, że jak coś zmieniam to program w ogóle nie działa. Kombinowałem już z opóźnieniami, licznikami i instrukcjami ale bez skutku.

Bardzo was proszę o pomoc.

Jak należy tym serwami sterować, tzn. jakie przebiegi chcesz uzyskać na wyjściu? Bo błąd jest imho nie w przerwaniu, a gdzieś indziej.

Tzn. kod jest ok, działa jak powinien. Problem leży w tym, że dając sygnał na przycisk (KEY1 | KEY2 | KEY3) wszystkie serwa startują jednocześnie. Chodzi właśnie o to aby startowały jedno po drugim, z jakimś opóźnieniem. Pojęcia nie mam co tutaj zmienić, dopisać...

Nie wiem na ile dalej chcesz iść z tym kodem, ale na na gorąco (i mało elegancko ;)) - dołóż jakąś zmienną, która będzie się inkrementować nie blokując pętli i dorzuć warunek na jej wartość do kolejnych ifów.

Spróbowałem tak zrobić ale niestety nic z tego :(

while(1)
	        {
	                if(key_flag) // jeśli flaga = 0, wykonaj program
	                {
	                        key_flag = 0;
	                        int i = 0;
	                        if (KEY1_DOWN && i==0) //klawisz 1 wcisniety
	                        {
	                        		servo1 = SERVO_MAX; i++;
	                        }
	                        if (KEY1_DOWN && i==1)
	                        {
	                                servo2 = SERVO_MIN; i++;
	                        }
	                        if (KEY1_DOWN && i==2)
	                        {
	                                servo3 = SERVO_MAX; i++;
	        				}
	                        if (KEY1_DOWN && i==3)
	                        {
	                                servo4 = SERVO_MIN; i++;
	                        }
	                        if (KEY1_DOWN && i==4)
	                        {
	                                servo5 = SERVO_MAX; i++;
	                        }
	                        i = 0;
.
.
.
 

Nie działa, co zmaściłem tym razem?

Jakieś większe zakresy tej zmiennej może? Z jaką częstotliwością pędzisz procesor?

1. twoja pętla nie ma żadnego opóźnienia
2. twoja flaga key_flag jak już raz zostanie ustawiona to kaplica, bo nigdzie jej nie czyścisz (IMO to jest główny problem).
3. flaga key_flag nie ma zabezpieczeń przed race condition (program wygląda na wielowątkowy, ale mogę się mylić, bo nie znam tego środowiska)
4. po co pętla skoro obsługujesz przerwanie?
5. mało kto zna tą bibliotekę, wiec musisz lepiej ospisywać co sie dzieje.
6. dziel wszystko na małe funkcje
7. za dużo makr