Atmega644 - pozycjonowanie serwa

[Panicz74]

Witam,

Pewnie sprawa będzie dla was banalna ale ja się z nią zmagam już jakiś czas. Poniższy kod 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 2 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ł
}

Wszystko działa dobrze, tylko chciałbym aby serwa ruszały po kolei, od SERVO1 do SERVO5. W tym momencie wszystkie wykonują zadany ruch jednocześnie. Zabiegi z "delay" nic nie pomagają.

Czy mogę liczyć na wasza pomoc?

[Phil]

Pokaż jak wyglądały "Zabiegi z delay"

[Panicz74]

M. in. tak:

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

                                servo1 = SERVO_MAX;
                                _delay_ms(50);
                                servo2 = SERVO_MIN;
                                _delay_ms(50);
                                servo3 = SERVO_MAX;
                                _delay_ms(50);
                                servo4 = SERVO_MIN;
                                _delay_ms(50);
                                servo5 = SERVO_MAX;

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

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

                        }

                }

[Phil]

Ale zdajesz sobie sprawę, że 50ms to 0,05s? Zwiększ wartości.

[Panicz74]

No własnie, już próbowałem. Zmieniłem te wartości nawet 5000ms i nic. Po tej zmianie program po prostu w ogóle nie rusza...

[Treker (Damian Szymański)]

Panicz74 i jak? Udało się uruchomić program?

[Panicz74]

Przepraszam was, że tak długo. Uruchomiłem program. Jedyne co należało zrobić to zwiększyć ilość warunków i w ten sposób puszczać serwa po kolei Jak nie spełnia jednego warunku to nie jedzie. Jeśli ktoś będzie potrzebował to udostępnię cały kod.

Tymczasem jest problem z zakłóceniami. To jest dopiero ciekawa sprawa. Patrzcie na wyniki z oscyloskopu. Dźwięk i obraz idealnie oddają postać rzeczy:

Może ktoś wie jak ten problem rozwiązać???

[Sabre]

Wygląda to tak, jakby raz na ileś cykli pętli programu zamiast sygnału dla serwa było 0, stąd widoczna pozioma linia. Musisz mieć jakiś błąd w kodzie, który to powoduje.

[Panicz74]

Błędem w kodzie, a raczej w fusbitach był fus CKDIV8... Spowalniał procka x8. Potem już zmiana bitów z CS10 na CS11, ICR1 na 40000, trochę przeliczania... i śmiga.

Jak pupa niemowlęcia

Innymi słowy, 100Hz to niedobry pomysł dla serwomechanizmów