WebSVN – svnkaklik – Blame – /roboti/istrobot/3Orbis/bak/06 PD Regulator/master/main.c

Rev	Author	Line No.	Line
2	kaklik	1	`#include ".\main.h"`
		2
		3	`#define KOLMO1 225 // predni kolecko sroubem dopredu`
		4	`#define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu`
		5	`#define STRED 128 // sredni poloha zataceciho kolecka`
		6	`#define BEAR1 6//10 // 3 stupne zataceni`
		7	`#define BEAR2 10//27`
		8	`#define BEAR3 40`
		9	`#define SPEEDMAX 160 // ANSMANN=140; GP=120; maximalni rozumna rychlost`
		10	`#define R17 255 // X nasobek rozumne rychlosti`
		11	`#define DOZNIVANI 10`
		12	`#define L 1 // cara vlevo`
		13	`#define S 2 // casa mezi sensory`
		14	`#define R 0 // cara vpravo`
		15
		16	`// servo`
		17	`#define SERVO PIN_A2`
		18
		19	`// IR`
		20	`#define IRTX PIN_B2`
		21	`#define CIHLA PIN_A3`
		22
		23	`//motory`
		24	`#define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred`
		25	`#define FL output_low(PIN_A1); output_high(PIN_A0)`
		26	`#define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad`
		27	`#define BL output_low(PIN_A0); output_high(PIN_A1)`
		28	`#define STOPR output_low(PIN_A6);output_low(PIN_A7)`
		29	`#define STOPL output_low(PIN_A0);output_low(PIN_A1)`
		30
		31	`//HID`
		32	`#define LED1 PIN_B1 //oranzova`
		33	`#define LED2 PIN_B2 //zluta`
		34
		35	`#define STROBE PIN_B0`
		36	`//#define SW1 PIN_A2 // Motory On/off`
		37
		38	`unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru`
		39	`unsigned int8 line; // na ktere strane byla detekovana cara`
		40	`//unsigned int8 dira; // pocita dobu po kterou je ztracena cara`
		41	`unsigned int8 uhel; // urcuje aktualni uhel zataceni`
		42	`unsigned int8 speed; // maximalni povolena rychlost`
		43	`unsigned int8 rovinka; // pocitadlo na zjisteni rovinky`
		44
		45	`short int preteceni; // flag preteceni timeru1`
		46
		47	`signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem`
		48	`signed int16 Rmotor; // a pravem motoru`
		49
		50	`// makro pro PWM pro motory`
		51	`#define GO(motor, direction, power) if(get_timer0()<=power) \`
		52	`{direction##motor;} else {stop##motor;}`
		53
		54	`////////////////////////////////////////////////////////////////////////////////`
		55	`/*`
		56	`void diagnostika()`
		57	`{`
		58	`if(input(SW1))STOPR;STOPL;While(TRUE);`
		59	`// if(LSENSOR==true) output_high(LED2); else output_low(LED2);`
		60	`// if(RSENSOR==true) output_high(LED1); else output_low(LED1);`
		61	`}`
		62	`*/`
		63	`////////////////////////////////////////////////////////////////////////////////`
		64	`#int_TIMER1`
		65	`TIMER1_isr()`
		66	`{`
		67	`preteceni = true;`
		68	`}`
		69	`////////////////////////////////////////////////////////////////////////////////`
		70	`#int_TIMER2`
		71	`TIMER2_isr() // ovladani serva`
		72	`{`
		73	`unsigned int8 n;`
		74
		75	`output_high(SERVO);`
		76	`delay_us(1000);`
		77	`for(n=uhel; n>0; n--) Delay_us(2);`
		78	`output_low(SERVO);`
		79	`}`
		80
		81	`////////////////////////////////////////////////////////////////////////////////`
		82	`short int IRcheck() // potvrdi detekci cihly`
		83	`{`
		84	`output_high(IRTX); // vypne vysilac IR`
		85	`delay_ms(100);`
		86
		87	`output_low(STROBE);`
		88	`sensors = spi_read(0); // cteni senzoru`
		89	`sensors=~sensors;`
		90	`output_high(STROBE);`
		91
		92	`if(true==bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal`
		93	`{`
		94	`output_low(IRTX); // zapne vysilac IR`
		95	`delay_ms(100);`
		96
		97	`output_low(STROBE);`
		98	`sensors = spi_read(0); // cteni senzoru`
		99	`sensors=~sensors;`
		100	`output_high(STROBE);`
		101
		102	`if(false==bit_test(sensors,7)) // otestuje, jestli je detekovana cihla`
		103	`{`
		104	`output_high(IRTX); // vypne vysilac IR`
		105	`delay_ms(100);`
		106
		107	`output_low(STROBE);`
		108	`sensors = spi_read(0); // cteni senzoru`
		109	`sensors=~sensors;`
		110	`output_high(STROBE);`
		111
		112	`output_low(IRTX); // zapne vysilac IR`
		113	`if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla`
		114	`}`
		115	`};`
		116	`output_low(IRTX); // zapne vysilac IR`
		117	`return 0; // vrat 0, kdyz je detekovano ruseni`
		118	`}`
		119	`////////////////////////////////////////////////////////////////////////////////`
		120	`void objizdka()`
		121	`{`
		122	`int8 shure=0;`
		123	`unsigned int16 n;`
		124
		125	`BR;BL;`
		126	`Delay_ms(400);`
		127	`STOPR;STOPL;`
		128
		129	`uhel=KOLMO1; // nastav zataceci kolecko kolmo na osu robota`
		130	`Delay_ms(100);`
		131	`BL;FR;`
		132	`Delay_ms(200); // minimalni toceni, kdyby se zastavilo sikmo k cihle`
		133
		134	`While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru`
		135	`{`
		136	`sensors = spi_read(0); // cteni senzoru`
		137	`sensors=~sensors;`
		138	`Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel`
		139	`}`
		140	`STOPL; STOPR;`
		141
		142	`for (n=0;n<1500;n++) // vystred se na hranu cihly`
		143	`{`
		144	`if(!input(CIHLA)) {BL; FR;} else {FL; BR;};`
		145	`delay_ms(1);`
		146	`}`
		147	`STOPR;STOPL;`
		148
		149	`uhel=STRED; // dopredu`
		150	`delay_ms(100);`
		151	`FR; FL;`
		152	`delay_ms(500);`
		153	`BL;BR;`
		154	`delay_ms(200);`
		155	`STOPL;STOPR;`
		156
		157	`uhel=STRED+BEAR3; // doprava`
		158	`delay_ms(100);`
		159	`FL;`
		160	`delay_ms(200);`
		161	`uhel=STRED+BEAR2; // min doprava`
		162	`FL;FR;`
		163	`delay_ms(200);`
		164	`uhel=STRED+BEAR1; // jeste min doprava`
		165	`FL;FR;`
		166	`delay_ms(200);`
		167	`uhel=STRED; // rovne`
		168	`FL;FR;`
		169	`delay_ms(100);`
		170	`While((sensors & 0b01111111)!=0) //dokud neni cara`
		171	`{`
		172	`sensors = spi_read(0); // cteni senzoru`
		173	`sensors=~sensors;`
		174	`Delay_ms(4); // cekani na SLAVE nez pripravi data od cidel`
		175	`}`
		176	`BL; BR;`
		177	`delay_ms(400);`
		178
		179	`uhel=STRED-BEAR3; // doleva`
		180	`}`
		181
		182	`////////////////////////////////////////////////////////////////////////////////`
		183	`void main()`
		184	`{`
		185
		186	`unsigned int8 n;`
		187	`unsigned int8 i,v;`
		188	`unsigned int8 last_sensors;`
		189	`unsigned int8 j=0;`
		190
		191	`setup_adc_ports(NO_ANALOGS);`
		192	`setup_adc(ADC_CLOCK_INTERNAL);`
		193	`setup_spi(SPI_MASTER\|SPI_H_TO_L\|SPI_XMIT_L_TO_H\|SPI_CLK_DIV_16);`
		194	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1);`
		195	`setup_timer_1(T1_INTERNAL\|T1_DIV_BY_8);`
		196	`setup_timer_2(T2_DIV_BY_16,140,16);`
		197	`setup_oscillator(OSC_8MHZ\|OSC_INTRC);`
		198
		199	`STOPR; STOPL; // zastav motory`
		200	`Lmotor=0;Rmotor=0;`
		201
		202	`uhel = STRED; // nastav zadni kolecko na stred`
		203	`rovinka = 0;`
		204
		205	`enable_interrupts(INT_TIMER2);`
		206	`enable_interrupts(INT_TIMER1);`
		207	`enable_interrupts(GLOBAL);`
		208
		209	`output_low(IRTX); // zapni IR vysilac`
		210
		211	`delay_ms(1000);`
		212
		213	`//!!!!`
		214	`speed=SPEEDMAX;`
		215
		216	`while(true)`
		217	`{`
		218
		219	`GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor`
		220
		221	`delay_us(1500); // cekani na SLAVE, nez pripravi data od cidel`
		222
		223	`last_sensors = sensors;`
		224	`j++;`
		225
		226	`output_low(STROBE); // vypni zobrazovani na posuvnem registru`
		227	`sensors = spi_read(0); // cteni senzoru`
		228	`sensors=~sensors;`
		229	`output_high(STROBE); // zobraz data na posuvnem registru`
		230
		231
		232
		233
		234	`if(false == preteceni)`
		235	`{`
		236	`v = 255 - (get_timer1() >> 8);`
		237	`v >>= 3;`
		238	`}`
		239	`else`
		240	`{`
		241	`v=0;`
		242	`preteceni=false;`
		243	`}`
		244
		245
		246
		247	`i=0; // havarijni kod`
		248	`for (n=0; n<=6; n++)`
		249	`{`
		250	`if(bit_test(sensors,n)) i++;`
		251	`}`
		252	`if (i>3) While(true){STOPR; STOPL;}; // zastavi, kdyz je cerno pod vice nez tremi cidly`
		253
		254
		255
		256	`if(bit_test(sensors,7)) // detekce cihly`
		257	`{`
		258	`//!!! objizdka(); // objede cihlu`
		259	`}`
		260
		261
		262
		263	`if(bit_test(sensors,3)) //...\|...//`
		264	`{`
		265	`uhel=STRED;`
		266	`Lmotor=speed;`
		267	`Rmotor=speed;`
		268	`line=S;`
		269	`set_timer1(0);`
		270	`if (rovinka < 255) rovinka++;`
		271	`// if (speed > SPEEDMAX) speed--;`
		272	`continue;`
		273	`}`
		274
		275	`if(bit_test(sensors,0)) //\|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu`
		276	`{`
		277	`if(sensors != last_sensors) uhel=STRED - BEAR3 - v;`
		278	`if(j>=DOZNIVANI)`
		279	`{`
		280	`j=0;`
		281	`if(uhel<STRED - BEAR3) uhel++;`
		282	`}`
		283	`Lmotor=0;`
		284	`Rmotor=speed;`
		285	`line=L;`
		286	`set_timer1(0);`
		287	`// if (speed > SPEEDMAX) speed--;`
		288	`continue;`
		289	`}`
		290
		291	`if(bit_test(sensors,6)) //......\|//`
		292	`{`
		293	`if(sensors != last_sensors) uhel=STRED + BEAR3 + v;`
		294	`if(j>=DOZNIVANI)`
		295	`{`
		296	`j=0;`
		297	`if(uhel>STRED + BEAR3) uhel--;`
		298	`}`
		299	`Rmotor=0;`
		300	`Lmotor=speed;`
		301	`line=R;`
		302	`set_timer1(0);`
		303	`// if (speed > SPEEDMAX) speed--;`
		304	`continue;`
		305	`}`
		306
		307	`if(bit_test(sensors,1)) //.\|.....//`
		308	`{`
		309	`if(sensors != last_sensors) uhel=STRED - BEAR2 - v;`
		310	`if(j>=DOZNIVANI)`
		311	`{`
		312	`j=0;`
		313	`if(uhel<STRED - BEAR2) uhel++;`
		314	`}`
		315	`Lmotor=speed-50;`
		316	`Rmotor=speed;`
		317	`line=S;`
		318	`set_timer1(0);`
		319	`// if (speed > SPEEDMAX) speed--;`
		320	`continue;`
		321	`}`
		322
		323	`if(bit_test(sensors,5)) //.....\|.//`
		324	`{`
		325	`if(sensors != last_sensors) uhel=STRED + BEAR2 + v;`
		326	`if(j>=DOZNIVANI)`
		327	`{`
		328	`j=0;`
		329	`if(uhel>STRED + BEAR2) uhel--;`
		330	`}`
		331	`Rmotor=speed-50;`
		332	`Lmotor=speed;`
		333	`line=S;`
		334	`set_timer1(0);`
		335	`// if (speed > SPEEDMAX) speed--;`
		336	`continue;`
		337	`}`
		338
		339	`if (bit_test(sensors,2)) //..\|....//`
		340	`{`
		341	`if(sensors != last_sensors) uhel=STRED - BEAR1 - v;`
		342	`if(j>=DOZNIVANI)`
		343	`{`
		344	`if(uhel<STRED - BEAR1)uhel++;`
		345	`j=0;`
		346	`}`
		347	`Lmotor=speed;`
		348	`Rmotor=speed;`
		349	`line=S;`
		350	`set_timer1(0);`
		351	`if (rovinka<255) rovinka++;`
		352	`// if (speed > SPEEDMAX) speed--;`
		353	`continue;`
		354	`}`
		355
		356	`if (bit_test(sensors,4)) //....\|..//`
		357	`{`
		358	`if(sensors != last_sensors) uhel=STRED + BEAR1 + v;`
		359	`if(j>=DOZNIVANI)`
		360	`{`
		361	`j=0;`
		362	`if(uhel>STRED + BEAR1) uhel--;`
		363	`}`
		364	`Rmotor=speed;`
		365	`Lmotor=speed;`
		366	`line=S;`
		367	`set_timer1(0);`
		368	`if (rovinka<255) rovinka++;`
		369	`// if (speed > SPEEDMAX) speed--;`
		370	`continue;`
		371	`}`
		372
		373	`if ((L==line) \|\| (R==line)) // Brzdeni pri vyjeti z trate`
		374	`{`
		375	`if (rovinka>250)`
		376	`{`
		377	`BL; BR;`
		378	`Delay_ms(100);`
		379	`};`
		380	`rovinka=0;`
		381	`speed=SPEEDMAX;`
		382	`};`
		383	`}`
		384	`}`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/3Orbis/bak/06 PD Regulator/master/main.c @ 2 – Rev