[MOC] Automatyczna 4-biegowa skrzynia biegów NXT

Mindstorms, WeDo, Lego Digital Desinger, Stud.io

Moderatorzy: Mod Team, Mod Team

Autor
Wiadomość
Awatar użytkownika
Sariel
VIP
Posty: 5418
Rejestracja: 2007-03-28, 08:16
Lokalizacja: Warszawa
brickshelf: Sariel
Kontakt:

 

[MOC] Automatyczna 4-biegowa skrzynia biegów NXT

#1 Post autor: Sariel »

Obrazek

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 &#40;mspeed < mspeed_low&#41; // gearing down 
          &#123; 
            nxtDisplayCenteredTextLine&#40;7, "Gearing down..."&#41;; 
            switch&#40;gear&#41; 
            &#123; 
              case 4&#58; 
                motor&#91;motorC&#93; = -100; 
                wait1Msec&#40;110&#41;; 
                motor&#91;motorC&#93; = 0; 
                gear = 3; 
                nxtDisplayCenteredTextLine&#40;4, "GEAR 3"&#41;; 
                wait1Msec&#40;shift_time&#41;; 
                break; 
 
              case 3&#58; 
                motor&#91;motorC&#93; = 100; 
                wait1Msec&#40;60&#41;; 
                motor&#91;motorC&#93; = 0; 
                wait1Msec&#40;300&#41;; 
                motor&#91;motorB&#93; = 100; 
                wait1Msec&#40;60&#41;; 
                motor&#91;motorB&#93; = 0; 
                gear = 2; 
                nxtDisplayCenteredTextLine&#40;4, "GEAR 2"&#41;; 
                wait1Msec&#40;shift_time&#41;; 
                break; 
 
              case 2&#58; 
                motor&#91;motorB&#93; = -100; 
                wait1Msec&#40;110&#41;; 
                motor&#91;motorB&#93; = 0; 
                gear = 1; 
                nxtDisplayCenteredTextLine&#40;4, "GEAR 1"&#41;; 
                wait1Msec&#40;shift_time&#41;; 
                break; 
 
              case 1&#58; 
                nxtDisplayClearTextLine&#40;7&#41;; 
                break; 
            &#125; 
          &#125; 
        &#125; 
      &#125; 
    &#125; 
  &#125; 
&#125;
Foty:
Obrazek Obrazek Obrazek

Film:
[youtube]http://www.youtube.com/watch?v=o_4GuD8VH4c[/youtube]
Ostatnio zmieniony 2012-02-25, 16:29 przez Sariel, łącznie zmieniany 1 raz.

Awatar użytkownika
Mrosik
Posty: 276
Rejestracja: 2009-05-05, 08:29
Lokalizacja: Poznań
brickshelf: Mros85
Kontakt:

 

#2 Post autor: Mrosik »

A jakby działała w praktyce? Możesz ją zamontować na jakimś prymitywnym podwoziu i pokazać jej działanie/skuteczność?

Jetboy

#3 Post autor: Jetboy »

Fajne!
Jedno co można poprawić to wcięcia w kodzie, byłby dzięki emu znacznie bardziej czytelny.
Ostatnio zmieniony 2012-02-25, 16:37 przez Jetboy, łącznie zmieniany 1 raz.

Awatar użytkownika
Sariel
VIP
Posty: 5418
Rejestracja: 2007-03-28, 08:16
Lokalizacja: Warszawa
brickshelf: Sariel
Kontakt:

 

#4 Post autor: Sariel »

Jetboy, zapomniałem użyć code zamiast quote, dzięki :P
Mrosik, w praktyce działałaby tak samo jak na filmie - widzisz co się dzieje jak generuję opór na wyjściu ręką. Tylko na filmowanie na podwoziu byłoby trudniejsze.

Awatar użytkownika
TT
Posty: 1181
Rejestracja: 2004-05-27, 13:52
Lokalizacja: Bielsko-Biała
brickshelf: TT

 

#5 Post autor: TT »

Paweł gratuluje debiutu w NXT ! :-)
Skrzynia działa bardzo dobrze. Trudno sobie wyobrazić tak sprawną skrzynię automatyczną LEGO bez udziału Mindstorms.

Jeśli chodzi o precyzję silników przy wykorzystaniu ich enkoderów do pozycjonowania to polecam Ci zainteresować się funkcją: nMotorEncoderTarget[]

U początku mojej kariery zmontowałem skrzynię automatyczną - zasada działania taka sama jak u Sariela. Dla zainteresowanych odsyłam do archaicznego filmu: http://www.youtube.com/watch?v=1IGsOswc ... ature=plcp (początek to pokaz działa trybu sekwencyjnego, potem załączam automat).
Ostatnio zmieniony 2012-02-25, 17:34 przez TT, łącznie zmieniany 1 raz.
Pozdrawiam,
Tomek

Awatar użytkownika
Emilus
Adminus Emeritus
Posty: 1460
Rejestracja: 2007-08-26, 19:58
Lokalizacja: Polska
brickshelf: Emilus
Kontakt:

 

#6 Post autor: Emilus »

Fajnie Paweł, super że startujesz w mindstorms.

Skrzynia działa, super. Szkoda tylko, że aby mogła pracować w trybie automatycznym, do napędu konieczny jest motor NXT. Ale rozumiem, nie używałeś czujników dlatego tak jest.

Awatar użytkownika
Atros
VIP
Posty: 801
Rejestracja: 2009-04-08, 19:03
Lokalizacja: Warszawa
brickshelf: Atrx
Kontakt:

 

Re: Automatyczna 4-biegowa skrzynia biegów NXT

#7 Post autor: Atros »

Witamy w świecie NXT :)

Skrzynia i idea działania bardzo prosta, też kiedyś testowałem coś takiego. Na stołku działa to fajnie ale po włożeniu do samochodu trudno jest dopasować momenty w którym biegi powinny być zmieniane.
Sariel pisze: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.
Takie działanie jest dziwne i niepokojące, obracanie silnika o kąt powinno być idealnie dokładne, czasowe ustawianie nigdy nie będzie tak dokładne ponieważ występują wahania napięcia i zmiany tarcia co ma wpływ na wynikową wartość obrotu.
W leJOSie działa to tak jak należy, w robotc też zapewne działa to poprawnie tylko coś skopałeś ;)

Awatar użytkownika
Emilus
Adminus Emeritus
Posty: 1460
Rejestracja: 2007-08-26, 19:58
Lokalizacja: Polska
brickshelf: Emilus
Kontakt:

 

#8 Post autor: Emilus »

Takie działanie jest dziwne i niepokojące, obracanie silnika o kąt powinno być idealnie dokładne, czasowe ustawianie nigdy nie będzie tak dokładne ponieważ występują wahania napięcia i zmiany tarcia co ma wpływ na wynikową wartość obrotu.
W leJOSie działa to tak jak należy, w robotc też zapewne działa to poprawnie tylko coś skopałeś ;)
Nigdy nie będzie idealnie dokładne, gdyż encoder w silniku NXT nie znajduje się przy wyjściu na ośkę a ileś zębatek wcześniej i niestety ustawianie silnika na kąt ma sporą bezwładność.
Ratunek to albo formuła "motortarget" ale ona hamuje na końcu silnik w określonej pozycji (więc zużywa cały czas prąd) albo finezja programisty :) Ja to robię na przedziałach z wykorzystaniem encodera w silniku i z płynnym zmniejszaniem prędkości silnika przy dochodzeniu do punktu "stop", aby zminimalizować bezwładność.

Awatar użytkownika
ontek
VIP
Posty: 202
Rejestracja: 2011-05-30, 20:41
Lokalizacja: Rzeszów
brickshelf: ontek

 

#9 Post autor: ontek »

Ostatnio w moich badaniach zajmowałem się sensoryką Lego. Jeżeli chodzi o te serwomechanizmy to jest to koszmar, przede wszystkim z bezwładnością oraz precyzją. Miałem też do czynienia z żyroskopem i akcelerometrem produkcji Hitechnic i też wygląda to tragicznie. Żeby skalibrować sensory aby poprawnie działały, to należy poświęcić kilka dni z życia. Sensory dają różne wskazania w zależności od temperatury, poziomu baterii, ciśnienia atmosferycznego, a nawet wilgotności powietrza, nie wspominając o czasie próbkowania.
Ostatnio zmieniony 2012-02-26, 10:44 przez ontek, łącznie zmieniany 2 razy.

Awatar użytkownika
Atros
VIP
Posty: 801
Rejestracja: 2009-04-08, 19:03
Lokalizacja: Warszawa
brickshelf: Atrx
Kontakt:

 

#10 Post autor: Atros »

Emilus pisze:Nigdy nie będzie idealnie dokładne, gdyż encoder w silniku NXT nie znajduje się przy wyjściu na ośkę a ileś zębatek wcześniej i niestety ustawianie silnika na kąt ma sporą bezwładność.
A to tego to wiadomo, że nie przeskoczymy.
Emilus pisze:Ratunek to albo formuła "motortarget" ale ona hamuje na końcu silnik w określonej pozycji (więc zużywa cały czas prąd) albo finezja programisty :) Ja to robię na przedziałach z wykorzystaniem encodera w silniku i z płynnym zmniejszaniem prędkości silnika przy dochodzeniu do punktu "stop", aby zminimalizować bezwładność.
W takim razie jest możliwość, że lejos lepiej radzi sobie ze sterowaniem serwami.
Tu jest np. metoda rotateTo która przy max prędkości silnika gdy zadam rotację do kąta 320 stopni obraca silnik z regulacją czyli silnik obróci się do max 322-323 stopni i potem wyreguluje do dokładnie 320, regulacja ta trwa dosłownie milisekundy.

Napiszę jeszcze o zmianie biegów an driving ringach bo trochę nad tym posiedziałem w [URL=http://www.00453005_0000002.pl/forum/viewtopic.php?t=11559]Volvo[/URL]. Żeby driving ringi nie blokwały się napisałem specjalną metodę zmieniającą biegi i wykrywającą, czy driving ring nie trafił akurat na tą wypustkę w tej z16. W nowej wersji lejosa metoda sprawdzająca zablokowanie silnika jest już w standardowej bibliotece.

Awatar użytkownika
Sariel
VIP
Posty: 5418
Rejestracja: 2007-03-28, 08:16
Lokalizacja: Warszawa
brickshelf: Sariel
Kontakt:

 

#11 Post autor: Sariel »

Ja myślałem o metodzie zatrzymującej silnik w momencie kiedy licznik obrotów w nim się zatrzyma. Czyli jak silnik trafi na opór, to go wyłączamy. Co do wyczucia kiedy skrzynię przełączyć, przeszło mi przez myśl że możnaby napisać algorytm który odpala silnik na jedynce, daje mu chwilę pochodzić, a potem wylicza sobie średnią prędkość i procentowe progi odchyleń od niej, co przyjmuje za podstawę zmiany biegów. Ale nie chciałem przekombinować, w końcu to mój debiut i jak się jest takim leszczem to trzeba po kroczku :)
Chciałbym jeszcze podziękować TT i Emilowi za wciągnięcie w świat NXT. Mój portfel Was nienawidzi, ale tak w ogóle to dzięki chłopaki ;)

Awatar użytkownika
Neo
Posty: 293
Rejestracja: 2011-04-29, 21:24
Lokalizacja: Kraków/Rzeszów/Starachowice

#12 Post autor: Neo »

Bardzo sympatyczna koncepcja. Ale nie kryguj się tak kolego - program nie jest skomplikowany, ale napewno wykracza poza słowo "prosty". Musiałeś mieć wcześniej kontakt przynajmniej z C :)

Awatar użytkownika
Emilus
Adminus Emeritus
Posty: 1460
Rejestracja: 2007-08-26, 19:58
Lokalizacja: Polska
brickshelf: Emilus
Kontakt:

 

#13 Post autor: Emilus »

ontek pisze:Ostatnio w moich badaniach zajmowałem się sensoryką Lego i jeżeli chodzi o te serwomechanizmy to jest koszmar w ogóle z bezwładnością oraz precyzją.
Bez przesady. Można sobie spokojnie poradzić. Kwestia sprawności mechanika/programisty.

Co do podziękowań, to nie ma za co Paweł. Ale jestem pewien, że sam prędzej czy później byś w to się wciągnął. Ale wiadomo, lepiej prędzej :)
Ostatnio zmieniony 2012-02-25, 21:20 przez Emilus, łącznie zmieniany 1 raz.

Awatar użytkownika
Sariel
VIP
Posty: 5418
Rejestracja: 2007-03-28, 08:16
Lokalizacja: Warszawa
brickshelf: Sariel
Kontakt:

 

#14 Post autor: Sariel »

Neo pisze:Musiałeś mieć wcześniej kontakt przynajmniej z C :)
Z PHP i z jQuery. Stąd pewne moje niezdecydowanie jaki język wybrać - jedno podobne do ROBOTC, drugie do leJOSa. Mam nadzieję, że leJOSem też będę miał szansę się pobawić.

Awatar użytkownika
TT
Posty: 1181
Rejestracja: 2004-05-27, 13:52
Lokalizacja: Bielsko-Biała
brickshelf: TT

 

#15 Post autor: TT »

ontek pisze:Ostatnio w moich badaniach zajmowałem się sensoryką Lego i jeżeli chodzi o te serwomechanizmy to jest koszmar w ogóle z bezwładnością oraz precyzją. Miałem tez do czynienia z żyroskopem i akcelerometrem produkcji hitechnic i też tragedia, żeby to skalibrować żeby poprawnie działało to dosłownie pare dni z życia wyjętych bo są inne wskazania w zależności od temperatury, poziomu baterii, ciśnienia a nawet wilgotności powietrza, nie wspominając o czasie próbkowania.
Fakt, LEGO nie nada się do chirurgi precyzyjnej. W połączeniu z klockami, których sama mechanika powoduje "spore" luzy to i tak to wszystko nawet ładnie działa.
Atr pisze:W takim razie jest możliwość, że lejos lepiej radzi sobie ze sterowaniem serwami.
Tu jest np. metoda rotateTo która przy max prędkości silnika gdy zadam rotację do kąta 320 stopni obraca silnik z regulacją czyli silnik obróci się do max 322-323 stopni i potem wyreguluje do dokładnie 320, regulacja ta trwa dosłownie milisekundy.
W RobotC jest prosta funkcja nMotorEncoderTarget[], która ma zaimplementowany regulator PID. Proporcje nastawów P,I,D można nawet podglądać w debugerze. Działa bardzo precyzyjnie, nie ma dużych przeregulowań i jest dobra dynamika. Jako, że RobotC to najszybsza platforma pod NXT to wierzę, że ta regulacja jest jedną z najlepszych osiąganych na NXT. Oczywiście można sobie samemu zaprogramować regulator i dobrać nastawy pod konkretny projekt. Wszystko się da w końcu to NXT!
Sariel pisze:Chciałbym jeszcze podziękować TT i Emilowi za wciągnięcie w świat NXT. Mój portfel Was nienawidzi, ale tak w ogóle to dzięki chłopaki ;)
Paweł ja już widzę, że NXT zdominuje Twoje moce w roku kalendarzowym 2012 :-)
Mam nadzieje, że Twój portfel posiada duże zdolności regeneracyjne - pamiętaj, że przed zakupem ostrzegałem, że na jednym zestawie na pewno się nie skończy ;-)
Ostatnio zmieniony 2012-02-25, 22:11 przez TT, łącznie zmieniany 1 raz.
Pozdrawiam,
Tomek

ODPOWIEDZ