Tworzenie oprogramowania dla robota

Najbardziej kłopotliwa część za nami. Do tej pory zbudowaliśmy urządzenie, które nie wykonuje żadnych bliżej określonych czynności. Musimy teraz „powiedzieć” robotowi co właściwie ma robić – innymi słowy: zaprogramować go.

Zakładam, że środowisko Atmel Studio 6 jest już zainstalowane, podobnie biblioteki Pololu. Skoro wszystko jest gotowe, to wczytujemy plik „KAMAMI-orangutan-robot.atsln” z rozpakowanego załącznika spod tego artykułu.

Poniżej przedstawiam kod, który powinien być widoczny po wybraniu pliku main.c z okienka Solution Explorer w Atmel Studio 6.

#include 

signed int roznica;
unsigned int obrot;
unsigned int prawysharp;
unsigned int lewysharp;
unsigned int prawysharp1;
unsigned int lewysharp1;
unsigned int prawysharp2;
unsigned int lewysharp2;
signed int silniklewy = 1;
signed int silnikprawy = 1;
signed int kierunek = 0;
float predkoscserwa = 1.0;

const int UWAGA=300;                           //wartosc, dla ktorej robot ma sie zatrzymac (zeby nie wpasc na przeszkode)



int main()
{
const unsigned char servoPinsB[] = {IO_D1};    //konfiguracja serwa
  servos_start_extended((unsigned char[]){}, 0, servoPinsB, sizeof(servoPinsB));
  set_servo_speed_b(0, 0);                     //zerowa wartosc oznacza maksymalna predkosc obrotu serwa
  set_servo_target_b(0, 1400);                 //obrot serwa do pozycji 1400 (neutralnej)
  
  clear();  
  set_analog_mode(MODE_10_BIT);                //konfiguracja przetwornika ADC

  while(1)     
  {
    
    lewysharp = analog_read_average(2, 5);     //wartosc zmierzona przez lewy czujnik
    prawysharp = analog_read_average(3, 5);    //wartosc zmierzona przez prawy czujnik
    
    clear;                                     //czyszczenie ekranu LCD
    lcd_goto_xy(0, 0);                         //ustawienie kursora w zadanych wspolrzednych
    print("R:");                               //drukowanie litery "R"
    print_long(roznica);                       //drukowanie zmiennej "roznica"
    print("    ");
    lcd_goto_xy(0, 1); 
    print("K:");  
    print_long(kierunek);
    print("    ");
    
    roznica = (prawysharp*2) - (lewysharp*2);
    obrot = 1400 + roznica;
    
    if (obrot>=1800) {                         //przekroczenie wybranego zakresu obrotu serwa
    obrot = 1800;
    } else if (obrot<=1000) { obrot = 1000; } predkoscserwa = abs(roznica)/3 +20; //im wieksza roznica miedzy wartosciami zmierzonymi przez lewy i prawy czujnik, tym szybciej serwo ma sie obracac if (predkoscserwa>=148.5) {                //przekroczenie zakresu predkosci serwa
    predkoscserwa=148.5;
    } else if (predkoscserwa<=30.5) {
    predkoscserwa=30.5;
    }  
    
    set_servo_speed_b(0, (int)(predkoscserwa+0.5));
    set_servo_target_b(0,obrot);
    
    if (lewysharp < UWAGA || prawysharp < UWAGA){ //brak przeszkody kierunek = (obrot - 1400); kierunek = (int)(kierunek/4); set_motors((kierunek+100), (-kierunek+100)); //zadanie predkosci dla lewego i prawego silnika delay_ms(150); //czekanie 150ms } else { //wykryta przeszkoda w niewielkiej odleglosci //cofniecie sie set_motors(-100, -100); delay_ms(300); set_motors(1,1); //rozgladanie sie set_servo_speed_b(0,0); set_servo_target_b(0,1000); //spojrzenie w lewo delay_ms(600); lewysharp1 = analog_read_average(2, 5); prawysharp1 = analog_read_average(3, 5); set_servo_speed_b(0,0); set_servo_target_b(0,1800); //spojrzenie w prawo delay_ms(800); lewysharp2 = analog_read_average(2, 5); prawysharp2 = analog_read_average(3, 5); set_servo_speed_b(0,0); //spojrzenie do przodu set_servo_target_b(0,1400); delay_ms(300); //okreslanie kierunku obrotu if (lewysharp1 > UWAGA) {                      //bliska przeszkoda po lewej stronie...
      
          if (prawysharp2 > UWAGA) {                   //bliska przeszkoda po prawej stronie...
       
            set_servo_speed_b(0, 0);
            set_servo_target_b(0,1000);                //spojrzenie w lewo
            set_motors(100, -100);                     //obrot w lewo w miejscu (zawracanie)
            delay_ms(350);
            set_servo_speed_b(0, 300);                 //wolne prostowanie "glowy"
            set_servo_target_b(0,1400);
            delay_ms(350);
            set_motors(1,1);
       
          } else {                                     //brak lub daleka przeszkoda po prawej stronie
       
            set_servo_speed_b(0, 0);
            set_servo_target_b(0,1600);                //spojrzenie w prawo
            set_motors(-100, 100);                     //obrot w prawo
            delay_ms(150);
            servo_speed_b(0, 300);                     //wolne prostowanie "glowy"
            set_servo_target_b(0,1400);
            delay_ms(150);
          }
      
          } else {                                    //brak lub daleka przeszkoda po lewej stronie
      
          if (prawysharp2 > UWAGA) {                  //bliska przeszkoda po prawej stronie
       
            set_servo_speed_b(0, 0);
            set_servo_target_b(0,1200);               //spojrzenie w lewo
            set_motors(100, -100);                    //obrot w lewo
            delay_ms(150);
            set_servo_speed_b(0, 300);                //wolne prostowanie "glowy"
            set_servo_target_b(0,1400);
            delay_ms(150);
       
          } else {                                    //brak lub daleka przeszkoda po prawej stronie
       
                                                      //sprawdzenie po której stronie jest więcej miejsca
       
          if ((lewysharp1-prawysharp2)>0) {           //po lewej stronie wiecej miejsca
        
            set_servo_speed_b(0, 0);
            set_servo_target_b(0,1200);               //spojrzenie w lewo
            set_motors(100, -100);                    //obrot w lewo
            delay_ms(150);
            set_servo_speed_b(0, 300);                //wolne prostowanie "glowy"
            set_servo_target_b(0,1400);
            delay_ms(150);
        
          } else {                                   //po prawej stronie wiecej miejsca
        
            set_servo_speed_b(0, 0);
            set_servo_target_b(0,1600);              //spojrzenie w prawo
            set_motors(-100, 100);                   //obrot w prawo
            delay_ms(150);
            set_servo_speed_b(0, 300);               //wolne prostowanie "glowy"
            set_servo_target_b(0,1400);
            delay_ms(150);
         
          }
        }
      }

    }
  }
}

Większość wykorzystanych funkcji została krótko opisana w komentarzach, ale warto zwrócić szczególną uwagę na najważniejsze z nich. Nie są one typowe dla programów pisanych w języku C dla mikrokontrolerów AVR. Pochodzą z bibliotek Pololu i są ściśle powiązane z platformą sprzętową, jaką jest sterownik Orangutan SV-328. Dostęp do wszystkich peryferiów został ułatwiony w taki sposób, aby uprościć początkującym naukę podstaw programowania. Bardzo podobnie wygląda programowanie sterowników bazujących na platformie Arduino. Oczywiście, nic nie stoi też na przeszkodzie, by zaprogramować robota w klasycznym C lub dowolnym innym języku programowania mikrokontrolerów (Basic, assembler itp.). Poniżej najważniejsze funkcje i ich przeznaczenie:

• set_servo_speed_b(X, Y) – określa prędkość obrotu serwa – Y to prędkość, a X to numer serwa, którego ta wartość ma dotyczyć; Y=0 oznacza wyłączenie kontroli prędkości – wszystkie obroty wykonywane są najszybciej, jak to możliwe,
• set_servo_target_b(X, Y) – zadanie obrotu serwa – X to numer serwa, a Y to pozycja
• zmienna = analog_read_average(X, Y) – pomiar wartości zmierzonych przez przetwornik ADC – X to numer kanału ADC, a Y oznacza ilość pomiarów, spośród których wyznaczana jest wartość średnia, przypisywana następnie do zmiennej,
• clear() – czyszczenie ekranu LCD – znikają wszystkie znaki wyświetlane wcześniej na LCD
• lcd_goto_xy(X, Y) – umieszczenie kursora na ekranie LCD w określonych współrzędnych – X oznacza kolumnę, a Y wiersz,
• print(tekst) – „drukowanie” na ekranie LCD wartości zmiennej tekst lub dowolnego ciągu znaków, napisanych w cudzysłowiu,
• set_motors(X, Y) – określa prędkość obrotową dwóch kanałów silników (X dla jednego, Y dla drugiego), wartości ujemne oznaczają obrót w przeciwnym kierunku,
• delay_ms(X) – zawieszenie działania programu na X ms,
• red_led(X) – gaszenie lub zapalanie czerwonej diody LED – X może przyjmować wartość 0 (gaszenie) lub 1 (zapalanie),
• green_led(X) – gaszenie lub zapalanie zielonej diody LED – X może przyjmować wartość 0 (gaszenie) lub 1 (zapalanie),
• wait_for_button_press(TOP_BUTTON | BOTTOM_BUTTON) – zawieszanie działania programu, dopóki nie zostanie wciśnięty górny lub dolny przycisk,
• wait_for_button_release(TOP_BUTTON | BOTTOM_BUTTON) – zawieszanie działania programu, dopóki nie zostanie puszczony górny lub dolny przycisk,
• play_note(X, Y, Z) – sterownik gra określony dźwięk – X to wysokość dźwięku podawana w specjalnej notacji, np. E(5), C(4), F_SHARP(2) itp., Y oznacza czas trwania dźwięku, a Z głośność (maksymalna głośność to 15); funkcja korzysta z tego samego timera co serwo, więc równoczesne korzystanie z obu peryferiów jest kłopotliwe,

Więcej funkcji wraz z praktycznymi przykładami można znaleźć w katalogu bibliotek Pololu: [wybrana podczas instalacji ścieżka dostępu]/libpololu-avr/examples/atmega328p.