Line Follower by Regy

[regrom]

Witam,

Przedstawiam własną konstrukcję robota wykonanego przeze mnie. Umieszczam w dziale nie ukończonych projektów, gdyż jest w fazie testowej.

Na początek:

Line follower przegubowy. Oś skrętna z użyciem serwomechanizmu. Czujniki umieszczone po łuku, nie na ramieniu.

➡️ Silnik Graupner Race 280

➡️ ATmega 8 - 16 PU

➡️ 2 x LM324

➡️ 7 x CNY 70

➡️ Przekładnia pasowa

➡️ Serwo Tower TG9e

➡️ Kółka z zabawek, całość wykonana z PCB

➡️ Zasilenie Nosram 7,2 V 2200 mAh

Robot napędzany jest jednym silnikiem. Układ napędowy to przekładnia pasowa + kółka z zabawek. Zasada działania banalnie prosta, wykryty dany czujnik do dany kąt skrętu oraz wartość PWM którym reguluje prędkość.

Jak widać robot jest połączeniem klasycznego typu line followera gdzie czujniki przytwierdzone są do całej konstrukcji oraz przegubowca które napędzane są jednym silnikiem.

Filmik z pierwszego testu, stała wartość PWM'a ustawiona na 180, jest to minimalna prędkość. Spory zapas. Komentujcie, pytajcie ;]

[Treker (Damian Szymański)]

Ciekawa konstrukcja, jaką prędkość chcesz osiągnąć?

Wydaje mi się, że przy większej prędkości serwo może już nie wyrabiać.

[Bobby]

Oj, za małe, za wolne to serwo. Nie lepiej byłoby też czujniki ułożyć w linii prostej? Sterujesz pojedynczym tranzystorem? No i chyba jednak lepiej by było, gdybyś zamontował czujniki na ramieniu, miałbyś wirtualnie większą rozdzielczość.

[regrom]

➡️Treker

Hmm, prędkość testowa maksymalna jest grubo ponad 1,5 m/s, myślę że jest to realne do opanowania, ale trzeba by go odchudzić, po pierwsze jakiś lipol zamiast NiCd itp. no i przede wszystkich porządny algorytm.

➡️BoBBy

W brew pozorom serwo nie jest takie wolne, a mocy mu nie brakuje gdyż jest w osi skrętnej i jedyny moment bezwładności to silnik z którym daje sobie radę 😉 , ale mam świadomość że przy większych prędkościach może być za wolne, chociaż 60 stopni / 0,1 sec to i tak nieźle jak na takiego maluszka.

Myślałem też żeby zamiast serwa dać silnik krokowy. Na pewno były szybszy i mocniejszy ale i cięższy.

Aha, czujniki nie są na ramieniu właśnie dlatego, żeby przetestować nową konstrukcję 😉 Nie miał to być kolejny "wonsz". Umieszczenie po łuku czujników też nie jest przypadkowe, ułatwia sterowanie, tam gdzie linia(czujnik) taki kąt skręcenia.

Sterowanie IRF 9520 (MOSFET P), załączany BC337. Jeden tranzystor, bo robot ma jechać tylko do przodu 🙂 nie potrzebny żaden mostek.

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Bobby]

Umieszczenie po łuku czujników też nie jest przypadkowe, ułatwia sterowanie, tam gdzie linia(czujnik) taki kąt skręcenia.

Miłego wyszukiwania kątów prostych. Nie da rady na takim układzie czujników, niestety.

IRF 9520 (MOSFET P)

:o Dlaczego P? Miałeś po prostu, czy akurat u Ciebie ten P jest tańszy?

[regrom]

Umieszczenie po łuku czujników też nie jest przypadkowe, ułatwia sterowanie, tam gdzie linia(czujnik) taki kąt skręcenia.

Miłego wyszukiwania kątów prostych. Nie da rady na takim układzie czujników, niestety.

IRF 9520 (MOSFET P)

:o Dlaczego P? Miałeś po prostu, czy akurat u Ciebie ten P jest tańszy?

Zauważ że robot zaczyna już skręcać , gdy któryś czujnik obok środkowego napotka linie. Jak pisałem wyżej jest to konstrukcja testowa. Kąty proste jak na razie nie są mu straszne.

Dlaczego P, bo miałem to raz, dwa że BUZ10 się nie sprawdzał, dlatego zastosowałem sterowania od plusa.

[ Dodano: 19 Cze 10 09:55 ]

Zamieszczam testowy, jak widać bardzo prymitywny program. Obsługa serwa, pwm i czujników. Jest pamięć jak zgubi linię, poszukuje jej przez jakiś czas, jeśli nie znajdzie to się zatrzymuje, pamięć polega na tym że zapamiętuje, który skrajny czujnik był ostatni aktywny i skręca w tą stronę do póki nie napotka linii. Przed startem ustawiamy sobie wartość pwm. Do tego obsługa LCD, start na przycisk itp.

Nie które części kodu z innych projektów które robiłem, jedne znalezione w sieci jak obsługa serwa.

Jakieś pytania, sugestie, pisać śmiało, krytyka też mile widziana 😉

#define F_CPU 1000000UL  // taktowenie wewnetrznym MHz
#include <avr/io.h>
#include <util/delay.h>  
#include <hd44780.c>
#include <stdlib.h>
#include <inttypes.h>
#include <avr/sfr_defs.h>
#include <avr/interrupt.h>
#include <math.h> 

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

//0 WEJSCIE!!
//1 WYJSCIE!!
#define P3 bit_is_set(PIND,0)
#define P2 bit_is_set(PIND,1)
#define P1 bit_is_set(PIND,2)
#define SR bit_is_set(PIND,3)
#define L1 bit_is_set(PIND,4)
#define L2 bit_is_set(PIND,5)
#define L3 bit_is_set(PIND,6)

#define UP bit_is_clear(PINB,0)
#define DOWN bit_is_clear(PIND,7)
#define OK bit_is_clear(PINC,4)
#define START bit_is_set(PINC,4)

unsigned int kat = 1350;
unsigned char pwm = 0;
unsigned char pwmustaw = 0;

unsigned char menu = 0;
unsigned char i = 0;
unsigned char k = 0;
int licz=0;

char bufor[20];


void InicjalizacjaSERWO(void)
{

 ICR1  = 9999;
 OCR1A = kat;

TCCR1A|=(0<<COM1A0)|(1<<COM1A1)|(0<<COM1B0)|(0<<COM1B1)|

(0<<FOC1A)|(0<<FOC1B)|(1<<WGM11)|(0<<WGM10);

TCCR1B|=(0<<ICNC1)|(0<<ICES1)|(1<<WGM13)|(1<<WGM12)|

(0<<CS12)|(0<<CS11)|(1<<CS10);//preskaler 1
} 

void InicjalizacjaPWM(void)
{
OCR2 = pwm;
//TIMER2 pwm
TCCR2 =  _BV(COM21) | _BV(WGM21) | _BV(WGM20) |  _BV(CS20); //| _BV(COM20)
}

int main (void)

{
sbi(DDRB,2); // wyjscie
cbi(PORTB,2); //BUZZER OFF

cbi(DDRB,0); // wejscie UP
sbi(PORTB,0); //+ 5V

cbi(DDRD,7); // wejscie DOWN
sbi(PORTD,7); //+ 5V

cbi(DDRC,4); // wejscie OK
sbi(PORTC,4); //+ 5V

//wejscie czujniks
cbi(DDRD,0); // P3
cbi(DDRD,1); // P2
cbi(DDRD,2); // P1
cbi(DDRD,3); // SRODEK
cbi(DDRD,4); // L1
cbi(DDRD,5); // L2
cbi(DDRD,6); // L3

//do 5V
sbi(PORTD,0); // P3
sbi(PORTD,1); // P2
sbi(PORTD,2); // P1
sbi(PORTD,3); // SRODEK
sbi(PORTD,4); // L1
sbi(PORTD,5); // L2
sbi(PORTD,6); // L3

sbi(DDRB,1); // wyjscie SERWO
sbi(DDRB,3); // wyjscie SILNIK

cbi(PORTB,1); // SERWO OFF
cbi(PORTB,3); // SILNIK OFF

cbi(DDRC,5); // wejscie DZIELNIK
sbi(PORTC,4); //+ 5V

InicjalizacjaSERWO();
InicjalizacjaPWM();
LCD_Initalize();
LCD_Clear();

LCD_GoTo(0,0);
LCD_WriteText(" INICJALIZACJA ");
LCD_GoTo(0,1);
LCD_WriteText(" NACISNIJ START ");

while(START){}; // czekaj na wcisniecie start

for(i=0;i<=4;i++) // sygnal dzwiekowy 5 razy
{
sbi(PORTB,2); //BUZZER ON
_delay_ms(200);
cbi(PORTB,2); //BUZZER OFF
_delay_ms(200);

}
//pwm=150;
pwmustaw = 200; // ustawienie pwm
LCD_Clear();
sei(); // przerwania // nie uyzwane chwilowo

while(1)
{

if(menu==0) // sekscja pod menu ustawiania PWM
{

	if(UP)
	{
	_delay_ms(80);
	pwmustaw=pwmustaw+10;
	_delay_ms(80);
	while(UP){};
	}

	if(DOWN)
	{
	_delay_ms(80);
	pwmustaw=pwmustaw-10;
	_delay_ms(80);
	while(DOWN){};
	}

	LCD_GoTo(0,0);
	LCD_WriteText("USTAW PWM");

	utoa(pwmustaw,bufor,10);

	LCD_GoTo(0,1);
	LCD_WriteText("PWM = ");
	LCD_GoTo(5,1);
	LCD_WriteText(bufor);

	if(OK) // przejscie do drugiego menu, start robota
	{
	_delay_ms(80);
	menu=1;
	LCD_Clear();
	_delay_ms(80);
	while(OK){};
	}

}


if(menu==1) // sekcja menu glownego programu
{

	if(PIND==0b10000000) // jesli zgubiono lub badz nie ma linii
	{
		if(k==1) // pamiec ostatniego czujnika - L3
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=785;
		OCR1A = kat;
		}
		if(k==2) // pamiec ostatniego czujnika - P3
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1880;
		OCR1A = kat;
		}

		licz++; // opoznienie do zatrzymania robota

			if(licz==10000) // stop robota jesli
			{
			pwm=0;
			OCR2=pwm;
			kat=1350;
			OCR1A = kat;
			LCD_GoTo(0,0);
			LCD_WriteText("SERWO STOP");
			LCD_GoTo(0,1);
			LCD_WriteText("BRAK TRASY");
			licz=0;
			k=0;
			}



	}
	else
	{


		if(L3)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=785;
		OCR1A = kat;
		k=1;
		licz=0;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK L3");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 785 ");

		}

		if(L2)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=969;
		licz=0;
		OCR1A = kat;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK L2");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 969 ");

		}

		if(L1)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1160;
		OCR1A = kat;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK L1");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 1160");

		}

		if(SR)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1350;
		OCR1A = kat;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK SR");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 1350");

		}

		if(P1)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1510;
		OCR1A = kat;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK P1");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 1510");

		}

		if(P2)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1690;
		OCR1A = kat;
		licz=0;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK P2");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 1690");

		}

		if(P3)
		{
		pwm=pwmustaw;
		OCR2=pwm;
		kat=1880;
		OCR1A = kat;
		k=2;
		licz=0;
		LCD_GoTo(0,0);
		LCD_WriteText("CZUJNIK P3");
		LCD_GoTo(0,1);
		LCD_WriteText("SERWO 1880");

		}



	} // KONIEC ELSE


}


}// KONIEC WHILE

}//KONIEC MAIN