[MOC] Automatyczna 4-biegowa skrzynia biegów NXT
: 2012-02-25, 15:42
Język: ROBOTC
Kostka: 1
Silniki: 3 x NXT
Sensory: żadnych
Moja pierwsza nieśmiała wyprawa w świat NXT. Szukałem okazji żeby poćwiczyć trochę ROBOTC, które jest dla mnie kompletną nowością, i przyszedł mi do głowy pomysł na skrzynię biegów która zmienia przełożenie sama, w oparciu o zmiany prędkości silnika który ją napędza. Prosty układ: kostka, 3 silniki, żadnych sensorów, więc nie mogłem tego za bardzo spaprać :)
Jako podstawa konstrukcji użyta jest ta skrzynia: http://sariel.pl/2008/12/4-speed-manual ... t-gearbox/
Dodałem do niej dwa silniki NXT które zmieniają biegi, oraz trzeci który ją napędza.
Program kontrolujący skrzynię - mój pierwszy w ROBOTC, proszę się nie śmiać - składa się z prostego wątku głównego z dwoma podwątkami, z których jeden monitoruje napięcie w baterii kostki (bo zainteresowało mnie jak potrafi ono skakać w zależności od obciążenia silników), a drugi przejmuje obsługę przycisków na kostce. Wątek główny kontroluje wszystkie silniki. Po uruchomieniu silnika napędowego, jego prędkość jest mierzona w stopniach obrotu na 0.1 sekundy. Pomiar jest banalnie prosty i odbywa się poprzez pętlę która co 0.1 sekund odczytuje licznik obrotu w silniku i go resetuje. Zależnie od stanu baterii i biegu wybranego w skrzyni, silnik wyciąga w przedziale 70-80 stopni na 0.1 sekundy. Kiedy prędkość silnika przekroczy górną lub dolną wartość skrajną, następuje zmiana biegu odpowiednio na wyższy lub niższy. Po zmianie program odczekuje chwilę żeby silnik zdążył "zaskoczyć" z nowym przełożeniem, a potem wraca do monitorowania. Trzy zmienne: obie wartości skrajne prędkości oraz okres oczekiwania po zmianie biegu - są deklarowane zaraz na początku programu, dla jego wygodnej konfiguracji.
Sama skrzynia jest zsynchronizowana i nieliniowa, da się ją więc przełączyć w dowolny sposób. Żeby nie przesadzić z ilością możliwych kombinacji, program ma przełączać tylko między najbliższymi biegami - więc z trójki może przejść na dwójkę albo czwórkę, ale nie na jedynkę. Wyjątkiem jest sytuacja kiedy program się wyłącza. Po naciśnięciu ciemnoszarego przycisku na kostce następuje zamknięcie programu, ale najpierw skrzynia jest przestawiana na jedynkę. Żeby to było możliwe, program musi cały czas śledzić aktualnie wybrany bieg - odpowiada za to zmienna gear, aktualizowana przy każdej zmianie biegu. W filmie widać jak skrzynia schodzi na jedynkę z czwórki, trójki i dwójki.
Jak wspomniałem, jest to mój pierwszy kontakt z ROBOTC i jestem zielony w temacie, program pewnie dałoby się napisać inaczej i krócej. Co mnie zdziwiło, to że kontrolowanie silników zmieniających biegi okazało się dokładniejsze przy użyciu funkcji czasowej niż przy obracaniu o zadany kąt. Silniki łatwo się "rozjeżdżały" i blokowały skrzynię kiedy ich pozycja była zmieniana w oparciu o kąt, natomiast przy dokładnym ustawieniu czasu w jakim mają pracować udało się uzyskać znacznie większą precyzję ruchów. To ważne, bo silniki NXT mają ogromny moment i potrafią wcisnąć driving ringa w zębatkę tak mocno że tarcie prawie zatrzymuje skrzynię.
Program - w czytelniejszej wersji, z tabulacją i kolorowanie składni do obejrzenia tutaj: http://sariel.pl/2012/02/nxt-automated-gearbox/
Kod: Zaznacz cały
// config start
int mspeed_high = 78;
int mspeed_low = 75;
int shift_time = 3000;
// config end
int gear = 1;
task Buttons()
{
while(true)
{
nNxtButtonTask = -2;
nNxtExitClicks = 2;
if(nNxtButtonPressed == 0)
{
eraseDisplay();
motor[motorA] = 0;
switch(gear)
{
case 1:
break;
case 2:
motor[motorB] = -100;
wait1Msec(120);
motor[motorC] = 0;
break;
case 3:
motor[motorC] = 100;
wait1Msec(60);
motor[motorC] = 0;
wait1Msec(500);
motor[motorB] = -100;
wait1Msec(70);
motor[motorB] = 0;
break;
case 4:
motor[motorC] = -100;
wait1Msec(60);
motor[motorC] = 0;
wait1Msec(500);
motor[motorB] = -100;
wait1Msec(70);
motor[motorB] = 0;
break;
}
StopAllTasks();
}
}
return;
}
task BatLev()
{
while(true)
{
nxtDisplayCenteredTextLine(0, "Brick up @ %3.1fV", nImmediateBatteryLevel / (float) 1000);
wait1Msec(50);
}
return;
}
task main ()
{
StartTask(Buttons);
StartTask(BatLev);
motor[motorA] = 0;
bool mstatus = false;
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
nxtDisplayCenteredTextLine(2, "MOTOR A OFF");
nxtDisplayCenteredTextLine(4, "GEAR 1");
wait1Msec(50);
while(true)
{
if(nNxtButtonPressed == 3)
{
if (mstatus == true)
{
PlaySound(soundBlip);
motor[motorA] = 0;
mstatus = false;
nxtDisplayCenteredTextLine(2, "MOTOR A OFF");
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(7);
wait1Msec(500);
}
else
{
motor[motorA] = 100;
mstatus = true;
nxtDisplayCenteredTextLine(2, "MOTOR A ON");
wait1Msec(500);
int mspeed = nMotorEncoder[motorA];
while(nNxtButtonPressed != 3)
{
nMotorEncoder[motorA] = 0;
wait1Msec(100);
mspeed = nMotorEncoder[motorA];
nxtDisplayCenteredTextLine(5, "Speed: %d", mspeed);
if (mspeed > mspeed_high) // gearing up
{
nxtDisplayCenteredTextLine(7, "Gearing up...");
switch(gear)
{
case 1:
motor[motorB] = 100;
wait1Msec(110);
motor[motorB] = 0;
gear = 2;
nxtDisplayCenteredTextLine(4, "GEAR 2");
wait1Msec(shift_time);
break;
case 2:
gear = 3;
motor[motorB] = -100;
wait1Msec(60);
motor[motorB] = 0;
wait1Msec(300);
motor[motorC] = -100;
wait1Msec(60);
motor[motorC] = 0;
nxtDisplayCenteredTextLine(4, "GEAR 3");
wait1Msec(shift_time);
break;
case 3:
motor[motorC] = 100;
wait1Msec(110);
motor[motorC] = 0;
gear = 4;
nxtDisplayCenteredTextLine(4, "GEAR 4");
wait1Msec(shift_time);
break;
case 4:
nxtDisplayClearTextLine(7);
break;
}
}
else if (mspeed < mspeed_low) // gearing down
{
nxtDisplayCenteredTextLine(7, "Gearing down...");
switch(gear)
{
case 4:
motor[motorC] = -100;
wait1Msec(110);
motor[motorC] = 0;
gear = 3;
nxtDisplayCenteredTextLine(4, "GEAR 3");
wait1Msec(shift_time);
break;
case 3:
motor[motorC] = 100;
wait1Msec(60);
motor[motorC] = 0;
wait1Msec(300);
motor[motorB] = 100;
wait1Msec(60);
motor[motorB] = 0;
gear = 2;
nxtDisplayCenteredTextLine(4, "GEAR 2");
wait1Msec(shift_time);
break;
case 2:
motor[motorB] = -100;
wait1Msec(110);
motor[motorB] = 0;
gear = 1;
nxtDisplayCenteredTextLine(4, "GEAR 1");
wait1Msec(shift_time);
break;
case 1:
nxtDisplayClearTextLine(7);
break;
}
}
}
}
}
}
}
Film:
[youtube]http://www.youtube.com/watch?v=o_4GuD8VH4c[/youtube]