pm:prj2010:avoinescu:ivolution

I-volution

I-volution

Proiectul consta in construirea unui robot din Lego Technic care va merge pe 4 picioare folosind servomotoare. Acesta va putea fi controlat prin infrarosu prin intermediul unei telecomenzi.

Mi s-a parut interesanta ideea de a recrea miscarile unui animal.

Descriere generala

Schema logica

Robotul se va deplasa pe 4 picioare, fiecare picior fiind controlat prin doua servomotoare (comandate prin PWM).

Hardware

Schema placii in Isis 6

Servomotoare

am optat pentru AS-100 Nano, cumparat de la Sierra:

Specificatii:

Dimensiuni: 22.5 x 12 x h 24 mm
Tensiune nominala: 4.8 - 6.0 V
Cuplu: 1.2 kg-cm
Viteza: 0.12 sec/ 60°

Servomotoarele au trei fire de intrare:

Rosu → Vcc (deobicei 4.8 - 6V)
Negru → Masa
Alb → PWM

Comanda servomotorului se face cu ajutorul semnalului modulat PWM, trimis pe pinul de PWM (). Servomotorul compara semnalul PWM cu pozitia sa curenta si o ajusteaza corespunzator. Circuitele interne ale servomotorul asteapta un semnal cu frecventa constanta de 50Hz ↔ se repeta o data la 20ms.

Semnalul trimis servomotorului este unul HIGH pentru 1-2ms si LOW pentru restul pana la 20 ms. Durata cat este semnalul HIGH determina noua pozitie a servomotorului. Servomotoarele mele au un unghi de rotatie maxim de 90 ° astfel ca:

1.0 ms ↔ -45 °
1.5 ms ↔ 0 °
2.0 ms ↔ +45 °

Ca hardware, am folosit doua circuite integrate cu 4 porti AND a cate doua intrari fiecare: am folosit aceste circuite pentru a crea un mod de Enable pe fiecare servomotor. Astfel in fiecare poarta AND intra semnalul PWM si cate un pin de la portul B (8 servomotoare - 8 pini portul B).

TODO :: adaugarea poze clare cu placuta de extensie

Modul de enable este bazat pe modul de functionare al portii AND: cat timp pinul de enable este 0, iesirea este 0, deci servomotorul nu primeste semnal. Cand pinul de enable trece in HIGH, poarta va lasa sa treaca doar partea HIGH din PWM, continuata iar de LOW, astfel trimitandu-i doar servomotorului corespunzator semnalul PWM.

Interesanta este constructia robotului:

Fiecare picior are 2 servomotoare: unul care il ridica de la sol, altul care il deplaseaza stanga-dreapta; astfel servomotoarele se impart in servomotoare incheietura si servomotoare trunchi.

Fiecare picior este fixat pe cate un colt al trunchiului (initial ele sunt paralele intre ele si perpendiculare pe trunchi).

Constructia piciorului

piciorul propriu-zis este format dintr-o parghie: cand servomotorul incheietura actioneaza asupra capatului parghiei, piciorul se ridica. Legatura dintre servomotorul incheietura si pargie s-a realizat printr-o sarma (agrafa).

TODO :: poza clara parghie picior

piciorul este prins de un profil U in care rezida servomotorul incheietura. Acest profil U este fixat in partea de jos de trunchi printr-un tambur rotitor, iar sus este prin (cu banda izolatoare) de servomotorul trunchi. Servomotorul trunchi este fixat de “acoperisul” trunchiului.

TODO :: film functionare picior

Servomotoarele au sursa diferita de tensiune: 6 baterii de 1.5 V inseriate, bateriile sunt reincarcabile (nu le incarc total sa nu sara de 6 V total pentru a nu arde servomotoarele).

Miscarea propriu-zisa este interesanta. Pentru exemplificare, voi discuta miscarea inainte:

Practic, la deplasarea unui singur picior intai se comanda servomotorul incheieturii pentru a ridica piciorul, apoi se comanda servomotorul de trunchi pentru a deplasa piciorul, iar la urma se comanda iar servomotorul incheietura pentru a cobori piciorul. Singura exceptie o face ultimul pas al oricarei miscari: dupa ce toate picioarele au fost pozitionate, servomotoarele trunchi pornesc toate 4 deodata (se da enable in PORTB pentru toate 4). Avand picioarele lipite de pamant (servomotoarele incheietura nu au fost comandate), trunchiul va fi impins inainte spre exemplu. Cele 4 servomotoare trunchi nu vor fi comandate prin acelasi semnal PWM ci prin doua: cele de pe partea stanga se vor deplasa -45°, pe cand cele de pe partea dreapta +45° (sau invers, in functie de miscare). Initial vroiam sa folosesc un singur semnal PWM cu care sa comand toate servomotoarele (acest fapt era posibil daca puneam doua din servomotoarele trunchi invers - servomotoarele trunchi sunt plasate pe verticala, astfel ele sunt prinse de partea de sus a trunchiului actionand pe profilul U al piciorului de sus in jos; puteam sa le fixez si de jos in sus, adica sub profilul U al piciorului).

Miscarea cu spatele este asemanatoare, doar ca se inverseaza valorile de PWM pentru motoarele trunchi.

Miscarea stanga/dreapta este mai interesanta. Exemplific pe miscarea spre dreapta (cea spre stanga are valorile PWM inversate): se deplaseaza picioarele stanga in fata (asemenea miscarii inainte), iar picioarele din dreapta se deplaseaza in spate (asemenea miscarii inapoi), iar la sfarsit motoarele trunchi se vor deplasa combinand miscarea inainte cu cea inapoi - motoarele trunchi de pe partea stanga se vor deplasa -45°, iar cele de pe partea dreapta +45° (toate 4 deodata).

De asemenea, am montat si un receptor Infrarosu, dar nu i-am gasit datasheet-ul pe net (mare atentie cand va cumparati vreunul, nu sunt scumpe, dar alegeti ceva la care aveti datasheet).

Software

Software-ul este foarte simplu (vezi Resurse). De fiecare data cand se doreste comandarea unui servomotor, se activa enable-ul corespunzator din Portul B si se genereaza PWM. Intotdeauna picioarele de pe o parte (stanga sau dreapta) pot fi ori perpendiculare pe robot, ori la -45° fata de el (ramase in spate). Acest fapt se datoreaza rotatie tututor celor 4 servomotoare trunchi la sfarsitul fiecare miscari. Singurul moment in care pot aparea discrepante este atunci cand robotul tocmai a mers in spate, si acum se merge in fata, dar daca un picior este deja in fata si el este comandat sa se duca la pozitia curenta nimic nu se va intampla (doar ca piciorul se va ridica si se va cobori fara a se deplasa in lateral).

Pentru a genera semnalul PWM de 50 Hz am folosit modul Phase and Frequency Correct al timerului 1 de pe Atmega16. In acest mod de operare, timer-ul porneste de la 0, numara pana la ICR1 si coboara inapoi la 0. Pentru ca acest ciclu sa dureze 20 ms am optat pentru:

prescaler 1/8
ICR1 = 20000
OCR1A si OCR1B vor lua valori intre 1000-2000 pentru 1-2ms

Cum functioneaza PWM-ul in acest mod?

Datasheet-ul Atmega16 ne lamureste cum functioneaza acest mod si cum interactioneaza cu OCR1A si OCR1B:

TCNT1 este registrul in care se numara de la 0 la ICR1: la intalnirea pe ramura ascendenta a OCR1A/OCR1B, OC1A/OC1B este pus pe HIGH, iar pe ramura descendenta pe LOW.

O diagrama mai edificatoare in acest sens:

Se poate observa folosirea ambelor canale PWM al Timerului 1 (A si B). Am decis folosirea ambelor intrucat motoarele trunchi de pe stanga nu doar ca se deplaseaza invers fata de cele de pe partea dreapta (vezi explicatia de la sectiunea Hardware), dar, de exemplu pentru miscarea fata, cele de partea dreapta trebuie sa se deplaseze la 1500 cat timp cele de partea stanga la 2000, astfel apareau discrepante. Cred ca problema se putea rezolva si doar cu un PWM, doar ca nu m-am prins cum sa ma joc cu durata ciclului servomotoarelor.

Fiind doua canale PWM si doua circuite AND cu 4 porti, canalul A va comanda primele 4 servomotoare (partea dreapta) si celelalte ultimele 4 (partea stanga).

Algoritmul de deplasare este simplist: trebuie avuta atentie la valorile PWM si la enable-urile de pe port-ul B (acesta este setat ca output LOW/HIGH in functie de nevoi).

Functia de comanda pentru canalul A (analog si pentru canalul B):

void PWM_channelA(uint8_t motor_mask, int value) {
	PORTB = motor_mask;		// dau enable motorului (sau motoarelor)
	OCR1A = value;			// stabilesc noua pozitie
	_delay_ms(1000);		// astept 20 ms (pentru PWM) + durata stabilirii mecanice a noii pozitii a servomotorului
}

Variabila motor_mask reprezinta pinii la care se face enable:

#define MOTOR1 _BV(PB0)	// Trunchi dreapta fata
#define MOTOR2 _BV(PB1)	// Incheietura dreapta fata
#define MOTOR3 _BV(PB2)	// Incheietura dreapta spate
#define MOTOR4 _BV(PB3)	// Trunchi dreapta spate
#define MOTOR5 _BV(PB4)	// Trunchi stanga fata
#define MOTOR6 _BV(PB5)	// Incheietura stanga fata
#define MOTOR7 _BV(PB6)	// Incheietura stanga spate
#define MOTOR8 _BV(PB7)	// Trunchi stanga spate

#define TORSO (MOTOR1 | MOTOR4 | MOTOR5 | MOTOR8) // trunchiul

Explicarea codului pentru miscarea inainte:

void move_forward() {
	if(offset_left > 0) {	// daca picioarele de pe partea stanga au ramas in spate
		// ele se vor misca primele
		// acum incep cu stangul din spate => offset pe dreapta
		offset_left = 0;
		offset_right = STEP;	// marchez ca partea dreapta va ramane in spate
		
		// picior stanga fata
		PWM_channelA(MOTOR6, MAX);   // ridic
		PWM_channelA(MOTOR5, MIN);   // deplasez
		PWM_channelA(MOTOR6, MED);   // cobor

		// picior stanga spate
		PWM_channelA(MOTOR7, MAX);   // ridic
		PWM_channelA(MOTOR8, MIN);   // deplasez
		PWM_channelA(MOTOR7, MED);   // cobor

		// miscare trunchi
		PORTB = TORSO;
		OCR1A = MED;
		OCR1B = MIN;
	} else {	// altfel se misca picioarele de pe partea dreapta
		// plec cu dreptul => offset pe stanga
		offset_right = 0;
		offset_left = STEP;	// marchez ca partea stanga va ramane in spate

		// picior dreapta fata
		PWM_channelB(MOTOR2, MIN);   // ridic
		PWM_channelB(MOTOR1, MAX);   // deplasez
		PWM_channelB(MOTOR2, MED);   // cobor
		
		// picior dreapta spate
		PWM_channelB(MOTOR3, MIN);   // ridic
		PWM_channelB(MOTOR4, MAX);   // deplasez
		PWM_channelB(MOTOR3, MED);   // cobor

		// miscare trunchi
		PORTB = TORSO;
		OCR1B = MED;
		OCR1A = MAX;
	}
}

In variabilele offset_left si offset_right retin daca vreuna din parti nu este perpendiculara pe trunchi (a ramas in spate in urma unei deplasari). Am nevoie de acesti indicatori pentru a sti pe cine mut intai (logic ca picioarele ramase in spate vor fi cele deplasate). Aceste variabile mai sunt extrem de folositoare la miscarea dreapta/stanga: daca vreau sa mut spre stanga si picioarele din stanga sunt deja in spate, nu mai are rost sa le mut.

Valorile MIN, MAX, MED si STEP sunt macro-uri pentru valori PWM (mai putin STEP, acesta putea lua orice valoare admisa, avand rol mai mult de booleana):

#define MAX 2000
#define MED 1500
#define MIN 1000
#define STEP 500

Apoi se poate observa miscarea propriu-zisa a unui picior: intai este comandat servomotorul care ridica piciorul, apoi se deplaseaza piciorul, apoi se coboara.

Miscarea trunchiului se realizeaza ultima. Aceasta nu se poate realiza cu functiile PWM_channel* intrucat doua dintre servomotoarele trunchi se afla sub comanda canalului A si celelalte 2 sub comanda canalului B.

Pentru mai multe detalii vezi Resurse.

TODO :: film functionare servomotoare in gol (pentru miscarea inainte)

Rezultate obtinute

Din nefericire constructia robotului a avut hibe, servo motoarele nu au fost destul de puternice pentru a ridica si muta picioarele (foarte grele au fost bateriile). In schimb, circuitul doar cu servomotoare a mers corespunzator (pacat ca fara folos).

Senzorul IR cumparat a fost defect (nu se gasea nici datasheet pe internet).

Concluzii

Proiectul mi s-a parut extrem de interesant, mai ales partea mecanica. Oarecum dezamagitor a fost ca nu am putut prevede ca servomotoarele nu pot ridica atata greutate.

Pe viitor voi incerca sa construiesc un alt “sasiu”, mai usor.