Lego Mindstorms NXT 2.0+żyroskop HiTechnic+RobotC = Segway

Po kilku wieczorach wreszcie udało się skomponować (hardware i software) w całość i oto powstał Segway. Ponieważ już dostałem wiele pytań, jak zbudować takie cudo, przedstawię instrukcję budowy oraz co najważniejsze program Segwaya. Dodam tylko, że jest to wersja pierwsza a w zamierzeniu robot będzie sterowany za pomocą Bluetootha za pomocą telefonu z Windows Mobile i będzie poruszał się do przodu, do tyłu oraz skręcał.





Do zbudowania Segwaya, potrzebne będą:

Następnie należy połączyć dwa serwomoechanizmy, kostkę NXT oraz przypiąć nasz żyrokompas. Powinniśmy otrzymać coś takiego jak na poniższych zdjęciach:







Serwomotory podłączamy do wejść B i C. Żyroskop podłączamy do portu S1.

Teraz przechodzimy do najtrudniejszej części a mianowicie oprogramowania. Na początku próbowałem używać oprogramowania NXT-G dostarczonego przez firmę Lego w zestawie, ale było zbyt wolne i cały algorytm stawał się nieczytelny. Do napisania orpogramowania na Segway polecam programowanie NXC lub RobotC. Wykorzystałem oprogramowanie RobotC. 

Zaprogramowanie Sagwaya to nie taka prosta sprawa, jest wiele problemów z którymi należy się zmierzyć:

  • Pierwszy problem to odczyt wskazań z żyroskopu. Żyroskop to jednoosiowy czujnik obrotu  mierzy prędkość kątową do +/- 360° na sekundę a sam pomiar rejestruje do 300 razy na sekundę. Jednak sam żyroskop po podłączeniu do zestawu NXT, wskazuje już pewną wartość, nazwijmy tą wartość offsetem, u mnie było to około 608. Wartość ta (offset) jest zależny między innymi od temperatury otoczenia. A zatem jeśli zmieni się temperatura, nasz czujnik wskaże zupełnie inne wartości. Dalej opiszę jak sobie z tym radzić.

  • Kolejny problem to serwomechaznimy, otóż okazuje się, że szybkość obrotów zależy między innymi od naładowania baterii. Im lepsze baterie tym szybciej poruszają się serwomotory, musimy kompensować te zmiany.

  • Najważniejszy problem a zarazem wyzwanie to sama regulacja (sterowanie) samymi serwomotorami, tak aby robot zachowywał pozycję pionową. Będziemy "ustawiali" robota do pionu około 100 razy na sekundę.
  
 Poniżej przestawiam napisany program:

task main(){

int ng,gn=607,nw=0,rp=0,rg,lrp=0,rwi=0,md;

nMotorPIDSpeedCtrl[motorB] = mtrNoReg;
nMotorPIDSpeedCtrl[motorC] = mtrNoReg;
bFloatDuringInactiveMotorPWM=true;

// Wyliczenie offsetu i wpisanie go do zmiennej gn
int suma=0,i=0;
int srednia;
for (i=0; i<11; i++)
{
suma=suma+SensorRaw(S1);
}


srednia=suma/11;
gn=srednia;

nxtDisplayCenteredTextLine(1, "%d", gn); //wypisanie offsetu na ekranie


//glowna petla programu
while(true){

ng=SensorRaw(S1)-gn; nw+=ng; //wyliczenie kąta
rp=nMotorEncoder(B); //odczytanie pozycji koła

rg=rp-lrp; lrp=rp; rwi+=rg; //obliczenie prędkość koła
if (abs(rwi)>15){gn-=sgn(rwi); rwi=0;} //kompensacja zyrokompasu


md=(nw+ng*12+rp*5+rg*360)>>5; //kontroler PD dla (kół dużych RXC)
//md=(nw+ng*10+rp*4+rg*200)>>4; //kontroler PD (NXT dla kół małych)


md+=sgn(md)*48; //kompensacja tarcia/baterii

motor[motorB]=motor[motorC]=md; //ustawienie serwomotorów
wait1Msec(10); // pętla około 100 x na sekundę

}
} 

Myślę, że kod źródłowy wyjaśnia działanie programu. Oczywiście dla swojego Segwaya, powinieneś dobrać parametry dla regulatora PD oraz kompensację baterii.

Na koniec przedstawię jeszcze wynik działania programu wraz ze wszytkimi zmiennymi (wykres przy użyciu Matlaba).


4 komentarze:

Łukasz Sadłocha pisze...

Witaj,
Mógłbyś dać więcej informacji do odnośnie kodu? szczególnie co to są w linijce:

md=(nw+ng*12+rp*5+rg*360)>>5;

te apersany, średniki i i gt.
pozdrawiam

Anonimowy pisze...

Trochę po kombinowałem i działa ten kod należy poprawić znaki porównania w pętli for i wyliczeniu md.

'>' oznacza znak '>'
a '<' znak '<'
I jeszcze w 31 linii poprawić
z B na motorB. Osobiście ściągnąłem gotowy program do swojego Segweya w języku NXT-G ze strony Hitechnic.com mamy tam dodane sterowanie kontrolerem irda. Dzięki obrazkowemu programowaniu łatwiej zrozumieć ten kod potem można przejść na język C.

Anonimowy pisze...

& gt ; to >
& lt ; to <

Przygotika pisze...

Aż trudno uwierzyć, że to dzieło powstało z klocków lego.