WebSVN – svnkaklik – Blame – /roboti/istrobot/tank/Kopie

Rev	Author	Line No.	Line
2	kaklik	1	`#include "D:\@KAKLIK\programy\PIC_C\robot\tank\main.h"`
		2
		3	`//motory`
		4	`#define FR output_high(PIN_B4);output_low(PIN_B5) // Vpred`
		5	`#define FL output_high(PIN_B6);output_low(PIN_B7)`
		6	`#define BR output_high(PIN_B5);output_low(PIN_B4) // Vzad`
		7	`#define BL output_high(PIN_B7);output_low(PIN_B6)`
		8	`#define STOPR output_low(PIN_B4);output_low(PIN_B5)`
		9	`#define STOPL output_low(PIN_B6);output_low(PIN_B7)`
		10
		11	`//cidla`
		12	`#define RSENSOR 1 // Senzory na caru`
		13	`#define LSENSOR 0`
		14	`//#define BUMPER SENSOR_2 // Senzor na cihlu`
		15
		16	`//human interface`
		17	`#DEFINE LED PIN_A2`
		18	`#DEFINE SOUND_HI PIN_B2`
		19	`#DEFINE SOUND_LO PIN_B3`
		20	`#include "sound.c"`
		21
		22	`//program`
		23	`#define MEZERA 38 // jak se muze jet dlouho bez cary`
		24	`#define COUVEJ 700 // kolik se ma couvat po detekci diry`
		25	`#define CIKCAK 40 // kolik se ma jet cik/cak`
		26	`#define PRES_DIRU 350 // jak predpokladame velkou diru`
		27	`#define TRESHOLD 50`
		28
		29	`#define L 0 // left`
		30	`#define R 1 // right`
		31	`#define S 2 // straight`
		32
		33	`int movement; // smer minuleho pohybu`
		34	`int line; // na ktere strane byla detekovana cara`
		35	`int dira; // pocitadlo pro nalezeni preruseni cary`
		36	`int n; // pomocna promena pro cyklus FOR`
		37
		38	`void cik_cak() // Hledani cary`
		39	`{`
		40	`n=CIKCAK/2; // poprve hned zatoc opacne, nez se jelo pred detekci diry`
		41	`switch(movement) // zmenime smer zataceni`
		42	`{`
		43	`case L:`
		44	`FL;STOPR; // doprava`
		45	`movement=R; // poznamenej kam jedem`
		46	`line=L; // kdyz prejedem, tak bude cara vlevo`
		47	`break;`
		48	`case R:`
		49	`FR;STOPL; // doleva`
		50	`movement=L; // poznamenej kam jedem`
		51	`line=R; // kdyz prejedem, tak bude cara vpravo`
		52	`break;`
		53	`}`
		54
		55	`while(true) // jed cik-cak, dokud nenajdes caru`
		56	`{`
		57	`set_adc_channel(RSENSOR);`
		58	`if (TRESHOLD > read_adc()) // je cara ?`
		59	`{`
		60	`STOPL;`
		61	`STOPR;`
		62	`break;`
		63	`};`
		64	`if (CIKCAK < n++) // Jedeme uz dost dlouho cik? Pak jed cak.`
		65	`{`
		66	`n=0;`
		67	`STOPL;`
		68	`STOPR;`
		69	`switch(movement) // zmenime smer zataceni`
		70	`{`
		71	`case L:`
		72	`FL; // doprava`
		73	`movement=R; // poznamenej kam jedem`
		74	`line=L; // kdyz prejedem, tak bude cara vlevo`
		75	`break;`
		76	`case R:`
		77	`FR; // doleva`
		78	`movement=L; // poznamenej kam jedem`
		79	`line=R; // kdyz prejedem, tak bude cara vpravo`
		80	`break;`
		81	`}`
		82	`}`
		83	`}`
		84	`}`
		85	`void diagnostika()`
		86	`{`
		87	`STOPL; //zastav vse`
		88	`STOPR;`
		89
		90	`FR; //pravy pas`
		91	`Delay_ms(1000);`
		92	`STOPR;`
		93	`Delay_ms(1000);`
		94	`BR;`
		95	`Delay_ms(1000);`
		96	`STOPR;`
		97	`Delay_ms(1000);`
		98
		99	`FL; //levy pas`
		100	`Delay_ms(1000);`
		101	`STOPL;`
		102	`Delay_ms(1000);`
		103	`BL;`
		104	`Delay_ms(1000);`
		105	`STOPL;`
		106	`Delay_ms(1000);`
		107
		108	`FL; //oba pasy`
		109	`FR;`
		110	`Delay_ms(1000);`
		111	`STOPL;`
		112	`STOPR;`
		113	`Delay_ms(1000);`
		114	`BL;`
		115	`BR;`
		116	`Delay_ms(1000);`
		117	`STOPL;`
		118	`STOPR;`
		119	`Delay_ms(1000);`
		120
		121	`}`
		122
		123	`void main()`
		124	`{`
		125	`SoundBeep(640,200); //beep`
		126	`Delay_ms(50);`
		127	`SoundBeep(640,200);`
		128
		129	`setup_adc_ports(RA0_RA1_RA3_ANALOG);`
		130	`setup_adc(ADC_CLOCK_DIV_2);`
		131	`Delay_ms(1000); // 1s`
		132
		133	`movement=R;`
		134	`line=S;`
		135	`dira=0;`
		136
		137	`//cik_cak(); // toc se, abys nasel caru`
		138	`FL; FR; // vpred`
		139
		140	`// Sledovani cary`
		141	`while(true)`
		142	`{`
		143	`snimani:`
		144	`set_adc_channel(RSENSOR);`
		145	`Delay_us(10);`
		146	`if (TRESHOLD > read_adc()) // Cara pod pravym senzorem`
		147	`{`
		148	`dira=0; // nuluj pocitadlo diry, protoze jsme videli caru`
		149	`line=R; // zaznamenej, kdes videl caru`
		150	`FL;FR;`
		151	`goto snimani;`
		152	`}`
		153
		154	`set_adc_channel(LSENSOR);`
		155	`Delay_us(10);`
		156	`if (TRESHOLD > read_adc()) // Cara pod levym senzorem`
		157	`{`
		158	`dira=0; // nuluj pocitadlo diry, protoze jsme videli caru`
		159	`line=L; // zaznamenej, kdes videl caru`
		160	`FL;FR;`
		161	`goto snimani;`
		162	`}`
		163
		164	`// oba senzory mimo caru`
		165	`if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat`
		166	`// v ostatnich cyklech nedelame nic (pro urychleni snimani)`
		167	`{`
		168	`STOPL;`
		169	`STOPR;`
		170	`Delay_ms(100);`
		171	`// kdyz nevidis caru, tak jed tam, kdes ji videl naposled`
		172	`if(line==L)`
		173	`{`
		174	`// doleva`
		175	`FR;`
		176	`movement=L; // poznamenej kam jedes`
		177	`}`
		178	`if(line==R)`
		179	`{`
		180	`// doprava`
		181	`FL;`
		182	`movement=R; // poznamenej kam jedes`
		183	`}`
		184	`dira++; // zvets pocitadlo diry, aby to pristi cyklus tudy neslo`
		185	`goto snimani; // co nejrychleji se vrat na snimani cary`
		186	`}`
		187
		188	`/*if (MEZERA < dira++) // nejedeme uz moc dlouho bez cary?`
		189	`{`
		190	`dira=0;`
		191	`switch(movement) // kam se jelo ?`
		192	`{`
		193	`case L:`
		194	`BR; // couvej doprava`
		195	`Delay_ms(COUVEJ);`
		196	`movement=R;`
		197	`break;`
		198	`case R:`
		199	`BL; // couvej doleva`
		200	`Delay_ms(COUVEJ);`
		201	`movement=L;`
		202	`break;`
		203	`}`
		204	`STOP(); // konec couvani`
		205	`FL; FR; // rovne pres diru`
		206	`Delay_ms(PRES_DIRU);`
		207	`STOP();`
		208	`cik_cak(); // najdi caru`
		209	`FR; FL; // vpred`
		210	`} // dira*/`
		211
		212	`} // while(true)`
		213
		214	`}`
		215
		216	`/*void objizdka()`
		217	`{`
		218	`BL; BR; // zacouvej`
		219	`Delay_ms(150);`
		220	`STOP();`
		221
		222	`cik_cak(); // vyrovnej se na caru`
		223	`cik_cak(); // radeji 3x, aby se vyrovnaly setrvacne sily`
		224	`cik_cak();`
		225
		226	`BL; FR; Delay_ms(220); // vlevo`
		227	`STOP();`
		228	`FL; FR; Delay_ms(650); // rovne`
		229	`STOP();`
		230	`FL; BR; Delay_ms(220); // vpravo`
		231	`STOP();`
		232	`FL; FR; Delay_ms(770); // rovne`
		233	`STOP();`
		234	`FL; BR; Delay_ms(210); // vpravo`
		235	`STOP();`
		236	`FL; FR;`
		237	`Delay_ms(200); // kousek rovne`
		238	`// rovne, dokud nenarazis na caru`
		239	`while(input(RSENSOR)); // pravej senzor, protoze cihla je vpravo`
		240	`STOP();`
		241	`BL; FR; // toc se doleva, dokud nenarazis na caru`
		242	`while(input(LSENSOR)); // levej senzor, protoze cara je urcite vlevo`
		243	`STOP();`
		244	`line=R; // caru predpokladame vpravo, kdybysme ji prejeli`
		245	`dira=0;`
		246	`FL; FR; // vpred`
		247	`}*/`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/tank/Kopie - main.c @ 361 – Rev