Forum LUGPOL Strona Główna Forum LUGPOL
www.lugpol.pl

FAQFAQ  SzukajSzukaj  UżytkownicyUżytkownicy  GrupyGrupy
RejestracjaRejestracja  ZalogujZaloguj

Poprzedni temat «» Następny temat
[MOC] Pilot kostka-kostka
Autor Wiadomość
Sariel 
VIP
p.o. majstra


Wiek: 36
Dołączył: 28 Mar 2007
Wpisy: 5055
Skąd: Warszawa
Wysłany: 2012-03-03, 18:45   [MOC] Pilot kostka-kostka



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:


Film:


Instrukcja:


Program nadawczy:
Kod:
#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 (nNxtButtonPressed == 1 && soundLevel < 3) soundLevel = soundLevel + 1;
    else if (nNxtButtonPressed == 2 && soundLevel > 0) soundLevel = soundLevel - 1;
  wait1Msec(350);
  }
}
 
task main()
{
  StartTask(BatLev);
  StartTask(Buttons);
  bFloatDuringInactiveMotorPWM = true;
  btConnect(1, "NXT2"); // connection: port, brick
  while(true)
  {
    ubyte valueToSend[5];
    valueToSend[0] = nMotorEncoder[motorA];
    valueToSend[1] = nMotorEncoder[motorB];
    valueToSend[2] = SensorValue(touch1);
    valueToSend[3] = SensorValue(touch2);
    valueToSend[4] = soundLevel;
    if(sending == true){cCmdMessageWriteToBluetooth(valueToSend, 5, 1);}
    nxtDisplayTextLine(3, "Drive: %d", valueToSend[0]);
    nxtDisplayTextLine(4, "Steering: %d", valueToSend[1]);
    nxtDisplayTextLine(5, "Touch BLUE: %d", valueToSend[2]);
    nxtDisplayTextLine(6, "Touch RED: %d", valueToSend[3]);
    nxtDisplayTextLine(7, "Volume: %d", valueToSend[4]);
    wait1Msec(50);
  }
}


Program odbiorczy:
Kod:
// config start
//bMotorReflected[motorA] = true;
//bMotorReflected[motorB] = true;
//bMotorReflected[motorC] = true;
bool buzzOnRev = true; // buzzing while on reverse
int steerAcc = 10; // steering accuracy margin (degrees)
// config end
 
task BatLev()
{
  while(true)
  {
    nxtDisplayCenteredTextLine(0, "BAT: L %3.1fV", nImmediateBatteryLevel / (float) 1000);
    ubyte valueToSend[1];
    valueToSend[0] = nImmediateBatteryLevel / (float) 100;
    cCmdMessageWriteToBluetooth(valueToSend, 1, 1);
    wait1Msec(50);
  }
  return;
}
 
task main()
{
  StartTask(BatLev);
  nVolume = 1; // starting volume
  ubyte valueReceived[5];
  valueReceived[0] = 0;
  valueReceived[1] = 0;
  valueReceived[2] = 0;
  valueReceived[3] = 0;
  valueReceived[4] = 1;
 
  while(true)
  {
    cCmdMessageRead(valueReceived, 5, 1);
    nxtDisplayTextLine(2, "Drive: %d", valueReceived[0]);
    nxtDisplayTextLine(3, "Steering: %d", valueReceived[1]);
    nxtDisplayTextLine(4, "Touch BLUE: %d", valueReceived[2]);
    nxtDisplayTextLine(5, "Touch RED: %d", valueReceived[3]);
    nxtDisplayTextLine(6, "Volume: %d", valueReceived[4]);
    nVolume = valueReceived[4];
 
    // motorA (drive)
    motor[motorA] = 0;
    int afactor = valueReceived[0] / 5;
    if (afactor > 1 && afactor < 15)
    {
      afactor = afactor * 10;
      if (afactor > 100) afactor = 100;
      motor[motorA] = afactor;
      if (buzzOnRev == true) ClearSounds();
    }
    else if (afactor > 1 && afactor > 25)
    {
      afactor = (51 - afactor) * -10;
      if (afactor < -100) afactor = -100;
      motor[motorA] = afactor;
      if (buzzOnRev == true) PlaySound(soundException);
    }
 
    // motorB (steering)
    motor[motorB] = 0;
    int bfactor = valueReceived[1];
    if (bfactor > 1 && bfactor < 75) bfactor = bfactor;
    else if (bfactor > 1 && bfactor > 75) bfactor = (256 - bfactor) * -1;
    nxtDisplayTextLine(7,"Mtr B: %d/%d",nMotorEncoder[motorB], bfactor);
 
    if (nMotorEncoder[motorB] < (bfactor + steerAcc)) while(nMotorEncoder[motorB] < bfactor) motor[motorB] = 10;
    else if (nMotorEncoder[motorB] > (bfactor - steerAcc)) while(nMotorEncoder[motorB] > bfactor) motor[motorB] = -10;
    else motor[motorB] = 0;
 
    // motorC
    motor[motorC] = 0;
    if (valueReceived[2] == 1) motor[motorC] = 100;
    else if (valueReceived[3] == 1)motor[motorC] = -100;
 
    wait1Msec(50);
  }
}
_________________
Kalkulator przełożeń | Generator miniaturek z BSa/Majhosta
 
 
 
Jetboy 

Wiek: 45
Dołączył: 29 Sie 2016
Wpisy: 1524
Skąd: Warszawa koło Otwocka
Wysłany: 2012-03-03, 19:54   

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.
 
 
 
Sariel 
VIP
p.o. majstra


Wiek: 36
Dołączył: 28 Mar 2007
Wpisy: 5055
Skąd: Warszawa
Wysłany: 2012-03-03, 20:34   

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.
_________________
Kalkulator przełożeń | Generator miniaturek z BSa/Majhosta
 
 
 
Jetboy 

Wiek: 45
Dołączył: 29 Sie 2016
Wpisy: 1524
Skąd: Warszawa koło Otwocka
Wysłany: 2012-03-03, 21:40   

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?
 
 
 
Sariel 
VIP
p.o. majstra


Wiek: 36
Dołączył: 28 Mar 2007
Wpisy: 5055
Skąd: Warszawa
Wysłany: 2012-03-03, 21:52   

3.05 ma szczerze mówiąc dokumentację do d...
_________________
Kalkulator przełożeń | Generator miniaturek z BSa/Majhosta
 
 
 
Pauelor
[Usunięty]

Wysłany: 2012-03-04, 03:33   

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!
 
 
 
Emilus 
Adminus Emeritus


Wiek: 38
Dołączył: 26 Sie 2007
Wpisy: 1460
Skąd: Polska
Wysłany: 2012-03-04, 17:23   

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...
_________________
legotrucktrial.boo.pl | Regulamin LTTC 2012 | Terminarz LTTC i CC 2012
Ostatnio zmieniony przez Emilus 2012-03-04, 17:25, w całości zmieniany 2 razy  
 
 
 
Wyświetl wpisy z ostatnich:   
Odpowiedz do tematu
Nie możesz pisać nowych tematów
Nie możesz odpowiadać w tematach
Nie możesz zmieniać swoich wpisów
Nie możesz usuwać swoich wpisów
Nie możesz głosować w ankietach
Nie możesz załączać plików na tym forum
Możesz ściągać załączniki na tym forum
Dodaj temat do Ulubionych
Wersja do druku

Skocz do:  

phpBB by przemo  
Strona wygenerowana w 0,137 sekundy. Zapytań do SQL: 9