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ą:

• Lego Mindstorms NXT 2.0, dobrze było by mieć duże koła RXC, ale nie będzie to konieczne
• Żyroskop HiTechnic (http://www.robonet.pl/?shop&p=nxt_gyro)

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).