Kráčajúci mobilný robot

Pohonný subsystém

Obr. 1. Pohľad na modelárske servo

• Ako pohonný subsystém som zvolil modelársky servopohon, (ďalej servo) lebo je pomerne lacný a spoľahlivý. Štandardné typy poskytujú krútiaci moment okolo 30Ncm/4,8V prípadne až 40Ncm/6V Servo obsahuje jednosmerný motor prevodovku a z výstupným hriadeľom spriahnutý miniatúrny potenciometer, použitý ako senzor uhlu natočenia, pre regulačný obvod polohy. U menších a lacnejších serv je potenciometer pripojený priamo na výstupný hriadeľ. U serv vyšších kategórii je pripojený cez zvláštny prevod (tzv. nepriamy náhon), ktorý veľmi dobre chráni pred prenosom vibrácii. Požadovaná hodnota natočenia výstupneho hriadeľa je vo forme PWM signálu s úrovňou TTL (pulse wide modulation – puzne šírkova modulácia).

• PWM signal je taký, ktorý ma konštantnú amplitúdu a mení sa len šírka impluzu. Na riadenie seva je potrebná perióda 20ms a šírka impluzu v rozpätí od 1 až 2 ms. Šírka impulzu 1ms zodpovedá maximálnemu ľavému natočeniu a 2 ms maximálnemu pravému natočeniu výstupneho hriadeľa. Stredná poloha hriadeľa je pri šírke impulzu 1,5 ms. Rozsah uhlu natočenia býva u väčšiny servomotorov 180°. Poloha výstupného hriadeľa je mimo tento rozsah mechanicky aretovaná v prevodovke. Prevodový pomer určuje pomer medzi rýchlosťou a ťahom serva. Bežné je, že sa serva vyrábajú vo dvojiciach, ktoré majú rovnaký motor a elektroniku, ale líšia sa prevodmi - napr. serva Hitec HS-625 a HS-645 Jedno znich ponúka vyšší krutiaci moment pri nižšej rýchlosti, druhé má zvýšenu rýchlosť na úkor momentu. Prevody sú najčastejšie plastové, pre väčšie zaťaženie a väčšiu spoľahlivosť sa používajú kovové.

Obr.2. Bloková schéma vnútornej elektroniky

• Zjednodušené zapojenie vnútornej elektroniky serva je uvedené na obr. 2. Na vstup serva sa periodicky privádza riadiaci implulz, ktorý spustí monostabilný klopný obvod. Ten vygeneruje impulz o dĺžke zodpovedajúcej momentálnej polohe serva, ale opačnej polarity než je vstupný riadiaci impulz. Tieto dva impulzy sa porovnajú a výsledkom je rozdielový impulz, ktorý po zosilnení cez mostíkový snímač spôsobí roztočenie elektromotora jedným alebo druhým smerom. Elektromotor cez prevodovku otáča výstupným hriadeľom a súčasne i potenciometrom, ktorý pôsobí ako spätná väzba polohy do monostabilného klopného obvodu. Smer otáčania je taký, že impulz generovaný monostabilným klopným obvodom sa svojou dĺžkou približuje dĺžke vstupného riadiaceho impulzu, až sú obidva impulzy rovnako dlhé, elektromotor sa zastaví. Servo dosiahlo polohu, ktorá zodpovedá momentálne prijímanému riadiacemu impulzu.

Obr.3. Natočenie hriadeľa serva v závislosti od šírky riadiaceho impulzu

Odkazy:

• [1]

Zdrojový kód:

/*
  Microcontroller: ATmega32 
  XTAL Frequency: 12.00 MHz
  program generuje fazovo frekvencne korigovany PWM signal na pinoch PD4 a PD5
  vyuziva k tomu timer1 perioda PWM je 20ms (f=50Hz) definovane v registri ICR1
   sirka impulzu sa meni od od zavislovi cisla v komparacnych registroch OCR1A/OCR1B
*/
#include <avr/io.h>
#define F_CPU 12E6
#include <util/delay.h>
#define Rmotor OCR1A // vyvod PD5
#define Lmotor OCR1B // vyvod PD4

        //funcia na nastavenie I/O portov procesora
	void nastav_porty(void)            
		{
 			PORTA = 0b00000000; //
 			DDRA  = 0b00000000; // "0" is input  "1" is output
 			PORTB = 0b00000000; //
 			DDRB  = 0b00000000; //
 			PORTC = 0b00000000; //
 			DDRC  = 0b00000000; //
 			PORTD = 0b00000000; //
 			DDRD  = 0b00110000; //
		}
        
        //funkcia na nastavenie PWM 
	void nastav_timer1(void)
		{
 			TCCR1A = (1<<COM1A1)|(1<<COM1B1); // piny OC1A a OC1B nastavene ako PWM out
 			TCCR1B = (1<<WGM13)|(1<<CS11);    //fazovo a frekvencne kor.PWM; fosc/8;
			ICR1H= 0x3a;                      //nastavenie periody PWM 20ms (50hz) 15000cyklov
			ICR1L= 0x98;
		}

	int main(void)
	{	
		nastav_porty();
		nastav_timer1();
		Rmotor=750;   // sirka impulzu = 1ms
		Lmotor=1500;  // sirka impulzu = 2ms
		
                while(1)
		{			
			Rmotor=750;
			Lmotor=1500;
						
		}
	
	return 0;
	}

Priebeh vystupneho napatia na porte PD5(OCR1A)

Senzorový subsystém - fotoelektrické snímače (fotorezistor)

Princíp:

Obr.1. fotorezistor

Princíp fotorezistoru je založený na vnútornom fotoelektrickom jave: fotorezistor je vyrobený z polovodiča s vysokým elektrickým odporom. Ak fotón svetla dostatočne krátkej vlnovej dĺžky narazí do elektrónu vo valenčnom pásme atómu, odovzdá mu svoju energiu. Elektrón tak získa dostatok energie na prekonanie zakázaného pásma a preskočí z valenčného pásma do vodivostného. Tým opustí svoj atóm a pohybuje sa ako voľný elektrón v priestore kryštálovej mriežky. Na jeho mieste vznikla tzv. diera - voľné miesto s kladným nábojom. Takto vzniknuté voľné elektróny a diery vedú elektrický prúd a znižujú tak elektrický odpor osvetleného fotorezistoru.

Konštrukcia:

Na polovodičovú základňu (najčastejšie Sulfid kademnatý - CdS, kremík, germánium) sú z protiľahlých strán nanesené vrstvičky kovu v tvare hrabličiek, vodivo spojené s vývodmi fotorezistoru, tvoriace prechod kov - polovodič. Vplyvom osvetlenia sa mení elektrická vodivosť polovodiča medzi kovovými vrstvičkami.

Vlastnosti:

Obr.2. VA char. fotorezistora

Odpor fotorezistoru klesá v závislosti od intenzity osvetlenia približne exponenciálne, do istej miery je ale možné túto závislosť linearizovať.V závislosti od použitého materiálu je možné fotorezistorom detekovať široké spektrum vlnových dĺžok – od infračerveného žiarenia, cez viditeľné svetlo až po ultrafialové žiarenie

Výhody voči iným fotocitlivým súčiastkam:

dostatočná citlivosť (bežný CdS fotorezistor má v tme odpor rádu jednotiek MΩ, pri plnom osvetlení klesne na rádovo stovky Ω) nízke výrobné náklady jednoduché použitie v el. obvodoch možnosť použitia v obvodoch jednosmerného aj striedavého prúdu

Použitie:

Fotorezistory majú široké využitie pri meraní a regulácii v závislosti od intenzity osvetlenia, napr. v expozimetroch kamier a fotoaparátov, súmrakových spínačoch osvetlenia, regulátoroch jasu displejov a pod. Využívajú sa tiež v obvodoch spätnej väzby v kompresoroch dynamiky, kde v kombinácii s malou osvetľovacou žiarovkou prípadne LED znižujú zisk zosilňovača v závislosti od intenzity signálu.

Obr.2. VA char. fotorezistora

Pripojenie fotorezistora k procesoru:

Fotorezistor (FR) spolu s rezistorom R tvoria odporovy delič, voči napájaciemu napätiu a zemi. Napatie z deliča privádzame na vstup A/D prevodníka, ktorý je súčasťou mikroprocesora. Pomocou A/D prevodníka vieme merať napätie na deliči a tým určovať veľkosť okolitého osvetlenia. Avšak keď použijeme fotorezistory dva môžeme pomocou nich sledovať lúč svetla a tak aplikovať braintenbergov algorytmus milujuceho robota. Milujúci robot používa na prejavenie svojho správania fotosnímače (fotorezistory). Robot sa pohybuje v prostredí náhodnými zmenami smeru pokiaľ nenájde zdroj svetla, potom ho sleduje a drží sa v jeho blízkosti.

Použitie interného A/D prevodnika:

• Pre riadenie zabudovaného A/D prevodníka používame niekoľko špeciálnych registrov. Jedným z nich je register ADMUX (ADC Multiplexer Selection Register).

Obr.3. Register ADMUX

Význam jednotlivých bit v registri ADMUX podľa datasheetu ATmega32 Datasheet

REFS0, REFS1 slúžia k výberu analógovej referencie.

     REFS1      REFS0
       0          0       -vnútorna referencia
       0          1       -AVCC (kondenzátor na AREF a GND)
       1          0       -rezervovaný - bez významu
       1          1       -vnútorná referencia  2,56 V

Dôležité je , že pokiaľ připojujeme vnútornú referenciu, nesmie byť na vývod AREF pripojené vonkajšie napätie, je nutné na tento vývod pripojiť kondenzátor voči zemi.

ADLAR slúži k nastaveniu zarovnania výsledku prevodu.

    0 -  výsledok je  zarovnaný štandardne napravo. 
    1 -  výsledek zarovnaný doľava.

MUX4 - MUX0 slúži k výberu kanálu. Môže byť vybraný jeden kanál, použiteľný pre prevod alebo dva kanály(diferenčný prevod), kde sa prevádza rozdiel mezi úrovňami napätia týchto kanálov. Pri diferenčnom prevode môžeme nastaviť aj zosilnenie. Nastasvenie jednotlivých bitov je v datasheete. ATmega32 Datasheet

• Dalším dôležitým registrom je register ADCSRA (ADC Control and Status Register A). Tento register riadi prevod a informuje nás o stave prevodu.

Obr.3. Register ADMUX

ADEN: Zapína a vypína AD prevodník. Pokiaľ je vykonávaný prevod je hneď ukončený.

ADSC: Začne s AD prevodem na vybranom kanáli ,alebo dvojici diferenčných kanálov. Čítaním tohoto bitu zjisťujeme v akom stave je prevod. Pokiaľ prevod stále prebieha, je bit nastavený. Pokiaľ je bit vynulovaný, tak je prevod dokončený.

ADATE: Tento bit zaistí automatické spúšťanie nastaveného prerušenia.

ADIF: Príznak přerušenia po dokončení AD prevodu.

ADIE: Povolí prerušenie od AD prevodníka.

ADPS2,ADPS1,ADPS0: Nastavuje deliaci faktor medzi f oscilatora a vstupnou f pre AD prevodník. Deliaci faktor je 2, 2, 4, 8, 16, 32, 64, 128. nastavenie jednotlivých bitov je v datasheeteATmega32 Datasheet

Výsledok prevodu sa zapisuje do registrov ADCH a ADCL .Hodnotu týchto registrov musíme vždy čítať v poradí ADCL a potom ADCH aby sme mali istotu že čítame celú hodnotu po dokončení A/D prevodu. Pri použití winavr je výsledna hodnota v premennej ADCW

Výsledok po AD prevode: $ADCW={\frac {Vin*1024}{Vref}}$

Príklad pouzitia v C :

Zdrojový kód:

/*na pine PA0/ADC0 je pripojeny potenciometer s odporom 5K Ohm(voci GND a Vcc)
  na porte PD je pripojeny modul s 8 LED, ktore signalizuju velkost 
  napatia na vstupe A/D prevodnika.
  */

# define F_CPU 1200000UL
#include <stdio.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <util/delay.h>

uint8_t ch;

	//funkcia nastavuje I/O porty
	void init_avr(void)
		{
 		
 			DDRD  = 0b11111111; //PORTD je nastaveny ako vysupny port 
			PORTD = 0b00000000; //pripojene LED
			
 			}

	//funkcia na inicializaciu A/D prevodnika
	void adc_init(void)

		{
		  //nastavenie napatovej referencie kondenzator na AREF pin
		  ADMUX |= (0 <<REFS1) | (1 <<REFS0);
			 
		  //nastevenie preddelicky a povolenie prerusenia od ADC 
		  ADCSRA|=(1<<ADEN)|(1<<ADIE);

		}

	//funkcia pre prevod
	//A/D prevodnik prevadza vstupne napatie na vybranom kanali na cislo
	void adc_start_prevod(uint8_t kanal)
		{
			//nastav  cislo prevadzaneho kanalu 
		        ch=kanal;
			//nastav ADC kanal 
			ADMUX=(ADMUX&0xF0)|kanal;
			//Odstartovanie prevodu s prerusenim po dokonceni prevodu
			ADCSRA |= (1<<ADSC)|(1<<ADIE);
		}

	//obsluha prerusenia po prevode ADC
	ISR(ADC_vect)
          {  
              //v premenej ADCW sa nachadza cela hodnota ADCH+ADCL po dokonceni prevodu	
	
	     if (ADCW<205)// do 1V
			PORTD=0b11101111;
	
	     else if (ADCW<409)//do 2V
			PORTD=0b11110111;
	
	     else if (ADCW<614)//do 3V
			PORTD=0b11110111;
	
	     else if (ADCW<818)//do 4V
			PORTD=0b11111011;

	     else if (ADCW<1023)//do 5V
			PORTD=0b11111101;
	  }

//hlavna slucka 
 int main(void)

	{	
		init_avr();	//incializacia portov
		adc_init();     //inicializacia A/D prevodnika
		sei();          //povolenie gobalnych preruseni
		
		while(1)
		{
		//start prevodu = citaj hodnotu napatia na kanali 0(PA0) 
               	adc_start_prevod(0);
		_delay_ms(30);
		PORTD=0xff;
				
		}        
      		
	}

Senzorový subsystém - Infračervený detektor prekážok

Obr.8. IR LED a IR T

Infračervený detektor prekážok (IR detektor/senzor) slúži na detekovanie prekážok v blízkom okolí robota, rádovo desiatky centimetrov, nazýva sa „Near proximity sensor". Tieto senzory sú citlivé v oblasti vlnových dĺžok pod viditeľným svetlom, najčastejšie okolo vlnovej dĺžky 880 nm a sú necitlivé na svetelné žiarenie iných vlnových dĺžok. Princípom IR senzora prekážok je detekcia odrazeného infračerveného svetla od prekážky. Toto svetlo je emitované infračervenou (ďalej IR) LED diódou. Ako senzor býva použitý fototranzistor citlivý v infračervené oblasti (IR tranzistor), nebo IR fotocitlivá dióda. Tento senzor poskytuje dvojhodnotový signál - detekuje odrazený IR signál nedetekuje odrazený IR signál, respektíve detekuje prekážku/nedetekuje prekážku. Nevýhodou IR senzorov pracujúcich na princípe detekcie odrazeného IR svetla je, že množstvo odrazeného svetla je závislé na farbe prekážky a druhu povrchu. Pre úplnosť dodajme, že intenzita emitovaného (a samozrejme i odrazeného) IR svetla je nepriamo úmerná druhej mocnine vzdialenosti. V praxi je z pravidla IR fototranzistor nahradený, respektíve doplnený špecializovaným IR prijímačom s integrovaným demodulátorom (ako napr. Sharp GP1U52X, Siemens SFH506, SFH5110 apod.). Výhodou týchto modulov (vďaka zabudovanému demodulátoru) je, že sú citlivé iba na modulované IR žiarenie o určitej vlnovej dĺžke generovanej IR LED (vysielačom). Modulačný kmitočet býva najčastejšie 36, 38, 48 a 56 kHz. Dôvodom použitia modulácie je eliminovanie vplyvu IR žiarenia v okolitom svetle. Vzhľadom k vyššie uvedenému princípu využívajúceho modulovaného infračerveného svetla je potreba budenie IR LED modulovať. To je v zásade možné riešiť hardwarovo, nebo softwarovo. Použitie modulovaného svetla je v praxi nevyhnutné, kvôli vysokým hodnotám okolitého IR žiarenia

Popis obvodu SFH 5110 od firmy OSRAM podľa datasheetu SFH5110 datasheet

SFH 5110 a SFH 5111 sú IR prijímače pre detekciu IR žiarenia z diaľkového ovládaných systémov. IC zahŕňa fotodiódu, predzosilňovač, automatickú kontrolu zosilnenia, Band-pass filter a demodulátor. Čierne puzdro snímača je navrhované s filtrom denného svetla tak, aby ho neovplyvňovalo.

Obr.9. blokov ashcema SFH 5110

Funkcie obvodu

• IC s monoliticky integrovanou fotodiódou (riešene na jednom čipe)
• Obzvlášť vhodné pre aplikácie pracujúce na vlnovej dĺžke 940 nm.
• Vysoká citlivosť
• Rôzne frekvencie s pásmovým filtrom
• TTL a CMOS kompatibilita
• Výstup: aktívna úroveň pre logiku je logická nula (Low)
• nie je potreba pripájať ďalšie súčiastky
• SFH 5110 sa vyrába s týmito modulačnými frekvenicami : 30,33,36,38,40 KHz

Aplikácie

• Diaľkovo ovládaný modul pre televízory, Video zariadenia, hi-fi,

audio prijímače, SAT prijímače a kompaktné diskové prehrávače.

• Optické spínače

Rozsahy snímača

• Rozsah prevádzkovej teploty: Top = – 10 až + 75 °C
• Napájacie napätie: Vcc max = 6.3 V
• Napájací prúd: Icc = 5 mA
• Výstupné napätie: Vout = 6.3V
• Výstupný prúd: Iout = 3 mA
• Výkonová strata: Ptot = 50 mW
• Kľudový prúd (Ucc=5V,E=0) Icc = 1.3 mA
• Max hodnota vlnovej dĺžky ls max = 940 nm

Pripojenie k mikroprocesoru

Rezistor R+ a kondenzátory C1 a C3 slúžia na filtráciu napájacieho napatia v prípade kolísania napájacieho napätia. Pri príjmani modulovaného IR svetla dostávam na výstupe snimača logickú úroveň nula(Low)

Obr.10. IR LED a prijmac sfh 5110

Zdrojovy kod

/*
  Microcontroller: ATmega32
  XTAL Frequency: 12.00 MHz

  program generuje fazovo frekvencne korigovany PWM signal na pine PD5, kde je 
  pripojena IR LED cez 5 KOhm trimer na zem. vyuziva k tomu timer1 perioda PWM 
  je 27,7us (f=35928Hz) definovane v registri ICR1=167 (0xA7)plnenie PWM je 50%
  (pri vacsom alebo mensom plneni vieme regulovat jas  IR LED)
  a definujeme ho v komparacnom registri OCR1A=167/2=84 (0x54).

  Na porte B je pripojeny modul 8xLED a ak detekujeme prekazku, tak LED na 
  PB0 bude svietit

*/
#include <avr/io.h>
#define F_CPU 12E6
#include <util/delay.h>
#define IRled OCR1A  // na vyvode PD5 je pripojena IRLed


        //funcia na nastavenie I/O portov procesora
	void nastav_porty(void)
		{
 			PORTA = 0b00000000; //
 			DDRA  = 0b00000000; // "0" is input  "1" is output
 			PORTB = 0b00000000; //
 			DDRB  = 0b00000001; //na porte B mame pripojene indikacne LED 
 			PORTC = 0b00000000; //
 			DDRC  = 0b00000000; //
 			PORTD = 0b00000000; //
 			DDRD  = 0b00110000; //
		}

        //funkcia na nastavenie PWM
	void nastav_timer1(void)
		{
 			TCCR1A = (1<<COM1A1)|(1<<COM1B1); // piny OC1A a OC1B nastavene ako PWM out
 			TCCR1B = (1<<WGM13)|(1<<CS10);    //fazovo a frekvencne kor.PWM; fosc/1;
			ICR1H= 0x00;                      //nastavenie periody PWM 27,7us (35928hz) 167cyklov
			ICR1L= 0xA7;
		}

	int main(void)
	{
		nastav_porty();
		nastav_timer1();
		IRled= 0x54;
		

                while(1)
		{
		   if(bit_is_clear(PIND,7))
	              PORTB=0b00000000;

		   if(bit_is_set(PIND,7))
	              PORTB=0b00000001;
					
		    }

	    return 0;
	 }

Meranie vyžarovacej charakteristiky IR snímača

Snímač obsahuje vysielač a príjmač.

• Ako vysielač je použitá IR LED ktorá je pripojená na PD5(OCR1A)cez 5 KOhm rezistor, ktorým nastavujeme intenzitu emitovaného svetla. Svetlo IR LED je modulované frekvenciou 36KHz aby sa zabránilo rušeniu z okolitých zdrojov IR žiarenia.

• Príjmač je tvorený obvodom SFH 5110 v zapojení podľa datasheetu s výstupom pripojeným na PD7. ak snímač detekuje prekážku, tak sa rozsvieti červena LED pripojená na PB0.

Charakteristiku som meral nasledujucim spôsobom:

Zobral som švorčekový papier s rastrom 5x5mm a od snímača som kolmo zakreslil jeho os a od tejto osi som zakreslil čiary v rozpäti 10°. potom som sa postupne približoval po týchto čiarach smerom k snímaču pohyboval plastovzm valčekom (d=10mm, oranžová farba) a kde snímač detekoval prekážku tam som zakreslil bod. nasledne som body pomocou excelu pospájal, vykreslil a dostal som reálnu smerovu charakteristiku snímača

Obr.11. Vyžarovacia charakteristika snímača

.

Riadiaci Subsystém - schéma zapojenia riadiacej dosky

Obr.12. Schéma zapojenia riadiacej dosky robota

.