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.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 |
#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.
