[MOC] Pilot kostka-kostka

Mindstorms, WeDo, Lego Digital Desinger, Stud.io

Moderator: Mod Team

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

 

[MOC] Pilot kostka-kostka

#1 Post autor: Sariel »

Obrazek

Język: ROBOTC
Kostki: 2
Silniki: 5 x NXT
Sensory: 2 x dotyk

Prosty pilot pozwalający przez łącze Bluetooth kontrolować jedną kostką silniki podłączone do drugiej kostki. Nic specjalnego, bo niektórzy trialowcy robili takie rzeczy już lata temu i nawet nie uznali za stosowne się tym pochwalić ;) ale na czymś muszę się uczyć.

Pilot miał być w założeniu prosty jak konstrukcja cepa, i pozwalać na kontrolę prędkości napędu oraz proporcjonalne sterowanie. Instrukcja budowy samego pilota poniżej, do działania natomiast potrzebny jest dwuczęściowy program - jedna część odpalona na kostce-nadawcy, druga część na kostce-odbiorcy. Całość wykorzystuje proste przesyłanie wiadomości w ROBOTC 3.05, co oznacza że kostki muszą się już wcześniej "znać" zanim się połączą. Komunikacja odbywa się w dwie strony, dzięki czemu kostka-nadawca podaje aktualne napięcie w baterii kostki-odbiorcy. Drobiazg, ale jednocześnie marzenie każdego lenia - zdalne sprawdzanie ile pałeru zostało jeszcze w bateriach :)

Silnik napędowy, podłączony do portu A kostki-odbiorcy, jest sterowany z kontrolą prędkości za pomocą wajchy po lewej stronie pilota. Ruch wajchy jest ograniczony mechanicznie, co pozwala uprościć program. Opcjonalnie kostka-odbiorca może brzęczeć przy cofaniu, co może się nadać do modeli ciężarówek czy autobusów. Przydaje się tu także zdalna regulacja głośności kostki-odbiorcy za pomocą przycisków strzałek na kostce-nadawcy.

Silnik do skrętu, podłączony do portu B kostki-odbiorcy, jest sterowany proporcjonalnie, co znaczy że zachowuje ten sam kąt co kierownica po prawej stronie pilota. Tak jak z wajchą do napędu, ruch kierownicy również jest ograniczony mechanicznie dla uproszczenia. Margines błędu wynosi 10 stopni i został przyjęty żeby uwzględnić bezwładność silników NXT. Zbyt niski margines błędu może sprawić że silnik dostanie czkawki próbując bezskutecznie ustawić się dokładnie na pożądanym kącie. Margines można dopasować w konfiguracji programu, znajdującej się na początku części odbiorczej.

Trzeci motor, podłączony do portu C kostki-odbiorcy, reaguje na wciskanie i przytrzymanie czujników dotyku. Tutaj bez żadnych bajerów.

Pomarańczowy przycisk na kostce-nadawcy włącza i wyłącza wysyłanie danych - po uruchomieniu programu jest ono domyślnie wyłączone, pozwalając dłubać w pilocie bez ryzyka zjechania MOCem ze stołu. W momencie wyłączenia wysyłania kostka resetuje wszystkie dane z wyjątkiem poziomu głośności, co zatrzymuje wszystkie silniki i centruje silnik do skrętu. Po ponownym włączeniu przesyłane są aktualne dane, a więc np. silnik do skrętu obróci się do aktualnej pozycji kierownicy.

Całość jest prosta i napisana przez początkującego :) ale mam nadzieję, że komuś się przyda.

Fotki:
Obrazek Obrazek

Film:
[youtube]http://www.youtube.com/watch?v=n6ZZw8j8mug[/youtube]

Instrukcja:
Obrazek Obrazek Obrazek Obrazek Obrazek Obrazek Obrazek

Program nadawczy:

Kod: Zaznacz cały

#pragma config(Sensor,S1,touch1,sensorTouch) 
#pragma config(Sensor,S2,touch2,sensorTouch) 
 
bool sending = false; 
int soundLevel = 1; 
 
task BatLev() 
{ 
  ubyte valueReceived[1]; 
  valueReceived[0] = 0; 
  while(true) 
  { 
    cCmdMessageRead(valueReceived, 1, 1); 
    nxtDisplayCenteredTextLine(0, "BAT: L%3.1f / R%3.1f", nImmediateBatteryLevel / (float) 1000, valueReceived[0] / (float) 10); 
    wait1Msec(50); 
  } 
  return; 
} 
 
task Buttons() 
{ 
  nNxtButtonTask  = -2; 
  nNxtExitClicks = 2; 
  nxtDisplayCenteredTextLine(1, "Not sending..."); 
  while(true) 
  { 
    if(nNxtButtonPressed == 3) 
    { 
      PlaySound(soundBeepBeep); 
      if(sending == false){ 
        sending = true; 
        nxtDisplayCenteredTextLine(1, "Sending..."); 
      } 
      else{ 
        sending = false; 
        ubyte valueToReset[5]; 
        valueToReset[0] = 0; 
        valueToReset[1] = 0; 
        valueToReset[2] = 0; 
        valueToReset[3] = 0; 
        valueToReset[4] = soundLevel; 
        cCmdMessageWriteToBluetooth(valueToReset, 5, 1); 
        nxtDisplayCenteredTextLine(1, "Not sending..."); 
      } 
    } 
    else if &#40;nNxtButtonPressed == 1 && soundLevel < 3&#41; soundLevel = soundLevel + 1; 
    else if &#40;nNxtButtonPressed == 2 && soundLevel > 0&#41; soundLevel = soundLevel - 1; 
  wait1Msec&#40;350&#41;; 
  &#125; 
&#125; 
 
task main&#40;&#41; 
&#123; 
  StartTask&#40;BatLev&#41;; 
  StartTask&#40;Buttons&#41;; 
  bFloatDuringInactiveMotorPWM = true; 
  btConnect&#40;1, "NXT2"&#41;; // connection&#58; port, brick 
  while&#40;true&#41; 
  &#123; 
    ubyte valueToSend&#91;5&#93;; 
    valueToSend&#91;0&#93; = nMotorEncoder&#91;motorA&#93;; 
    valueToSend&#91;1&#93; = nMotorEncoder&#91;motorB&#93;; 
    valueToSend&#91;2&#93; = SensorValue&#40;touch1&#41;; 
    valueToSend&#91;3&#93; = SensorValue&#40;touch2&#41;; 
    valueToSend&#91;4&#93; = soundLevel; 
    if&#40;sending == true&#41;&#123;cCmdMessageWriteToBluetooth&#40;valueToSend, 5, 1&#41;;&#125; 
    nxtDisplayTextLine&#40;3, "Drive&#58; %d", valueToSend&#91;0&#93;&#41;; 
    nxtDisplayTextLine&#40;4, "Steering&#58; %d", valueToSend&#91;1&#93;&#41;; 
    nxtDisplayTextLine&#40;5, "Touch BLUE&#58; %d", valueToSend&#91;2&#93;&#41;; 
    nxtDisplayTextLine&#40;6, "Touch RED&#58; %d", valueToSend&#91;3&#93;&#41;; 
    nxtDisplayTextLine&#40;7, "Volume&#58; %d", valueToSend&#91;4&#93;&#41;; 
    wait1Msec&#40;50&#41;; 
  &#125; 
&#125;
Program odbiorczy:

Kod: Zaznacz cały

// config start 
//bMotorReflected&#91;motorA&#93; = true; 
//bMotorReflected&#91;motorB&#93; = true; 
//bMotorReflected&#91;motorC&#93; = true; 
bool buzzOnRev = true; // buzzing while on reverse 
int steerAcc = 10; // steering accuracy margin &#40;degrees&#41; 
// config end 
 
task BatLev&#40;&#41; 
&#123; 
  while&#40;true&#41; 
  &#123; 
    nxtDisplayCenteredTextLine&#40;0, "BAT&#58; L %3.1fV", nImmediateBatteryLevel / &#40;float&#41; 1000&#41;; 
    ubyte valueToSend&#91;1&#93;; 
    valueToSend&#91;0&#93; = nImmediateBatteryLevel / &#40;float&#41; 100; 
    cCmdMessageWriteToBluetooth&#40;valueToSend, 1, 1&#41;; 
    wait1Msec&#40;50&#41;; 
  &#125; 
  return; 
&#125; 
 
task main&#40;&#41; 
&#123; 
  StartTask&#40;BatLev&#41;; 
  nVolume = 1; // starting volume 
  ubyte valueReceived&#91;5&#93;; 
  valueReceived&#91;0&#93; = 0; 
  valueReceived&#91;1&#93; = 0; 
  valueReceived&#91;2&#93; = 0; 
  valueReceived&#91;3&#93; = 0; 
  valueReceived&#91;4&#93; = 1; 
 
  while&#40;true&#41; 
  &#123; 
    cCmdMessageRead&#40;valueReceived, 5, 1&#41;; 
    nxtDisplayTextLine&#40;2, "Drive&#58; %d", valueReceived&#91;0&#93;&#41;; 
    nxtDisplayTextLine&#40;3, "Steering&#58; %d", valueReceived&#91;1&#93;&#41;; 
    nxtDisplayTextLine&#40;4, "Touch BLUE&#58; %d", valueReceived&#91;2&#93;&#41;; 
    nxtDisplayTextLine&#40;5, "Touch RED&#58; %d", valueReceived&#91;3&#93;&#41;; 
    nxtDisplayTextLine&#40;6, "Volume&#58; %d", valueReceived&#91;4&#93;&#41;; 
    nVolume = valueReceived&#91;4&#93;; 
 
    // motorA &#40;drive&#41; 
    motor&#91;motorA&#93; = 0; 
    int afactor = valueReceived&#91;0&#93; / 5; 
    if &#40;afactor > 1 && afactor < 15&#41; 
    &#123; 
      afactor = afactor * 10; 
      if &#40;afactor > 100&#41; afactor = 100; 
      motor&#91;motorA&#93; = afactor; 
      if &#40;buzzOnRev == true&#41; ClearSounds&#40;&#41;; 
    &#125; 
    else if &#40;afactor > 1 && afactor > 25&#41; 
    &#123; 
      afactor = &#40;51 - afactor&#41; * -10; 
      if &#40;afactor < -100&#41; afactor = -100; 
      motor&#91;motorA&#93; = afactor; 
      if &#40;buzzOnRev == true&#41; PlaySound&#40;soundException&#41;; 
    &#125; 
 
    // motorB &#40;steering&#41; 
    motor&#91;motorB&#93; = 0; 
    int bfactor = valueReceived&#91;1&#93;; 
    if &#40;bfactor > 1 && bfactor < 75&#41; bfactor = bfactor; 
    else if &#40;bfactor > 1 && bfactor > 75&#41; bfactor = &#40;256 - bfactor&#41; * -1; 
    nxtDisplayTextLine&#40;7,"Mtr B&#58; %d/%d",nMotorEncoder&#91;motorB&#93;, bfactor&#41;; 
 
    if &#40;nMotorEncoder&#91;motorB&#93; < &#40;bfactor + steerAcc&#41;&#41; while&#40;nMotorEncoder&#91;motorB&#93; < bfactor&#41; motor&#91;motorB&#93; = 10; 
    else if &#40;nMotorEncoder&#91;motorB&#93; > &#40;bfactor - steerAcc&#41;&#41; while&#40;nMotorEncoder&#91;motorB&#93; > bfactor&#41; motor&#91;motorB&#93; = -10; 
    else motor&#91;motorB&#93; = 0; 
 
    // motorC 
    motor&#91;motorC&#93; = 0; 
    if &#40;valueReceived&#91;2&#93; == 1&#41; motor&#91;motorC&#93; = 100; 
    else if &#40;valueReceived&#91;3&#93; == 1&#41;motor&#91;motorC&#93; = -100; 
 
    wait1Msec&#40;50&#41;; 
  &#125; 
&#125;

Jetboy

#2 Post autor: Jetboy »

Dzięki! To się na pewno każdemu zaczynającemu przygodę z NXT przyda.

Na początku filmiku widać że jest pewne opóźnienie w obrotach silnika sterowanego kierownicą. Jak to się sprawuje w praktyce? Da się jakoś sensownie kierować przy większych prędkościach niż widać na filmiku?

Nie wiem czy to dobre miejsce, ale mam kilka dodatkowych pytań dotyczących programowania NXT więc jako początkujący nie wiem czy będziesz w stanie na nie odpowiedzieć (ale Emilus pewnie tak):

Widzę że przesyłasz stan całego układu w jednej paczce. Orientuje się ktoś z was jak tu wyglądają narzuty na transmisje? Jaki jest maksymalna ilość danych jaką można przesłać jednym poleceniem? Haki wpływ ma ilość danych na czas transmisji?

Ile trwa zczytanie wartości z poszczególnych czujników/silników?

W zależności od odpowiedzi na te pytania mogę mieć jakieś pomysły na optymalizacje.

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

 

#3 Post autor: Sariel »

Jest pewne opóźnienie, nie wiem z czego ono wynika. Wysyłanie i odczytywanie wiadomości odbywa się w pętli z powtórzeniem co 50 ms. Patrząc na wyświetlacze, opóźnienie występuje po stronie kostki-odbiorcy: dane na jej wyswietlaczu aktualizują się praktycznie natychmiast, natomiast silniki działają już nieco wolniej.
Co do narzutów, nie mam pojęcia ale żeby ograniczyć ilość danych do minimum, wysyłam je jako pojedyńcze bajty. Stąd np. wysyłanie pozycji kierownicy nie jako wartości dodatniej i ujemnej, tylko w zakresie 0-255. Tak więc każda wiadomość to w sumie 4 bajty danych, plus wiadomość zwrotna ze stanem baterii opisanym jednym bajtem.

Jetboy

#4 Post autor: Jetboy »

Ponieważ opóźnienie jest najbardziej odczuwalne na skrętach, spróbuj zmienić nieco szyk programu i skręty obsługuj na początku, potem resztę, a wyświetlanie danych jako najmniej istotne zamiast na początku na końcu. To raczej nie będzie duża różnica, ale warto sprawdzić.

Wydaje mi się że nie trzeba odczytywać stanu baterii co 50 milisekund, odczyt raz na sekundę, albo nawet co parę sekund powinien wystarczyć, a obciążenie dla kostki mniejsze.

Zdaje się że cCmdMessageRead() czeka na pakiet danych, więc opóźnienie w pętli odbiorczej nie jest potrzebne a raczej szkodliwe.

Ale się nakręciłem na NXT, a kupić będę mógł dopiero w czerwcu :/ Jest do tego RobotC dostępna jakaś dobra dokumentacja?

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

 

#5 Post autor: Sariel »

3.05 ma szczerze mówiąc dokumentację do d...

Pauelor

#6 Post autor: Pauelor »

Podoba mi się to, że mówicie po Polsku, a ja was w ogóle nie rozumiem :) Ale i tak uznanie za podzielenie się pomysłem!

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

 

#7 Post autor: Emilus »

To opóźnienie w przesyłaniu informacji między 2 kostkami w tak małym programiku dyskwalifikuje zupełnie tą nową wersję robotaC nie tylko w CC ale nawet w TrTr. Naprawdę, poślizg z reakcją jest porażający :( Elektrozawory w demagu nie miały by szans działać z takimi lagami.

Panowie z robotC okazali się zwykłymi ch....i (ze względu na podejście do klienta - moja i nie tylko moja licencja niedawno bezprawnie wygasła) więc nie pozostaje nic innego jak tylko powrócić do wersji albo 2 albo jak się nie uda, to nawet wcześniej...
Ostatnio zmieniony 2012-03-04, 17:25 przez Emilus, łącznie zmieniany 2 razy.

ODPOWIEDZ