WebSVN – svnkaklik – Blame – /roboti/istrobot/3Orbis/bak/07 1.funkcni/master/main.BAK

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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/3Orbis/bak/07 1.funkcni/master/main.BAK – Rev 5