# Roboti >  Robots URMAS - konstrukcija, shēma, programma

## karloslv

Esmu nolēmis publiskot sava jaunākā līnijsekotāja robota Urmas konstrukciju. Šis robots ieguva dalītu pirmo vietu robotikas sacensībās septembrī. 

Kopskats: robots uzmontēts uz piedziņas bloka, kurš atbalstās uz sliedītēm un riteņiem, turpat uz sliedītēm priekšgalā ir līnijas sensors, virs piedziņas bloka ir divi litija jonu akumulatori un kontroliera plate, aizmugurē jaudas plate


Augšskats: redzama kontroliera plate (principā tikai ATMega88 un spraudņi, tie divi opampi augšpusē šobrīd shēmā netiek izmantoti) un jaudas plate (L298N dubultais H-tilts, barošanas slēdzis, noplūdes diodes, radiators)


Apakšskats: redzams līnijas sensors (6 atstarotās gaismas sensori vieglā tekstolīta kastītē), Tamiya zobratu un motoru piedziņas komplekts un robota atbalsta sliedītes


Sānskats: priekšplānā redzams LEGO ritenis, uz plates UART spraudnis un poga, uz jaudas plates kopējais barošanas slēdzis.


Turpinājumā sekos domu gājiens, shēma un programma.

----------


## Vinchi

Baigi smukie riteņi!  ::  Robots arī izskatās tā tīri neko, vienīgais kāpēc nekodināji shēmu, laika trūkums?

----------


## Epis

Kādi ir nākotnes plāni robota uzlabošanā saistībā ar sensoriem, tagat skatos ka izņemot atstarotās gaismas sensorus vairāk laikam nekādu apkārtējās vides sensoru tur nav, ir doma kautko vēl pielikt klāt ??

----------


## karloslv

Shēmu nekodināju tādēļ, ka nemaz to negribēju. Visas detaļas tur ir caurumos lodējamās, uzskatu, ka tādām plati taisīt ir veltīga laika šķiešana. Šeit tika izmantota maketplate, kurai visi celiņi apakšā garenvirzienā ir jau savienoti. Tad atliek tikai pārgriezt vajadzīgajās vietās, un shēmu ir viegli salikt. Tādai platei ir liels pluss - ļoti viegli izveidot barošanas šinas (tās jau tur ir). Šai platei abpus AVR iet +5/GND šinas, un no tām ir viegli ar mazu vadiņu tālāk novadīt. Vēlāk nofotografēšu plates apakšpusi - tajā ir tikai kāju lodējumi, nekādu detaļu, nekādu vadiņu, tikai celiņi garenvirzienā, kuri vietumis pārgriezti. Turklāt tādu plati ir ļoti viegli papildināt. Uz šīs pašas plates darbojās iepriekšējais robots (Desotājs), tad plate pārtapa par bateriju lādētāju (tur, kur tagad opampi, bija ielodēts P-ch mosfets, drosele u.c. detaļas), un tagad tur ir ielodēti opampi, kas strādā kā enkodera komparatori (tie pat strādā, šim pašam robotam vienā riteņu apgriezienā 400 tikšķu). Par enkodera nolasītājiem izmantoju optopārus no mehāniskās peles, un par pārtraucēju izmantoju tos pašus piedziņas zobratiņus. Taču enkoderis robotam izrādījās nevajadzīgs, brauc arī bez tā. 

Tālāk par domu gājienu.

Galvenais mērķis bija radīt ļoti manevrētspējīgu robotu. Pakasot aiz auss, kļuva skaidrs, ka jāizmanto vieglas detaļas un vismasīvākās detaļas jākoncentrē maksimāli vienkopus starp riteņiem - tas samazina inerces momentu. Jo mazāks inerces moments, jo mazāka inerce pagriezieniem. 

Tamiya double gearbox (http://www.pololu.com/products/tamiya/0114/) izrādījās ideāli piemērots. Tas pat labu laiku bija Tevalo piedāvājumā (kaut kur šeit: http://www.tevalo.lv/cgi-bin/index.cgi?ec=4a7b15c), taču šobrīd izskatās pazudis. Galvenais labums ir tāds, ka pārnesumus var salikt 4 variantos (12.7:1, 38:1, 115:1, un 344:1). Ir svarīgi izvēlēties pareizo pārnesumu ne tikai ātruma dēļ, bet arī griezes momenta dēļ. Es lēsu, ka mans robots svērs ap 200g un brauks ar ātrumu 1m/s. No svara var izrēķināt maksimālo paātrinājumu (pieņemot riteņu saķeres berzes koef. 0,7) 7m/s^2 un spēku 140g, katram ritenim tātad 70g. Riteņu diametrs man ir 4cm, tātad maksimālais griezes moments katram ritenim 140 g-cm. Būtu vēlams, lai tādu griezes momentu ritenis var attīstīt, griežoties ar 480 rpm (tas izriet no 1m/s ātruma un 4cm diametra). Pilnais ieskriešanās/bremzēšanas ceļš pie tādiem nosacījumiem ir ~7,5 cm - manuprāt labs rādītājs manevrētspējai (protams, neņemot vērā griezes momentu). 

Tālāk lasām specenes. Viena motoriņa griezes moments pie 3V barošanas un 0RPM (stall) ir 36 g-cm. Es izvēlējos barot motorus forsēti, pieņemsim, ar 6V. Tātad griezes moments būs 72 g-cm. Tas pats ar apgriezieniem bez slodzes - 12300 RPM vietā būs ap 20000 RPM (ņēmu mazāk, jo pārnesuma zobrati rada savu slodzi). Tātad pārnesuma koeficients 38:1 derēs lieliski - tad pie 6V bez slodzes riteņi griezīsies ar 526 RPM, bet maksimālais griezes moments būs 2700g-cm. Tas nozīmē, ka vēlamos 140 g-cm es varēšu sasniegt vēl pat pie 500 RPM. Šķiet, ka tāds risinājums būs ideāls izvēlētajam svaram.

Barošanu izvēlējos 2x LiIon akumulatorus (7,4 - 8,2V) no veca laptopa. Nekādus pamatotus argumentus šobrīd neatceros (varbūt īsti nesanāca izvēlēties piemērotu pārnesumu pie tāda sprieguma). Iespējams, ka ar vienu elementu varētu radīt pat vēl veiklāku robotu, jo samazinās kopējais svars. Iepriekšējā robotā arī tika izmantoti 2 litija akumulatori. 

Motoru sprieguma vadībai bija paredzēts PWM tāpat kā iepriekšējā modelī. Ērti izmantot Atmega iebūvētos PWM. Šoreiz gan iecerēju izmantot pilnos H-tiltus, jo bija skaidrs, ka kontroles algoritms var izdomāt kādu riteni forsēti bremzēt (ar negatīvu spriegumu). H-tiltus es labprāt būtu gribējis uz MOSFET, taču laika trūkuma un citu apstākļu dēļ paņēmu L298N. Tas nav nekāds skaistulis uz tik zemu barošanas spriegumu un lielām strāvām, jo uz tranzistoriem nokrīt savi 2V. Tādēļ arī radiators.

Tālāk iedvesmojos no ELM robota (http://elm-chan.org/works/ltc/report.html). Protams, uz tādu miniaturizāciju netēmēju, taču dažas idejas bija lieliskas, piemēram, līnijas sensors. Ap šo brīdi biju pamatīgi ieinteresējies par kontroles algoritmiem, un bija skaidrs, ka, lai novadītu robotu precīzi pa līniju (kas nebūt nenozīmē visātrāko trajektoriju, taču garantē izbraukšanu), nepieciešams precīzi zināt, cik liela ir tekošā nobīde no trases. 

Labās idejas ir vairākas:
1) fotosensoru mērījumus veikt divreiz - ar ieslēgtu apgaismojumu un bez tā (protams, tas jāvada no mikrokontroliera)
2) salīdzināt blakusesošo fotosensoru mērījumus un noteikt, kur tieši starp tiem atrodas līnija
3) PD kontroles algoritms, kurš ņem vērā gan novirzi no trases, gan to, cik ātri šī kļūda mainās

----------


## karloslv

Epi: šim robotam neko nedomāju papildināt, jo tas ir tikai līnijsekotājs, un strādā labi. Rakstīju jau par enkoderiem, tos biju uztaisījis un tie strādāja diezgan labi, taču nebija nepieciešami.

Shēma sanāca šāda:


Dažas piezīmes par shēmu:
1) Visas infrasarkanās gaismasdiodes mikrokontrolieris ieslēdz reizē. Kopējā gaismasdiožu strāva sanāk ap 16 mA. 
2) Izmantoju iebūvēto 8 MHz oscilatoru, jo laika skaitīšana nav tik kritiska, lai izmantotu kvarcu
3) Izmantoju savu štekeri ISP programmēšanai (man ir paštaisīts STK200 analogs uz printera porta), kurš sastāv no 2 trijniekiem - to ir daudz ērtāk izvietot uz plates (MOSI/MISO/SCK trijnieks tieši sakrīt ar ATMega kājām) nekā standarta 2x5 pinu ligzdu.
4) saziņai ar datoru izmantoju UART, taču pārvēršanu no TTL uz RS232 līmeņiem neveicu pats - man ir kaut kāds vecs Nokia kabelis, kuram tas pārveidotājs jau ir iebūvēts spraudnī.

----------


## abergs

Arī par sensoriem.Kā sekošanu iespaido virsma: matēts galds+spīdīga līnija,spīdīgs galds+matēta līnija?
Un cik daudz sekošanu ietekmē apgaismojums(kvēlspuldzes,dienasgaismas,zibspuldzes)?

----------


## karloslv

abergs: Sensori ir ITR9908 (gūglī var dabūt specenes) - tie faktiski ir infrasarkanā gaismasdiode (IR204/L10) un fototranzistors (PT204-6B) sabāzti vienā platsmasas detaļā. Tādus var dabūt Argusā.

Ārējais apgaismojums ietekmē minimāli, jo sensoru mērījumi tiek veikti divreiz (ieslēdzot un izslēdzot IR diodes), un nolasījums ir abu mērījumu starpība. Turklāt drošības pēc uzbūvēju kastīti, kas nosedz lielāko daļu traucējošo gaismu. 

Par virsmu runājot - neesmu tā izvērsti eksperimentējis, taču zinu, ka uz sacensību trases strādāja (papīrs + tinte), un strādāja arī man mājās, kur es to robotu testēju un veidoju. Tur ir kartona grīda, kas reiz krāsota brūna, taču krāsa ir izberzta, un tādēļ grīda plankumaina. Uz grīdas uzlīmēju melnu izolācijas lenti, un robots brauca diezgan labi. 

Man ir sajūta, ka līnijas noteikšanas algoritms ir diezgan robusts un pietiek ar pat pavisam nelielu atšķirību atstarošanas koeficientā, lai robots spētu sekot līnijai. Nelieli traucējumi uz trases (plankumi) arī īpaši netraucē. Ieliku vienkāršu loģiku, kas apstrādā arī gadījumu, kad robots iebrauc krustojumā. 

Vakarā ielikšu arī robota programmu (valodā C).

----------


## abergs

Paldies, skaidrs!  ::

----------


## Delfins

Viss ko varu pateikt - žetons  :: 

PS: neesi domājis par sensoru izvietošanu pa loku !? To biš lai centra sensori ir tālāk no centra, jo tad "redzēs" nedaudz tālāk un varēs iegūt to papild ātrumu trases pārvarēšanai un attiecīgi iegūt pirmo vietu  ::

----------


## Andrejs

Malacis! 
Saņemšos un ielikšu savējo "zvēru"bildes. Taa teikt durnoj primer zaraziķeļen   ::  
Iesaku pārējiem robotikas faniem sekot labajam piemēram.

Andrejs
P.S. vai nav bildes no 1. sacensību aparāta?

----------


## karloslv

Paldies, Andrej  ::  Es tieši tā arī biju iecerējis, lai arī nepateicu ievadā - cerams, ka sasparosies gan tie, kas ir kaut ko uztaisījuši, gan tie, kas nevar vien saņemties. Cerams arī, ka kādam mans domu gājiens un apsvērumi noderēs, veidojot kaut ko līdzīgu.

No visiem aparātiem bildes ir šeit: http://picasaweb.google.com/karlis.goba/Robo (tiesa gan, Desotājs tur redzams bez plates. Desotājam tika izmantota šī pati plate ar uzlodētiem 2 lauktranzistoriem. un izrādās, Desotājam tomēr bijuši 4 LiIon elementi, 2 paralēli, 2 virknē)

----------


## karloslv

Delfin: neesmu pārliecināts, ka tāds izvietojums tiešām kaut ko dod. Respektīvi, intuitīvi varbūt tā arī liekas, bet pamatojumu pagaidām neredzu. Turklāt šādi bija daudz vienkāršāk uztaisīt  ::

----------


## noos_

> Vakarā ielikšu arī robota programmu (valodā C).


 Ļoti interesētu.

----------


## karloslv

Lūk arī solītā programma. Pievienoju arī attachmentā WinAVR projektu, kuru var kompilēt arī uz Linux (Makefile turpat galvenajā direktorijā).

Programmu var nosacīti dalīt tā:
1) dzelžu inicializācija - taimeri, porti, pwm, adc, uart
2) servisa funkcijas - eeprom, uart, pwm, lai nav jāraksta uzreiz reģistros, bet ērtāk saprast programmu
3) PID kontrolieris
4) enkodera signālu dekoderis uz pārtraukumiem
5) robota stūrēšana (sensoru aptaujāšana + loģika + PID)
6) kalibrācija (tomēr katrs sensors ir nedaudz atšķirīgs)
7) konfigurēšana un reporti (sazināšanās ar PC)
8 ) galvenā programmas cilpa un laika atskaite uz pārtraukuma (lai stabilāk)

Dažas vietas programmā var būt nekonsistentas - piemēram, kaut kāds komentārs, kas attiecas jau uz vecu versiju, un vairs nav spēkā esošs. Tas pats zināmā mērā attiecas uz stilu.



```
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <avr/io.h>
#include <string.h>
#include <util/delay.h>
#include <stdio.h>

/******** HW PART *********/

// speed 1 m/s = 100 cm/s
// 100 Hz = 10 mm
// 200 Hz = 5 mm (5 ms)
// 12 ADC measurements = ~1 ms

enum {
	TIMER0_CONST = 256-78,		// 400 Hz, 2,5 ms
	PWM_TOP = 400,          // 10 kHz PWM 
	DUTY_FULL = 100,
	DUTY_STOP = 0,
};

void hw_init(void);
void hw_init_start(void);
void hw_init_stop(void);

#define		set_bit(byte, bit)	byte |= (1 << bit)
#define		clear_bit(byte, bit)	byte &= ~(1 << bit)

#define		PIN_LEDS	PD2
#define		PIN_LED		PB0
#define		PIN_PWMA	PB1
#define		PIN_PWMB	PB2
#define		PIN_SENSFL	PC2
#define		PIN_SENSFR	PC3
#define		PIN_SENSL	PC4
#define		PIN_SENSR	PC5
#define		PIN_START	PD7
#define		PIN_DIRA	PD4
#define		PIN_DIRB	PD3

#define		PIN_MOSI	PB3
#define		PIN_MISO	PB4
#define		PIN_SCK		PB5
#define		PIN_RESET	PC6

#define ADDR_VFULL	6
#define ADDR_THR	8
#define ADDR_KP		10
#define ADDR_KD		12
#define ADDR_KI		14
#define ADDR_LIM	16
#define ADDR_SETP	18


void hw_init(void) {
	// configure PORTB (LED, PWM_A, PWM_B, MOSI, MISO, SCK) for output
	DDRB = _BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB3) | _BV(DDB5);

	// configure PORTD
	PORTD = _BV(PIN_START);        // key pullups
	DDRD = _BV(PIN_LEDS) | _BV(PIN_DIRA) | _BV(PIN_DIRB);
	// disable comparator
	ACSR |= _BV(ACD);

	// set system clock prescaler to 1
	CLKPR = _BV(CLKPCE);
	CLKPR = 0;

	UCSR0B = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0);
	UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);
	UBRR0 = 103;		// 4800 baud @ 8 MHz
}

void hw_init_start(void) {
	// enable ADC, prescaler /64 (8MHz > 125kHz)
	ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1);

	// enable Timer0, prescaler /256 (8MHz > 31,25kHz)
	TCNT0 = TIMER0_CONST;
	TCCR0A = 0;
	TCCR0B = _BV(CS02);

	// enable Timer0 overflow interrupt
   	TIMSK0 = _BV(TOIE0);

	// enable Timer2, prescaler /8 (8MHz > 1000 kHz)
//	TCNT2 = 0;
//	TCCR2A = 0;
//	TCCR2B = _BV(CS21);
	// enable Timer2 overflow interrupt
//   	TIMSK2 = _BV(TOIE2);

	// enable timer1, prescaler /1 (8MHz), freq / (2*PWM_TOP)
	OCR1A = 0;
	OCR1B = 0;
	ICR1 = PWM_TOP;
	TCCR1A = _BV(COM1A1) | _BV(COM1A0) | _BV(COM1B1) | _BV(COM1B0);    // set OC1A/OC1B on TOP, clear on match
//	TCCR1A = _BV(COM1A1) | _BV(COM1B1);    // set OC1A/OC1B on TOP, clear on match
	TCCR1B = _BV(CS10) | _BV(WGM13);

	// enable pin change interrupts 
//	PCICR = _BV(PCIE0) | _BV(PCIE2);
//	PCMSK0 = _BV(PCINT6) | _BV(PCINT7);
//	PCMSK2 = _BV(PCINT21) | _BV(PCINT22);

}

void hw_init_stop(void) {
	// stop ADC
	ADCSRA = 0;

	// stop timer1 (PWM)
	TCCR1A = 0;
	TCCR1B = 0;

	// stop timer2
	TCCR2A = 0;
	TCCR2B = 0;

	// disable timer interrupts
//	TIMSK2 = 0;
}

inline void uart_send(uint8_t data) {
	UDR0 = data;
	loop_until_bit_is_set(UCSR0A, UDRE0);
}

inline void uart_send_string(const char *string) {
	while (*string) {
		uart_send(*string);
		string++;
	}
}

inline void eeprom_writeb(uint16_t addr, uint8_t data) {
	loop_until_bit_is_clear(EECR, EEPE);
//	loop_until_bit_is_clear(SPMCSR, SELFPRGEN);
	EEAR = addr;
	EEDR = data;
	set_bit(EECR, EEMPE);
	set_bit(EECR, EEPE);
}

inline void eeprom_writew(uint16_t addr, uint16_t data) {
	eeprom_writeb(addr, data);
	eeprom_writeb(addr + 1, data >> 8);
}

inline uint8_t eeprom_readb(uint16_t addr) {
	loop_until_bit_is_clear(EECR, EEPE);
	EEAR = addr;
	set_bit(EECR, EERE);
	return EEDR;
}

inline uint16_t eeprom_readw(uint16_t addr) {
	return eeprom_readb(addr) | (((uint16_t)eeprom_readb(addr + 1)) << 8);
}

// reads ADC channel (adc_chan) as 10-bit value
inline uint16_t adc_read10bit(uint8_t adc_chan) {
//	uint8_t cnt;
//	uint16_t res = 0;

    ADMUX = adc_chan  | _BV(REFS0)| _BV(REFS1);
//	for (cnt = 0; cnt < 2; cnt++) {
    	ADCSRA |= _BV(ADSC);
    	loop_until_bit_is_clear(ADCSRA, ADSC);
//		res += ADCW;
//	}
	return ADCW;
}

void set_led(uint8_t state) {
	if (state) {
		PORTB |= _BV(PIN_LED);
	}
	else {
		PORTB &= ~_BV(PIN_LED);
	}
}


/******** MAIN PROGRAM *********/

#define STATE_STOP		0
#define STATE_READY		1
#define STATE_DRIVE		2

int state;

char recv_buf[32];
volatile uint8_t recv_size, recv_done;

volatile struct {
	uint16_t display;
	uint16_t control;
	uint16_t heart;
} time;

struct {
	int16_t pos_l, pos_r;
	uint16_t timer;
	uint16_t t1_l, t1_r;
	uint16_t t2_l, t2_r;
	uint8_t	state1, state2;
} enc;

struct {
	int16_t vfull;
	int8_t	ok;
	int16_t pos, floor, ceil, thr;
	int16_t sp;
	int16_t	vc, vd;
	int16_t	sens[6];
	uint8_t calib[6];
	int16_t	timeout;
} steer;

typedef struct {
	int16_t	pv0, pv1, pv2;
	int16_t	co;
	int16_t	limit;
	int16_t	kp, ki, kd;
} piddata_t;

piddata_t pid1;

int16_t pid_update(piddata_t *pd, int16_t pv, int16_t sp) {
	int16_t co = pd->co;
	int16_t e = sp - pv;

	pd->pv2 += e;
	pd->pv1 = pd->pv0;
	pd->pv0 = pv;

	co = (pd->kp * e) >> 4;
	co += (pd->ki * pd->pv2) >> 4;
	co += (pd->kd * (pd->pv1 - pd->pv0)) >> 4;

	if (pd->pv2 > pd->limit) pd->pv2 = pd->limit;
	if (pd->pv2 < -pd->limit) pd->pv2 = -pd->limit;
/*
	pd->pv2 = pd->pv1;
	pd->pv1 = pd->pv0;
	pd->pv0 = pv;

	co -= ((int32_t)pd->kp * (pd->pv0 - pd->pv1)) >> 8;
	co += ((int32_t)pd->ki * e) >> 8;
	co -= ((int32_t)pd->kd * (pd->pv0 - pd->pv1 - pd->pv1 + pd->pv2)) >> 8;
	if (co > pd->limit) co = pd->limit;
	if (co < -pd->limit) co = -pd->limit;
*/

	pd->co = co;
	return co;
}

void process_encoder() {
	uint8_t s1 = 0, s2 = 0;
	uint8_t d1 = PIND;
	uint8_t d2 = PINB;
	if (d1 & _BV(PD5)) s1 |= 1;
	if (d2 & _BV(PB6)) s1 ^= 3;
	if (d1 & _BV(PD6)) s2 |= 1;
	if (d2 & _BV(PB7)) s2 ^= 3;
	if (enc.state1 - s1 == 1 || s1 - enc.state1 == 3) {
		enc.pos_l++;
	}
	if (s1 - enc.state1 == 1 || enc.state1 - s1 == 3) {
		enc.pos_l--;
	}
	if (enc.state2 - s2 == 1 || s2 - enc.state2 == 3) {
		enc.pos_r++;
	}
	if (s2 - enc.state2 == 1 || enc.state2 - s2 == 3) {
		enc.pos_r--;
	}
	enc.state1 = s1;
	enc.state2 = s2;
}

void set_speed(int16_t cc, int16_t cd) {
	cd = -cd;
	if (cc == 0) {
	    set_bit(PORTD, PIN_DIRA);
		OCR1A = 0;
	}
	else if (cc < 0) {
		clear_bit(PORTD, PIN_DIRA);
		OCR1A = PWM_TOP + cc;
//		OCR1A = -cc;
	}
	else {
		set_bit(PORTD, PIN_DIRA);
		OCR1A = cc;
//		OCR1A = PWM_TOP - cc;
	}
	if (cd == 0) {
		set_bit(PORTD, PIN_DIRB);
		OCR1B = 0;
	}
	else if (cd < 0) {
		clear_bit(PORTD, PIN_DIRB);
		OCR1B = PWM_TOP + cd;
//		OCR1B = -cd;
	}
	else {
		set_bit(PORTD, PIN_DIRB);
		OCR1B = cd;
//		OCR1B = PWM_TOP - cd;
	}
}

int16_t div16(int16_t a, int16_t b) {
	int16_t c = 0;
	while (a > 0) {
		a -= b;
		c++;
	}
	return c;
}

void control(void) {
	uint8_t		i, j, ord[8];

	// nolasīt ātrumu
	//int16_t vc = enc.pos_l << 8;
	//int16_t vd = enc.pos_r << 8;
	//enc.pos_l = 0;
	//enc.pos_r = 0;

	// ieslēgt gaismas, nomērīt līniju
	set_bit(PORTD, PIN_LEDS);
	_delay_us(100);
	for (i = 0; i < 6; i++) steer.sens[i] = adc_read10bit(i);
	clear_bit(PORTD, PIN_LEDS);
	_delay_us(100);
	for (i = 0; i < 6; i++) {
		steer.sens[i] -= adc_read10bit(i);
		if (steer.sens[i] < 0) steer.sens[i] = 0;
		else steer.sens[i] = ((int16_t)steer.calib[i] * steer.sens[i]) >> 5;
	}
	for (i = 0; i < 6; i++) ord[i] = i;
	for (i = 0; i < 6; i++) {
		for (j = i + 1; j < 6; j++) {
			if (steer.sens[ord[i]] > steer.sens[ord[j]]) {
				uint8_t t = ord[i];
				ord[i] = ord[j];
				ord[j] = t;
			}
		}
	}
	int16_t floor = (steer.sens[ord[5]] + steer.sens[ord[4]]) / 2;
	steer.floor = floor;
	int16_t m0 = floor - steer.sens[ord[0]];
	int16_t m1 = floor - steer.sens[ord[1]];
	steer.ceil = m0;
	if (m0 < steer.thr) {
		steer.ok = 0;
	}
//	else if (ord[1] != ord[0] - 1 && ord[1] != ord[0] + 1) {
//		steer.ok = 0;
//	}
//	else if (ord[0] > 0 && ord[0] < 5 && ord[2] != ord[0] - 1 && ord[2] != ord[0] + 1) {
//		steer.ok = 0;
//	}
	else {
		steer.ok = 1;

		int16_t m2 = floor - steer.sens[ord[2]];

		int16_t c = div16((m0 - m1) << 5, (m0 - m2));
		if (ord[1] > ord[0]) c = 64 - c;
		steer.pos = (ord[0] << 6) + c - (3 << 6);
	}
	//steer.pos <<= 4; // -12.0 ... +12.0

	int16_t act = pid_update(&pid1, steer.pos, steer.sp);

	// izrēķināt mērķa vc, vd
	int16_t vlt = steer.vfull - act;
	int16_t vrt = steer.vfull + act;
	if (vlt > PWM_TOP) vlt = PWM_TOP;
	if (vlt < -PWM_TOP) vlt = -PWM_TOP;
//	if (vlt < 0) vlt = 0;
	if (vrt > PWM_TOP) vrt = PWM_TOP;
	if (vrt < -PWM_TOP) vrt = -PWM_TOP;
//	if (vrt < 0) vrt = 0;

	if (steer.ok) {
		steer.vc = vlt;
		steer.vd = vrt;
		steer.timeout = 0;
	}
	else {
		if (steer.timeout < 100) {
			steer.timeout++;
			steer.vc = steer.vfull;
			steer.vd = steer.vfull;
		}
		else {
			steer.vc = steer.vd = 0;
		}
	}

	if (state == STATE_DRIVE) set_speed(steer.vc, steer.vd);
}

void display(void) {
//	if (state == STATE_READY) {

	char buf[32];
	sprintf(buf, "P%d I%d D%d\n", pid1.kp, pid1.ki, pid1.kd);
	uart_send_string(buf);
	if (!steer.ok) {
		sprintf(buf, "T[---]\n");
		uart_send_string(buf);
		return;
	}
	sprintf(buf, "T[%d] [%d] [%d]\n", steer.pos, steer.floor, steer.ceil);
//	sprintf(buf, "G0A\r\nT%d %d %d \r\n", steer.sens[0], steer.sens[1], steer.sens[2]);
//	sprintf(buf, "G0A\r\nT%2d %2d %2d \r\n", steer.calib[0], steer.calib[1], steer.calib[2]);
//	sprintf(buf, "G0A\r\nT%d %d \r\n", enc.vc, enc.vd);
	uart_send_string(buf);
//	sprintf(buf, "G1A\r\nT%d %d %d \r\n", steer.sens[steer.min1], steer.sens[steer.min2], steer.sens[steer.min3]);
//	sprintf(buf, "G1A\r\nT%d %d \r\n", steer.sens[steer.min_i], steer.sens[steer.max_i]);
//	sprintf(buf, "G1A\r\nT%d %d %d \r\n", steer.sens[3], steer.sens[4], steer.sens[5]);
//	sprintf(buf, "G1A\r\nT%2d %2d %2d \r\n", steer.calib[3], steer.calib[4], steer.calib[5]);
//			sprintf(buf, "G1A\r\nT%2d %2d %2d \r\n", calib[5] / cal_n, calib[3] / cal_n, calib[4] / cal_n);
//	uart_send_string(buf);
//	}
}

void calibrate(void) {
	uint8_t i;
	char buf[32];

	// ieslēdzam gaismas
	set_bit(PORTD, PIN_LEDS);
	_delay_us(100);
	for (i = 0; i < 6; i++) steer.sens[i] = adc_read10bit(i);
	// izslēdzam gaismas
	clear_bit(PORTD, PIN_LEDS);
	_delay_us(100);
	for (i = 0; i < 6; i++) {
		steer.sens[i] -= adc_read10bit(i);
		if (steer.sens[i] < 0) steer.sens[i] = 0;
	}
	// rēķinām dalījumu
	for (i = 0; i < 6; i++) {
		if (steer.sens[i] == 0) continue;
		steer.calib[i] = div16(24*1024, steer.sens[i]);
		eeprom_writeb(i, steer.calib[i]);
	}

	sprintf(buf, "T%2d %2d %2d \n", steer.calib[0], steer.calib[1], steer.calib[2]);
	uart_send_string(buf);
	sprintf(buf, "T%2d %2d %2d \n", steer.calib[3], steer.calib[4], steer.calib[5]);
	uart_send_string(buf);
}

void start_drive(void) {
	steer.vc = steer.vd = 0;
	steer.sp = 0;
	pid1.co = 0;
	pid1.pv0 = pid1.pv1 = pid1.pv2 = 0;
}

void stop_drive(void) {
	set_speed(0, 0);
}

void button_pushed(void) {
	switch (state) {
	case STATE_STOP:
//		calibrate();
//		state = STATE_READY;
//		break;
//	case STATE_READY:
		start_drive();
		state = STATE_DRIVE;
		break;
	case STATE_DRIVE:
		stop_drive();
		state = STATE_STOP;
		break;
	}
}

void init_pid() {
	uint8_t i;

	pid1.co = 0;
	pid1.pv0 = pid1.pv1 = pid1.pv2 = 0;
/*
	pid1.limit = 10000;
	pid1.kp = -50;
	pid1.kd = -2500;
	pid1.ki = 0;
	calibrate();
	steer.vfull = 0;
	steer.thr = 100;
	steer.sp = 0;
*/
	pid1.limit = eeprom_readw(ADDR_LIM);
	pid1.kp = eeprom_readw(ADDR_KP);
	pid1.kd = eeprom_readw(ADDR_KD);
	pid1.ki = eeprom_readw(ADDR_KI);

	for (i = 0; i < 6; i++) {
		steer.calib[i] = eeprom_readb(i);
	}

	steer.vfull = eeprom_readw(ADDR_VFULL);
	steer.thr = eeprom_readw(ADDR_THR);
	steer.sp = eeprom_readw(ADDR_SETP);
}

void menu_cmd(const char *cmd) {
	if (cmd[0] == 0) return;
	char type = cmd[0];
	uint16_t param;

	switch (type) {
	case 'P':
	case 'I':
	case 'D':
	case 'V':
	case 'S': 
	case 'L': 
	case 'T': 
		sscanf(cmd + 1, "%d", &param);
		break;
	case 'C':
//		uart_send_string("abc");
//		uart_send('\r');
//		uart_send('\n');
		calibrate();
		break;
	case 'W':
		display();
		break;
	default:
		return;
	}
	switch (type) {
	case 'P': pid1.kp = param; eeprom_writew(ADDR_KP, param); break;
	case 'I': pid1.ki = param; eeprom_writew(ADDR_KI, param);break;
	case 'D': pid1.kd = param; eeprom_writew(ADDR_KD, param);break;
	case 'V': steer.vfull = param; eeprom_writew(ADDR_VFULL, param);break;
	case 'S': steer.sp = param; eeprom_writew(ADDR_SETP, param); break;
	case 'L': pid1.limit = param; eeprom_writew(ADDR_LIM, param); break;
	case 'T': steer.thr = param; eeprom_writew(ADDR_THR, param); break;
	}
	uart_send_string(cmd);
//	uart_send('\r');
	uart_send('\n');
}

int main(void) {
	uint8_t idx;

	recv_size = 0;
	recv_done = 0;

	enc.pos_l = 0;
	enc.pos_r = 0;

	hw_init();
	sei();
	
	set_speed(0, 0);
	init_pid();

	state = STATE_STOP;
	hw_init_start();

	for (idx = 0; idx < 50; idx++) _delay_ms(10);
	set_led(1);

	while (1) {
		sleep_mode();
		if (recv_done > 0) {
			menu_cmd(recv_buf);
			recv_done = 0;
		}
		if (bit_is_clear(PIND, PIN_START)) {
			button_pushed();
			for (idx = 0; idx < 100; idx++) _delay_ms(10);
		}
		if (time.control == 0) {
			time.control = 1;
			control();
		}
		if (time.display == 0) {
			time.display = 400;
//			display();
		}
	}

	return 0;
}

SIGNAL(SIG_PIN_CHANGE0) {
	process_encoder();
}

SIGNAL(SIG_PIN_CHANGE2) {
	process_encoder();
}

// ~1000x sekundee
ISR(TIMER1_OVF_vect) {
//	enc.timer += 256;
}

// ~400x sekundee
ISR(TIMER0_OVF_vect) {
	TCNT0 += TIMER0_CONST;

	// nodrošinām tikai laika skaitīšanu un heartbeat
	if (time.heart == 40) set_led(1);
	if (time.heart > 0) time.heart--;
	else {
		set_led(0); 
		if (state == STATE_READY) time.heart = 200;
		else if (state == STATE_STOP) time.heart = 400;
		else time.heart = 100;
	}
	if (time.display > 0) time.display--;
	if (time.control > 0) time.control--;
}

ISR(__vector_default) {
	// do nothing
}

SIGNAL(SIG_USART_RECV) {
	uint8_t data = UDR0;
//	if (recv_done == 1) return;
	if (data == 0x0D) {
		recv_buf[recv_size] = 0;
		recv_done = 1;
		recv_size = 0;
	}
	else if (data == 0x0A) {
	}
	else if (recv_size < 32) {
		recv_done = 0;
		recv_buf[recv_size] = data;
		recv_size++;
	}
}
```

----------


## karloslv

Papildinājums: guvu vakar mācību. Steigā gatavojot robotu Ventspils braucienam, pēkšņi viens motors pārstāja strādāt. Tā ir ar to forsēšanu - attaisīju vaļā, slotiņa jau pavisam noļurkāta. Padomāju, ka pilnīgi lieki būšu to darījis - motors labi strādā arī no 3V, un tad pietiktu ar pārnesumu 1:12,7. Tad gan ir vai nu jābaro ar <50% PWM no diviem litija elementiem, vai nu jādabū lauktranzistoru H-tilts, uz kura nav tik liela sprieguma krituma (savukārt tiem parasti nepieciešama 10-15V barošana).

----------


## Epis

karlosLV tev šitam kodam ir kautkāds programmas flowcharts (shēma), kas vairāk atspoguļotu programmas darbību un plūsmu nelienot iekšā tajā C kodā ? 
jo kods ir tāds pagarš 
+ es īsti vēl nēsu nevienu mikreni ar C nopietni programmējis tākā man ir pagrūti to kodu saprast jo ir atšķirības kodēšanā starp objekt orjentēto C# un šito embaded C valodām.

----------


## karloslv

Hm, tādas gan man nav. Būs Tev vai nu jāmeklē kāds rīks, vai nu jābrauc iekšā. Protams, svešā kodā vienmēr ir grūti iebraukt. Reizēm pat pašam savējā, tāpēc baigi svarīgi ir pieturēties pie kaut kāda kodēšanas standarta. Man tas attīstās un veidojas, un programmās reizēm var redzēt vecu stilu piejaukumus.

----------


## Epis

a šitas kods uz IAR progas kompilēsies? 
pamazām sāku to IAR progu apgūt jo tas WinAVR man neiet un tad šitas sanāk vienīgais C compileris kurš man tagat uzinstalēts priekš AVR ir un labi arī tas kad IAR atbalsta arī citus procesorus kā ARM7, līdz ar to nevaidzēs meklēt, apgūt jaunu compileri.
un es lasot to IAR progas manuālus atceros kad tur bīj kautkādi shematiskie kodu attēlotāji, kas attēlo to programmas plūsmu, vienīgi vai viņi paši gēnerējās vai bīj jātaisa to es nezinu, tādos shematiskos attēlos smuki vare redzēt kā uzbūvēta programma  :: 
Zinu kad fpga ir shematiskie State machine vizualizētāji un ar viem navarotiem + loģikas shēmu līmeņa attēli, kas ģenerējās no loģikas koda, principā tur var redzēt kāda loģika no koda ir sanākusi.

----------


## karloslv

Ko nozīmē WinAVR neiet? Viņš iet visur. Atpako un lieto. Uzinstalē WinAVR, AvrStudio4 un viss notiek. Es to kodu arī uz Linux esmu kompilējis ar avr-gcc.

Uz IAR ir savas atšķirības. Pats neesmu viņu izmantojis, taču esmu redzējis kodu, kurš ir paredzēts gan GCC, gan IAR. Atšķiras include failu nosaukumi un, šķiet, neiet cauri piešķiršana reģistriem (piemēram, TIMSK |= (1 << TOIE1)), bet jāraksta ar outb(). Bet pārliecināts par to neesmu. Internetā noteikti jābūt informācijai par to.

----------


## karloslv

Man tā programma nav tik sarežģīta, kā liekas. Visas funkcijas, kas ir sākumā (vairāk par pusi no visa faila, man šķiet) ir tikai palīgfunkcijas - respektīvi, ērti saīsinājumi, lai kods saprotamāks. Ja visu sarakstītu vienā blāķī, nekas garš tur nesanāktu.

----------


## Epis

agrāk bīju uztaisījis topiku kur aprakstīju kā instalēju un kas negāja "Par Avr C programmēsanu un WinAVR?"
viewtopic.php?f=24&t=1425
tā arī tas win AVR negāja un līdz šim arī neiet tādēl vienīgais kas iet ir 8K limitētais IAR.

----------


## Zalic

Varbūt kāds var palīdzēt, ir viss nepieciešamais, bet shēmā nav paredzēt slēgt pei programatora ar 2x5 Pin, bet kaut kādu citu kabeli, līdz ar to, varbūt kāds var palīdzēt un pastāstīt, ka pieslēgt šo 2x5 PIN štekeri?

----------


## kaspich

nu to piesleegumu tak skaties peec mcu un programeetajaa datasheet. ja nevari savilkt kopaa vadus ar vienaadiem nosaukumiem, pak^*%* buus.

es gan teiktu, ka iista beedu ieleja ir ar 'obvesu'.
1. nje pojnaatno, kaapeec ledi ir paraleeli un taadu R sleegumu;
2. L298 shaadaa sleegumaa [izskataas, ka paredzeeti augstomiigi stepperi, vadiiba - shausmiigi tupa, bez I kontroles, kaut minisoljiem, jeb arii DC motorelji] - ar lieliem zudumiem  [taa tak ir augstiem U paredzeeta, uz bipolaarajiem]..


to optronu dalju es taisiitu.. nu, ok, iipashi nesarezghihot - paaris diodes, 10 pretestiibas, un buutu 3 liimenju jutiiba [lai straadaa arii spilgtaakaa apgaismojumaa, u.t.t.]

----------


## Zalic

aaa, skaidrs, laikam domu sapratu. tākā ar mikrokontrolioeri kramejos pirmo reizi, tad tā ir kā ir, bet nu laikam viss sanāks

----------


## JDat

Optronu traņi tak katrs pie sava ADC pieslēgti. Pilnīgs programmeru variants. Tur ir vairāk par 3 līmeņiem.  :: 

Lai saprogrammetu MCU, jāpieslēdas pie ISP1 un ISP2. Kā tieši? Tā kā datu lapā rakstīts.

----------


## Zalic

UART jeb J3 vai tad arī nav vajadzīgs? domāju ka visi J1, J2 un J3 ir pareizi jāsavieno ar manu manu programatoru?

----------


## JDat

UART nevajag pie programmatora. Vienīgi kāskatās kodā, kāpēc robotam vispār ir UART. Debug lietām noteikti.

----------


## Zalic

vai attēlā redzmajiem vadu spraudņiem un ligzdām arī ir kādi nosaukumi speciāli?

----------


## JDat

> vai attēlā redzmajiem vadu spraudņiem un ligzdām arī ir kādi nosaukumi speciāli?


 Ko? Kādi speciāli nosaukumi.

----------


## Andrejs

nu uzcēluši vecu tēmu...
 vecie zēni: film neviģel - mņeņija viskažu!
punkts 1. URMAS konkrētajaā sacenē vinnēja!
2.  tie nav nekādi optroni, bet atstarotās gaismas dačiki.
3.l298 varbūt nav pats labākais pasaulē h-tilts, bet savu darbu izdarja ļoti labi. un kāda xy pēc tur stepperi? Parasti motori!

----------


## JDat

nuu, ne optroni, bet nu tie... fricken sūdi kur vienā korpusā (IR) gaismas diode un fototranzistors. Karoče optopāris, kas nostrādā kad gaisma atstarojas no priekšmeta, kurs piestumts klāt. Šodien vairs nav enerģija sekot līdzi terminoloģijai.

----------


## Zalic

> vai attēlā redzmajiem vadu spraudņiem un ligzdām arī ir kādi nosaukumi speciāli?
> 
> 
>  Ko? Kādi speciāli nosaukumi.


 nu ka RCN un DIN utt, lai veikalā zin ko prasīt un kā netā meklēt

----------


## kaspich

jdat - ADC ir labi [labaak], bet iipashi plash dinamiskais diapazons tur nebuus  :: 
hvz, es tur redzu vaajo vietu  :: 

[vismaz 2 liimenjus toch taisiitu, 1/10..1/30 peec I]

taas l298 vietaa: 
http://www.fairchildsemi.com/ds/FD/FDS9934C.pdf

reku Ron=0.04ohm, I>5A, mazs korpuss, nekas nekarst, ceeeena - leetaak [4gab] kaa l298..
zudumi nesaliidzinaami mazaaki, motori grieziisies njipraak..

man skjiet, ka par shiim lietama ir/buutu jaapadomaa, pirms sak taadu projektu. peec tam/velaak visu paartaisiit..gemors..

----------


## JDat

> jdat - ADC ir labi [labaak], bet iipashi plash dinamiskais diapazons tur nebuus 
> hvz, es tur redzu vaajo vietu


 hz. Režimi nav pareizi? Nafig!. Negribu vairs domāt un rēķināt.  ::

----------


## kaspich

> jdat - ADC ir labi [labaak], bet iipashi plash dinamiskais diapazons tur nebuus 
> hvz, es tur redzu vaajo vietu 
> 
> 
>  hz. Režimi nav pareizi? Nafig!. Negribu vairs domāt un rēķināt.


 nee, es sagaidu, ka dazhaados apstaakljos gaismas pluusma uz fototranjiem var atskjirties kaadas 1000X vismaz [piem., ne iipashi aktiiva liinija tumsaa/atstarojosha materiaala lenta pat ne saules gaismaa, bet vnk gaishaa telpaa].
liidz ar to - lai kaa vinjus reguleetu [fikseets reguleejums, te tadas iespeejas nav] - vienaa gadiijumaa - pamatiigs piesaatinaajums, otraa- dazhi/dazhi desmiti mV, ko ADC korekti nesameeriis.

jaanjem veera, ka shadiem iebuuveetajiem ADC [pienjemsim, 10bitu]:
+-10..15mV ir offset [ieskaitot sheemu, nopluudes, sampling kljudas], kas no 4.096v references.. ir daudz. reaali izmantojami ir kaadi 8biti, da i pie maza liimenja [liidz paarsimts mV] tur gljuki/troksnji vien buus..

----------


## JDat

Paga, tu tak neizini ka koderi (tie kas taisa web lapas un VID datu bāzes) būvē robotus. Domāju ka šis ir viens no tiem gadījumiem...  :: 

Tā teikt: this is hardware problem, we solve it in software!

----------


## karloslv

> Paga, tu tak neizini ka koderi (tie kas taisa web lapas un VID datu bāzes) būvē robotus. Domāju ka šis ir viens no tiem gadījumiem... 
> 
> Tā teikt: this is hardware problem, we solve it in software!


 Iztiksim bez nepamatotiem uzbraucieniem un nekonstruktīvām replikām. Ar dzelžiem darbojos tikpat sen kā ar izstrādi, fiziku un matemātiku. Ņemiet arī vērā, ka šis tika būvēts pasen un dažas lietas tagad darītu citādi.

6 ADC līnijas aptaujāt šajā gadījumā bija vieglāk nekā komutēt tos ārpus uC. 

Par piesātinājumu - tak raksturlīkne ir nelineāra, līdz ar to dinamika krietni plašāka nekā lineārā gadījumā. Par 8 bitiem piekrītu, taču programma neiespringa par visiem 10 bitiem. 

Realitātē strādāja ļoti labi un ar pareiziem PID parametriem pielipa pie līnijas. Traucēja luftes mehānikā, neoptimāls svara izvietojums un viss kas cits, kas ar slinkumu netika uzlabots.

----------


## karloslv

[dupe]

----------


## Vikings

Bet cilvēki mīļie! Jautājums bija par ISP štekeri. Konkrētajā shēmā norādīti divi štekeri ISP1 un ISP2. Izmantojot pie šiem štekeriem esošos vadu apzīmējumus, to vietā vajag pieslēgt zemākajā attēlā redzamo 10 pinu štekeri.

----------


## Zalic

paldies par atbildi un zimejumu

----------


## kaspich

torni - es tikai peec saviem postiem izlasiiju Tavu aprakstu. liidz ar to - ar skumjaam naacaas atziit, ka daudz ko jau pats piemineeji  :: 

nu, Tu ruupiigi no gaismas ekraneeji fototranjus, es uz to neiespringtu - taisiitu, lai straadaa max plashaa gaishuma diapazonaa. tad arii lielaaka droshiibas sajuta par temu - lai kada buutu virsma [spiidiiga, vai mateeta, lenta - atstarojosha vai tieshi otraadi, absorbeejosha], lenta - viss po.


zin, ko es savam robotam taisiitu?
2 rindas ar fotodiodeem. pirmo [jauno] izbiidiitu uz priekshu par kaadiem 5cm.
taa es redzeetu ne tikai tekosho situaaciju, bet arii naakotni/virzienu. pagriezienos vareetu drifteejot iet iekshaa  :: 

un fototranju skaitu taisiitu nepaara. lai ir orientaacija uz centraalo..

p.s. par to komutaaciju - es biju domaajis, ka tiek komuteeti tranju slodzes R. respektiivi - spilgtaakaa apgaismojumaa paarsleedzas 33/3k, piemeram.
nu, 7 pretestiibu vietaa var izmantot 3, lediem polaritaate otraadi, bet - tie ir siikumi [sheemaa], driizaak kaa tests idejas kopeetaajiem  :: 


p.s. peec datasheet man raada baigi smuki lineaaro attieciibu Ee/Ic..

----------


## JDat

> zin, ko es savam robotam taisiitu?
> 2 rindas ar fotodiodeem. pirmo [jauno] izbiidiitu uz priekshu par kaadiem 5cm.
> taa es redzeetu ne tikai tekosho situaaciju, bet arii naakotni/virzienu. pagriezienos vareetu drifteejot iet iekshaa


 Ko tik vāji? Tikai 2 rindas...
Man kā minimālistam pietiek ar vienu rindu: http://elm-chan.org/works/ltc/report.html
Tur arī video...

Maksimālistiem ir cita alternatīva: videokamera
http://elfa.lv/forum/viewtopic.php?f=28&t=1221
 :: 
Esmu tālāk ticis par kaspichu robotu būvēšanā. Man ir riteņi un tagad štukoju kā uzlikt uz šasijas.
Esošais līnijsekotāja būvešanas temps ir ļoti straujš: apmēram 1 izurbts caurums reizi 2 mēnešos. Domāju ka tāds temps ir ļoti straujš. Līnijsekotājs varētu būt gatavs apmēram uz pesionēšanās vecumu. Tobiš, kad man būs 97 gadi un iešu pensijā.  ::

----------

