WebSVN – svnkaklik – Blame – /roboti/istrobot/tank/main.c

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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/tank/main.c @ 78 – Rev