[C] Kinematyka odwrotna - implementacja w AVR

[AE]

Ja bym na Twoim miejscu zmienił kod na taki, w którym w przerwaniu ustawiana była jakaś flaga, a następnie w głównym kodzie gdy flaga będzie spełniona wpisujesz odpowiednie wartości do rejestrów timera.

Zastosowałem się do rady i zrobiłem ustawianie flag w przerwaniach, a w pętli głównej za pomocą switch() ustawiane są rejestry - bez efektu

wój przycisk zwiera do plusa, czy do masy? Jeśli do masy, to czy masz włączonego pull-up'a? Możesz też dać przed pętlą główną ze 100ms opóźnienia, bo w trakcie startu mikrokontrolera mogą się dziać rzeczy dziwne.

Wszystkie przyciski zwierają do masy i mam włączonego pull-up`a. Dodałem również owe opóźnienie - również bez poprawy

Sprawdź czy dla różnych wartości TCP jest ok gdy podajesz tylko raz. Sprawdź też czy gdy wprowadzasz nową wartość TCP to czy przypadkiem któreś zmienne pomocnicze nie przechowują jeszcze starych wartości. Może trzeba je wyzerować przed rozpoczęciem kolejnych obliczeń.

Owszem, jakie współrzędne bym nie wpisał to noga je osiągnie (oczywiście te możliwe opcje ), ale jeśli deklaracja tych współrzędnych występuje tylko raz w kodzie. Dodałem również pętle for(), aby przed każdym kolejnym obliczaniem q, owo q było zerowane - zgadnijcie jaki efekt

Niestety nie mam LCD, aby wyświetlać te zmienne, ale zrobiłem taki myk, że nie są to już stricte zmienne, a ciąg znaków zdefiniowany przez #define. Czyli, przykładowo coś takiego: #define m2 (pow(m1,2) + pow(FootZ,2))

Dodatkowo przerobiłem cały kod, aby był bardziej czytelny. Nie ma żadnych konwersji z radianów na stopnie itp. Najprościej jak się dało.

Zamieszczam również filmy, które nie działały.

https://www.youtube.com/embed/AOwAP9UUfLE

https://www.youtube.com/embed/VUPCqgjFjA4

A tutaj przerobiony kod. Skrócony wraz z wszelkimi waszymi wskazówkami.

#define F_CPU 16000000L
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include <util/delay.h>

//tutaj pseudo zmienne, wszystko te "e-m-y" są wstawiane we wzorach na kinematykę odwrotną
#define     m1		((FootY * pow(sin(q[0]),-1)) - 62.0)
#define	m2		(pow(m1,2) + pow(FootZ,2))
#define	m3		(FootZ * pow(m2,-0.5))
#define	m4		((m2-7500.0) * pow(m2,-0.5) * 0.01)
#define	m5		((m2-12500.0) * 0.0001)

volatile unsigned int ServoPosition = 24500;
volatile unsigned char ServoNr = 0, ServoAdress[] = {PD0,PD1,PD2,PD3,PD4,PD5}, Flag = 0;

ISR(TIMER0_COMP_vect)
{
Flag = 1;              //flagi w przerwaniach
}

ISR(TIMER1_COMPA_vect)
{
Flag = 2;              //flagi w przerwaniach
}

int main(void)
{
int FootX = 130, FootY= 10, FootZ = -35;   //współrzędne stopy
float q[3];     //wyniki po obliczeniu kinematyki odwrotnej

DDRD |= (1<<PD0) | (1<<PD1) | (1<<PD2) | (1<<PD3) | (1<<PD4) | (1<<PD5);	//wyjścia do sterowania serwami

DDRC &= ~(1<<PC0) & ~(1<<PC1) & ~(1<<PC2);	//przyciski START i STOP i położenie A
PORTC |= (1<<PC0) | (1<<PC1) | (1<<PC2);     //pull`up

TCCR0 |= (1<<WGM01);	//Timer 0 -> CTC, prescaler 256
TCNT0 = 0;
OCR0 = 103;		//3,33 ms
TIMSK |= (1<<OCIE0);	//przerwania T0

TCCR1B |= (1<<WGM12);	//Timer 1 -> CTC, prescaler 1
TCNT1 = 0;
TIMSK |= (1<<OCIE1A);	//przerwania T1

sei();

_delay_ms(100);            //opóźnienie dla stabilizacji

while(1)
{
	switch(Flag)             //to co wcześniej było w przerwaniach
	{
		case 1: {
					OCR1A = ServoPosition;
				  	PORTD |= (1<<ServoAdress[ServoNr]);
				  	TCCR1B |= (1<<CS10);
					Flag = 0;
					break;
				}
		case 2: {
					PORTD &= ~(1<<ServoAdress[ServoNr]);
					TCCR1B &= ~(1<<CS10);
					ServoNr++;
					Flag = 0;
					break;
				}				
	}

	if(!(PINC & 0x01))	//STOP
	{
		TCCR1B &= ~(1<<CS10);
		TCCR0 &= ~(1<<CS02);
		ServoNr = 0;
	}

	if(!(PINC & 0x02))	//START
	{
		TCCR0 |= (1<<CS02);
	}

	if(!(PINC & 0x04))	//położenie A
	{
		FootY = 1;     //jeśli tutaj podam jakąś współrzędną stopy to wszsytko się sypie nawet jak nie wcisnę przycisku, gdy to miejsce jest puste to wszystko gra
	}

	if(ServoNr > 5)
	{
		ServoNr = 0;
	}

	//---------------------Kinematyka odwrotna--------------------

	if(FootY == 0){FootY++;}      //to musi być, bo FootY musi być różne od 0

	for(int i=0;i<=2;i++)
	{
		q[i] = 0;	//zerowanie q
	}

	q[0] = atan2(FootY,FootX);             //obl. q, wszsytko w radianach
	q[1] = asin(m3) + acos(m4);
	q[2] = -acos(m5);

	//------------------------------------------------------------

	q[0] = 0.8726 + q[0];           //tutaj dodajemy kilka radianów
	q[1] = 1.8325 - q[1];            //gdy wszystkie (z prawej strony) q[0]=q[1]=q[2]=0 to noga ma połozenie
	q[2] = 0.5235 - q[2];           //jak na rysunku, który zamieściłem wcześniej

	ServoPosition = (9856 * q[ServoNr]) + 9000;          //to działanie musi być, bo OCR1A jest z zakresu <9000,41000>, odpowiednio max w lewo i max w prawo, 24500 to neutral

}
}

[airgrzybek]

Może masz jakiś błąd w tym sterowaniu PWM. Przy jednej stałej konfiguracji działa to nieźle, ale jak zmieniasz ustawienia to się wszystko sypie. Może to dobra poszlaka i warto to zbadać?

[AE]

Założyłem homonto na matlaba i zaciągnąłem go do pracy. Rozpisałem również notację DH. Zrobiłem model w matlabie i wszystkie obliczenia są prawidłowe - zadane położenia osiągalne.

Posiedziałem jeszcze nad sprawdzeniem ustawień timerów - tutaj akurat znalazłem błąd. Timer 0 taktował z dwa razy większą częstotliwością niż chciałem. Zmieniłem również rozdzielczość - teraz OCR1A zawiera się <1350; 5000>. Innych błędów w ustawieniach nie znalazłem. Po poprawieniu błędu z Timerem 0, noga nadal działa dla niezmieniających się współrzędnych stopy (osiąga położenie zadane i trzyma, bez drgań). Wcześniej noga wymachiwała w każdym kierunku jak podałem gdzieś w kodzie inne współrzędne. Teraz (po poprawieniu błędu z Timerem 0) noga osiąga jakieś tam położenie (oczywiście nie takie jakie zadałem) i mocno drga. Może to jakiś problem z lokowaniem tych zmiennych w pamięci? Są nadpisywane przez inne albo coś w tym stylu? W takich momentach przydałby się oscyloskop Kończą mi się pomysły...

Pozdrawiam

[Treker (Damian Szymański)]

Mi przychodzą tylko do głowy pomysły z nadpisywaniem stosu, przepełnianiem zmiennych lub używanie zmiennych ze starymi wartościami. Może spróbuj cały kod przerobić na funkcję działająca wyłącznie na zmiennych lokalnych zerowanych na początku. Wtedy nie będzie już opcji, aby raz działało, a raz nie...

[AE]

Kolejny niewypał. Po wykorzystaniu lokalnych zmiennych i funkcji do obliczania kinematyki odwrotnej po raz kolejny poległem. Ktoś, komu udało się zrealizować kinematykę odwrotną, mógłby zamieścić jakiś poglądowy schemat blokowy kolejnych kroków wykonywanych przez AVRka? Sprawdziłem już chyba wszystko pod każdym możliwym kątem. Nie mam pojęcia co jest źle. Poległem...

Poniżej jest funkcja, która oblicza kinematykę odwrotną. Później w kodzie głównym wartość zwracana jest przypisywana do ServoPosition, czyli tak -> ServoPosition = ikine(130, 0, -35);. Oczywiście podane współrzędne są osiągalne przez stopę. Po napisaniu tej funkcji noga zachowuje się tak jak wcześniej. Z tą różnicą, że nawet dla nie zmiennych wartości współrzędnych dla stopy, noga ustawią się "jakoś tam" i mocno drży. Jak daję zerowanie wartości q i tab na początku tej funkcji to już w ogóle jest kaplica...

int ikine(int FootX, int FootY, int FootZ)
{	
float q[3], tab[3];

if(!FootY){FootY++;}

tab[0] = atan(FootY/FootX);
tab[1] = asin(m3) + acos(m4);
tab[2] = -acos(m5);

q[0] = 0.67 + tab[0];
q[1] = 1.83 - tab[1];
q[2] = 0.35 - tab[2];

int wynik = (q[ServoNr] * 1118) + 1350;

return wynik;
}

[airgrzybek]

Spróbuj skorzystać z algebraicznej metody odwracania kinematyki. Ja z takiej korzystam w swoim hexapodzie i nie miałem z nią jakiś większych problemów. Nakład obliczeń jest na pewno większy, bo trzeba skorzystać z funkcji trygonometrycznych. Problem można uprościć przez zastosowanie tablicowania. Opis metody znajdziesz na pewno w internecie i w książce Teresy Zielińskiej Maszyny kroczące, polecam do przeczytania całą książkę.

[AE]

Wykorzystałem tablicowanie. Ogólnie obliczanie kinematyki odwrotnej (wyznaczonej metodą geometryczną) wykonałem w exelu, a wartości, które otrzymałem wrzuciłem do tablicy 2-wymiarowej i na jej podstawie steruje serwami. Krok podzielilem na 4 fazy, a każdą faze na 10 odcinków. Na jakiś czas musi wystarczyć. Udało mi się wygenerować pełen krok - jak na razie tylko jednej nogi. Muszę przeprojektować robota, bo nogi się wyginają pod cieżarem własnym konstrukcji.

Jak już wspomniałem, wykorzystałem exela i tablicowanie - efekt poniżej:

Nie wiem czy do końca o to chodziło. Chyba raczej o to, aby avr obliczał kinematykę odwrotną i zamiast od razu sterować serwami, wrzucał obliczone wartości do tablicy dwuwymiarowej, a następnie stąd, pobierane wartości sterują serwami?

Mam duży kłopot z znalezieniem tej książki. Kiedyś ją miałem w rękach, jeszcze na studiach. Nigdzie w księgarnach nie ma, przejrzałem też antykwartiaty i biblioteki do których mam dosęp, peb, chomikuj i wujka google... Ma ktoś tę książke i może udostępnić?

Wielkie dzięki za pomoc

Pozdrawiam

[AE]

Dalej brnąłem w kazamatach kodu. Wygenerowałem chód, ale tylko dla dwóch nóg, bo zabrakło mi już kabelków Wykorzystałem tablicowanie, ruch podzieliłem na kilka faz kroku, a później w odpowiedniej funkcji za pomocą switch() generuje sekwencje kroków. Ogólnie działa, ale kod jest obszerny. Tak z ciekawości... czy koledzy również tak generowali chód? czy robili to bardziej elegancko? Poniżej film (serwa to niestety MG995 - mają istnego parkinsona... , nie to co HITECH-y).

Jeszcze raz dziękuje za pomoc i nie pozostaję mi nic innego jak zrobić nowy korpus i dalej w to brnąć

[Bobby]

AE, takie tylko pytanko mam co do poprzedniego kodu, z którym miałeś problemy - liczysz wszystko w radianach czy stopniach? Bo funkcje trygonometryczne z math.h chcą argument w radianach

[AE]

Jako argumenty funkcji podawałem radiany i następnie mnożyłem przez 180/pi, aby otrzymać stopnie - wychylenia poszczególnych serw. Aktualnie nie bawię się w żadne konwersje i wszystko jest liczone w radianach.