Freestyle Robot unikający kolizji

[mati 6]

Wstęp

Witam serdecznie. Prezentuję dziś mobilnego robota, którego zadaniem jest wykrywanie przeszkód i unikanie kolizji. Jest to pierwszy projekt po długiej przerwie, starałem się możliwie rzetelnie podejść do sprawy.

Opis działania

Robot posiada tylko jeden sensor ( cyfrowy czujnik Sharp o zasięgu do 10cm) umieszczony na orczyku serwa, które stale obraca go w zakresie mniej więcej od 0 do 100 stopni. Dodatkowo nad czujnikiem znajduje się łuk diod LED, które sygnalizują bieżące wychylenie czujnika i przeszkody (widać na filmie).

Po wykryciu przeszkody robot zmienia kierunek jazdy ( kierunek zależny od miejsca wykrycia przeszkody).

Powstawanie w 3 krokach

Mechanika

Sam przyznaję, że to wygląda dziwnie :/

Napęd stanowią dwa przerobione ( pozbawione elektroniki i mechanicznych blokad) serwomechanizmy typu standard. Obracaniem czujnika zajmuje się micro serwo. Konstrukcja oparta jest na laminacie. Montaż głównie za pomocą śrubek i dystansowników. Trzeci punkt podporu stanowi małe kółko obrotowe ( dostępne w każdym sklepie budowlanym). Koła są kupne, z mocowaniem pasującym do wału standardowego serwomechanizmu. W trakcie budowy problemem okazał się za mocno do przodu wysunięty środek ciężkości. Zniwelowałem to dokładając z tyłu dużą śrubę z nakrętkami służącą za obciążnik. Akumulator przymocowany jest za pomocą rzepu samoprzylepnego.

Elektronika

Serce robota to atmega88p. Zasilanie: pakiet lipo s2 7.4V 400mAh. Dwa stabilizatory ( jeden dla elektroniki, drugi dla serwa*). Mostek l293D i trochę drobnicy ( wszystko na schemacie i zdjęciach).

*Nie jest to rozwiązanie idealne, jednakże chyba najprostsze, by w łatwy sposób dostarczyć 5V dla jednego serwa.

Program

Nic specjalnego, jednakże jest to mój pierwszy projekt napisany w C ( przed przerwą zaczynałem w Bascomie). Timer0 generuje sygnał dla serwa, Timer1 zajmuje się PWMem dla silników. Serwo przemieszcza się wyłącznie między 11 na sztywno zaprogramowanymi położeniami ( pokrywającymi się z położeniem diod LED ). Brak przerwań, stan czujnika odczytywany jest tylko w postojach pomiędzy jednym, a drugim LEDem ( ok. 11 stopni), jednak kąty widzenia czujnika chyba niwelują martwe pole.

Zmiana kierunku jazdy jest wprost proporcjonalna od odległości przeszkody od środka robota ( mówiąc inaczej, gdy przeszkoda jest z boku robot skręca lżej, im bliżej środka tym mocniej robot „odbija”)

Najprawdopodobniej powstanie jakiś fajniejszy algorytm

Wady

Na zdjęciu widoczne dociążenie robota

Przede wszystkim kompletnie zapomniałem o środku ciężkości, wydawało mi się, że sam ciężar serw wystarczy. Stąd też potrzebne było dodatkowe obciążenie.

Cały system wykrywania przeszkody jest swego rodzaju kompromisem. Z jednej strony zasięg czujnika jest mały, z drugiej strony zbyt szybki obrót serwa również wpływa negatywnie na przeszukiwanie terenu. Robot jest celowo spowolniony, by miał czas na przebadanie otoczenia.

Problemy są z wykrywaniem za wysokich, zbyt niskich przeszkód, oraz przeszkód w kolorze czarnym. (Czego się oczywiście spodziewałem).

Galeria

Schematy

Płytki

Program v1.01

#define F_CPU 1000000L
#include <avr/io.h>
#include <util/delay.h>                

#define LED1 0xFE
#define LED2 0xFD
#define LED3 0xFB
#define LED4 0xF7
#define LED5 0xEF
#define LED6 0xDF
#define LED7 0xFE
#define LED8 0xEF
#define LED9 0xF7
#define LED10 0xFB
#define LED11 0xFD
#define silnik_lewyA 0b10000000
#define silnik_lewyB 0b00100000
#define silnik_prawyA 0b01000000
#define silnik_prawyB 0b10000000
#define sensor 0b0000001

int i;

void czekaj_ms(uint16_t ms)
{
for(uint16_t g=0; g<ms; g++) _delay_ms(1);
}


void przod_przod(int x, int y, int z)
{	
PORTB|=silnik_lewyA | silnik_prawyA;
PORTD&= (~silnik_lewyB) & (~silnik_prawyB);
OCR1A=x;
OCR1B=y;
czekaj_ms(z);

}

void tyl_tyl(int x, int y, int z)
{
PORTB&=(~silnik_lewyA) & (~silnik_prawyA);
PORTD|= silnik_lewyB | silnik_prawyB;
OCR1A=x;
OCR1B=y;
czekaj_ms(z);
}

void przod_tyl(int x, int y, int z)   // Lewy silnik do przodu, prawy do tylu
{
// silnik lewy do przodu
OCR1B=x;
PORTB|=silnik_lewyA;
PORTD&= (~silnik_lewyB);
//silnik prawy do tylu
OCR1A=y;
PORTB&=(~silnik_prawyA);
PORTD|=silnik_prawyB;

czekaj_ms(z);
}

void tyl_przod(int x, int y, int z)   // Lewy silnik do tylu, prawy do przodu
{
// silnik lewy do tylu
OCR1B=x;
PORTB&= (~silnik_lewyA);
PORTD|= silnik_lewyB;
//silnik prawy do przodu
OCR1A=y;
PORTB|=silnik_prawyA;
PORTD&=(~silnik_prawyB);

czekaj_ms(z);
}







void inicjalizacja()
{
//PWM DLA SERWA ( timer0) STEROWANIE PRZEZ PODANIE NA OCR0A wartosci od 10 do 35
TCCR0A = ( 1 << COM0A1 ) |  (1<<WGM01) | (1<<WGM00);
TCCR0B = (1<<CS00) | (1<<CS01);
DDRD|=0x40;
PORTD=0x40;
//PWM DLA SILNIKOW (timer1)
TCCR1A= (1<< COM1A1)|(1<<COM1B1)|(1<<WGM11);  //MODE 14 FAST PWM
TCCR1B= (1<<WGM13)|(1<<WGM12)|(1<<CS10);  // PRESCALER=1, frec.PWM = 1kHz
ICR1=999;

}





void dioda(int x)
{

//Zapalanie kolejnych diod
switch(x)
	{

	case 11:
		PORTC&=LED1;

		break;
	case 10:
		PORTC&=LED2;

		break;
	case 9:
		PORTC&=LED3;

		break;
	case 8:
		PORTC&=LED4;

		break;
	case 7:
		PORTC&=LED5;

		break;
	case 6:
		PORTC&=LED6;

		break;
	case 5:
		PORTD&=LED7;

		break;
	case 4:
		PORTD&=LED8;

		break;
	case 3:
		PORTD&=LED9;

		break;
	case 2:
		PORTD&=LED10;

		break;
	case 1:
		PORTD&=LED11;

		break;
	}

}
void dioda_off()
{

PORTC|=0b00111111;
PORTD|=0b00011111;

}



void przeszkoda()
{


	//Hamowanie
	tyl_tyl(800,800,100);

	OCR1A=0;
	OCR1B=0;

	for (uint8_t g=0;g<4;g++)  // ZAMRUGANIE DIODAMI
			{

			dioda(i/2 -4);
			dioda(i/2 -5); 
			dioda(i/2 -6);
			czekaj_ms(300);
			dioda_off();
			czekaj_ms(200);
			}
tyl_tyl(500,500,i*30);
if (i>=22) przod_tyl(800,800,((-30)*i+1060));
else tyl_przod(800,800,(30*i-260));
przod_przod(500,500,1);	





}




void szukanie()
{
	int czas=20;
	while(1)

	   {


	   for (i=12; i<33;i+=2)
			{

			OCR0A=i;
			czekaj_ms(czas);
			dioda_off();
			czekaj_ms(czas);
			dioda(i/2 -5);
			if (!(PINB & sensor)) przeszkoda();

			}

		for (i=32; i>11; i-=2)
		{

			OCR0A=i;
			czekaj_ms(czas);
			dioda_off();
			czekaj_ms(czas);
			 dioda(i/2 -5); 
			if (!(PINB & sensor)) przeszkoda();



		}


	  }

}





void main(void)
{
inicjalizacja();

DDRD=0b11111111;
PORTD=0b00000000;
DDRC=0b00111111;
DDRB=0b11111110;
PORTB=0b11100000;

przod_przod(500,500,1);
szukanie();

}

Film

Najprawdopodobniej powstanie jakiś fajniejszy algorytm

[Treker (Damian Szymański)]

Pomysłowa konstrukcja Pytanie tylko, czy machanie serwem nie odbywa się zbyt szybko? SHARPy znane są z dość dużych stałych czasowych. Dobierałeś jakoś doświadczalnie prędkość skanowania?

[mati 6]

Zgadza się. Z tego powodu ograniczona jest prędkość całego robota. Machanie dobierałem doświadczalnie ( ciekawa zabawa, by dodatkowo połączyć to z odpowiednim zapalaniem i gaszeniem diod by wprowadzić efekt synchronizacji). W chwili obecnej wygląda to tak:

->komenda, by serwo przesunęło się o jedną jednostkę ( ok. 10-11 stopni)

-> odczekaj 20ms

-> zgaś diodę, pod którą wcześniej był czujnik

-> odczekaj kolejne 15ms

-> zapal kolejną diodę

-> sprawdź stan czujnika na nowej pozycji

Nie omieszkam pobawić się jeszcze tymi parametrami, by uzyskać jeszcze lepszą synchronizację.

[piotreks-89]

Ciekawa konstrukcja, gratuluję!

W kwestii programu, nie przekazuj do delay'ów zmiennych, bo będą one rzutowane na typ zmiennoprzecinkowy i program urośnie ci w kobyłę. Sam popełniłem już ten błąd i program, który powinien zajmować 300B, zajmował 4kB

Druga sprawa - nie twórz funkcji z możliwością zwrotu danych wyjściowych, skoro tego nie wykorzystujesz. Wykorzystaj wówczas typ "void". We wszystkich funkcjach masz argumenty typu "int", a wcale nie wykorzystujesz całej długości tych zmiennych. Np. timer od PWM'a masz 16-bitowy, to do OCRx przekazuj argumenty typu "uint16_t" (przecież i tak nie używasz liczb ujemnych ).

Dobrym nawykiem jest używanie dyrektyw "#define" zamiast wpisywania wszędzie wartości binarnych/heksadecymalnych. Za jakiś czas po prostu będzie ci się trudno odnaleźć w kodzie, który sam napisałeś. Twój kod można bardzo łatwo zoptymalizować, więc może warto się tym zająć

[sosnus]

Gratuluję konstrukcji, a przede wszystkim tego rysunku ze Sketchupa

No właśnie, piotreks-89, mnie ubiegł, najpierw przeanalizowałem kod, później przeczytałem komentarze .

To:

_delay_ms(czas); 
 _delay_ms(z);

Dobrze by było zamienić na:

void czekaj_ms(uint8_t ms)
{
for (uint8_t i=0; i<ms ; i++) _delay_ms(1);
}

i w programie zamiast:

_delay_ms(czas); 
_delay_ms(z);

używać:

czekaj_ms(czas);
czekaj_ms(z);

Za pomocą zmiennych int LEDx zapychasz niepotrzebnie ram, a skoro i tak nie zmieniasz wartości tych zmiennych możesz to zrobić inaczej, omijając zapychanie ramu:

int LED1=0xFE; 
#define LED1 0xFE

Jestem ciekaw ile to by dało oszczędności RAMu i flasha.

Ale oczywiście ilu programistów, tyle rozwiązań

[marek1707]

Już nie mówiąc o tym, że cały wielki kawał kodu - poprzez użycie tablicy - zamienia się w kilka prostych operacji:

uint16_t led_bits[] =
   {
       0x0000, 0x0200, 0x0400, 0x0800, 0x1000, 0x0100, 0x0020, 0x0010, 0x0008, 0x0004, 0x0002, 0x0001
   };

void zapal_diode(uint8_t numer_diody)
{
   uint16_t
       mask = (numer_diody < sizeof(led_bits)) ? led_bits[numer_diody] : 0x0000;

   PORTD &= ~(mask >> 8);
   PORTC &= ~mask;
}

Pamiętaj, że programy piszesz dla siebie, nie dla procesora. Inaczej wstukiwałbyś binarne kody instrukcji. Jeżeli diodki są na pewnych bitach portów, w tablicy używaj jedynek na tej pozycji bo to jest dużo bardziej czytelne. Niech procesor sobie to odwróci, szybki jest. Po co odwalasz robotę za niego?

Moja tablica ma słowa 16-bitowe i pobieram maskę od razu dla dwóch portów. Przekazanie argumentu numer_diody=0 powoduje pobranie z indeksu [0] i brak jakiejkolwiek zmiany. Przy zbyt dużym numerku diody ograniczeniem jest porównanie z wielkością tablicy. Wtedy także mask dostaje wartość 0x0000.

Jeżeli chcesz oszczędzić RAM i przechowywać tablicę w pamięci FLASH, przy deklaracji musisz użyć atrybutu PROGMEM a odczyt wykonywać przy pomocy funkcji pgm_read_word().

[Elvis]

A ja bym się programu nie czepiał. Najważniejsze że działa, a z czasem można go udoskonalać na wiele sposobów.

Natomiast oszczędzanie pamięci, czy sprytne tablice to nie najlepszy pomysł dla kogoś kto zaczyna. Po pierwsze, to co zaoszczędzimy i tak nie będzie używane... Po drugie łatwo o błąd.

Kod który podał marek1707, zawiera poważny błąd.

sizeof() w języku C zwraca liczbę bajtów zajmowaną przez dany typ lub zmienną. Więc wyrażenie

sizeof(led_bits)

zwróci ile bajtów zajmuje ta tablica. Nie jest to liczba elementów, ale w przypadku typu uint16_t dokładnie dwa razy więcej. Mamy więc przykład programu, w którym nawet ktoś z doświadczeniem może łatwo zrobić błąd.

Przy okazji, kod powinien wyglądać:

    uint16_t 
       mask = (numer_diody < (sizeof(led_bits)/sizeof(led_bits[0])) ? led_bits[numer_diody] : 0x0000; 

[mati 6]

Dzięki wszystkim. Nie spodziewałem się aż takiego odzewu.

Niezaprzeczalne, że kod jest do poprawy. Jutro na pewno do tego przysiądę.

Niemniej jednak pierwszorzędną sprawą było dla mnie dostarczenie w pierwszej kolejności działającego kodu, który ( w chwili jego pisania ) rozumiałem i mogłem łatwo modyfikować. Przypominam, to moje pierwsze połączenie Uc i C.

Za niektóre " błędy " jest mi wstyd już teraz , z niektórych będę się śmiał jutro, a część pewnie będę jeszcze przez długi czas popełniał.

Na tym etapie mikrokontroler jeszcze wiele wybacza, ale macie rację, że to nie powód by robić to byle jak.

Tak czy inaczej nie żałuję, że wstawiłem ten kod. Byłem gotowy na konstruktywną krytykę i jeszcze raz wielkie dzięki za nią.

Jeśli jacyś inni początkujący chcieliby coś zaczerpnąć z takiej konstrukcji to dobrze, by zastali jak najładniejszy kod.

Pozdrawiam

edit: Do czasu poprawy kodu dodałem w pierwszym poście ostrzeżenie o nie-perfekcyjności kodu

[marek1707]

Absolutnie nie traktuj naszych uwag jako czepiania się. Przecież po to jest Forum, by dyskutować. Program, który pierwszego dnia wydaje się fajny, po miesiącu okazuje się żenująco prymitywny nawet dla autora - każdy przez to przechodzi. Dobrze jest wiedzieć, że można go poprawić w wielu miejscach a niektóre konstrukcje nie przyjdą do głowy same - trzeba je zobaczyć. I po to są nasze propozycje. Ja, dzięki czytaniu postów na Forum chyba nauczyłem się BASCOMa "przez dyfuzję", choć (na szczęście) nigdy nie musiałem go używać w praktyce.

Elvis - masz mnie Na usprawiedliwienie (i auto-pocieszenie) mogę powiedzieć tylko tyle, że nie popełnia błędów ten kto nic nie robi. Pisałem z głowy bardziej zwracając uwagę na bity diodek. Swoją drogą zawsze mnie irytowała ta semantyka sizeof'a choć rozumiem, że autorzy języka musieli wybrać jakiś uniwersalną jednostkę długości niezależną od struktury pamięci komputera ani od wielkości typów podstawowych.

[mati 6]

Jestem ciekaw ile to by dało oszczędności RAMu i flasha.

Proszę bardzo, już informuję

( Zamiast "(uint8_t ms) " użyłem "(uint16_t ms)" ponieważ niektóre delaye są dłuższe.

Stara wersja (info przy kompilacji):

Program:    2362 bytes (28.8% Full)
(.text + .data + .bootloader)

Data:         24 bytes (2.3% Full)
(.data + .bss + .noinit)

Nowa wersja:

Program:    1170 bytes (14.3% Full)
(.text + .data + .bootloader)

Data:          2 bytes (0.2% Full)
(.data + .bss + .noinit)

A zmian przecież tak dużo nie ma

Wstrzymałem się jeszcze z przebudową funkcji do zapalania diod ( ale faktycznie pomysł Marka1707 jest sprytny )

Absolutnie nie traktuj naszych uwag jako czepiania się

Nie jesteśmy w piaskownicy Jestem szalenie wdzięczny za wasze uwagi.