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

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