WebSVN – svnkaklik – Diff – /roboti/istrobot/cholerik/cholerik.c


// Program pro MiniSumo na R-Day 2006
//"$Id: cholerik.c 243 2007-04-19 19:57:53Z kakl $"
 
#include "cholerik.h"
 
#define THRESHOLD     15     // rozhodovaci uroven pro okraj areny
                            // 0-15 (od 0.25 do 0.75 Vdd)
                            // 0 jasnejsi; 15 tmavsi
 
//motory            //Napred vypnout potom zapnout!
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
#define FL         output_low(PIN_B7); output_high(PIN_B6)
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
#define BL         output_low(PIN_B6); output_high(PIN_B7)
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
 
//cidla
#define SIDE_R    !input(PIN_A7)     // Sensory na soupere
#define SIDE_L    !input(PIN_A4)
#define FRONT     !input(PIN_A6)
#define BACK      !input(PIN_B3)
#define GRAVITY   !input(PIN_A1)
// PIN_A3   Prave cidlo na okraj areny (C1OUT)
// PIN_A2   Leve cidlo na okraj areny (C2OUT)
 
#DEFINE SOUND_HI   PIN_B1     // komplementarni vystupy pro piezo pipak
#DEFINE SOUND_LO   PIN_B2
 
// makro pro PWM
#define GO(motor, direction, power) if(get_timer0()<=power) \
{direction##motor;} else {stop##motor;}
 
unsigned int8 majak=0;
unsigned int8 sl=0;
unsigned int8 sr=0;
unsigned int8 b=0;
unsigned int8 f=0;
unsigned int8 g=0;
int1  arena_l=FALSE;
int1  arena_r=FALSE;
int1 diag=FALSE;
 
#int_TIMER0
TIMER0_isr()
{
   int1 stav;
 
   stav = ((majak & 0b1) == 0b1);
   if (((SIDE_R && stav) || (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;};
   if (((SIDE_L && stav) || (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;};
   if (((BACK && stav) || (!BACK && !stav))) {if (b<255) b++;} else {b=0;};
   if (((FRONT && stav) || (!FRONT && !stav))) {if (f<255) f++;} else {f=0;};
   majak++;
   stav = ((majak & 0b1) == 0b1);
 
   if (!C1OUT) arena_r=TRUE; else arena_r=FALSE;
   if (!C2OUT) arena_l=TRUE; else arena_l=FALSE;
 
   if (stav)
   {
      set_pwm1_duty(27);      // 1:1
   }
   else
   {
      set_pwm1_duty(55);      // 1:0
   };
   setup_comparator(NC_NC_NC_NC);   // inicializace komparatoru
   if (GRAVITY) {if (g<255) g++;} else g=0;
   setup_comparator(A3_VR_A2_VR);   // inicializace komparatoru
   if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se
}
 
// Primitivni Pipani
void beep(unsigned int16 period, unsigned int16 length)
{
   unsigned int16 nn;
 
   disable_interrupts(GLOBAL);
   for(nn=length; nn>0; nn--)
   {
     output_high(SOUND_HI);output_low(SOUND_LO);
     delay_us(period);
     output_high(SOUND_LO);output_low(SOUND_HI);
     delay_us(period);
   }
   enable_interrupts(GLOBAL);
}
 
/**** DIAG ********************************************************************/
inline void diagnostika()
{
   unsigned int16 n;
 
   setup_comparator(NC_NC_NC_NC);   // inicializace komparatoru
   if (GRAVITY)
   {
      setup_comparator(A3_VR_A2_VR);   // inicializace komparatoru
      diag=TRUE;
      enable_interrupts(INT_TIMER0);
      enable_interrupts(GLOBAL);
      while (true)         // Diagnostika cidel
      {
         if (g>100) beep(800,100);
         Delay_ms(50);
         if (arena_l) {beep(1000,200); delay_ms(10);beep(1000,200);};
         Delay_ms(50);
         if (arena_r) {beep(2000,300); delay_ms(10);beep(2000,300);};
         Delay_ms(50);
 
         if (sr>10) beep(3000,400);
         Delay_ms(50);
         if (f>10) beep(4000,500);
         Delay_ms(50);
         if (sl>10) beep(5000,500);
         Delay_ms(50);
         if (b>10) beep(6000,600);
         Delay_ms(50);
         if((g>100) && arena_l && arena_r) break;  // Preklopen na zada a bily papir na obou cidlech na okraj areny
      };
 
      while(TRUE)   // Diagnostika podvozku
      {
         for (n=500; n<800; n+=100)
         {
            beep(n,n); //beep UP
         };
         Delay_ms(1000);
         //zastav vse
         STOPL; STOPR;
         //pravy pas
         FR; Delay_ms(1000); STOPR; Delay_ms(1000);
         BR; Delay_ms(1000); STOPR; Delay_ms(1000);
         Beep(880,100); Delay_ms(1000);
         //levy pas
         FL; Delay_ms(1000); STOPL; Delay_ms(1000);
         BL; Delay_ms(1000); STOPL; Delay_ms(1000);
         Beep(880,100); Delay_ms(1000);
         //oba pasy
         FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
         BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
      };
   };
}
 
/**** MAIN ********************************************************************/
void main()
{
   unsigned int16 n; // for FOR
 
   STOPL; STOPR;     // zastavi motory
 
   setup_oscillator(OSC_8MHZ|OSC_INTRC);     // CPU clock 8MHz
   setup_adc_ports(NO_ANALOGS);   // komparatory vypnuty
   setup_adc(ADC_OFF);
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);  // Casovac pro SW PWM a cteni cidel
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz
   setup_ccp1(CCP_PWM);    // HW PWM ON
   set_pwm1_duty(27);      // 1:1
   setup_comparator(A3_VR_A2_VR);   // inicializace komparatoru
   setup_comparator(NC_NC_NC_NC);   // inicializace komparatoru
   setup_vref(VREF_HIGH|THRESHOLD);        // 16 kroku od 0.25 do 0.75 Vdd
 
   Beep(1000,200);     //double beep
   Delay_ms(50);
   Beep(1000,200);
   diagnostika();
 
   enable_interrupts(INT_TIMER0);
   enable_interrupts(GLOBAL);
 
   for (n=1;n<=5;n++)   // 5s do zacatku souboje
   {
      Delay_ms(720);
      Beep(1000,200);
   }
 
/*----- Main Loop ------------------------------------------------------------*/
   while(true)       // hlavni smycka
   {
LOOP:
 
      GO(L, F, 150); GO(R, F, 150);
 
      if (arena_r)
      {
         BL; BR;
         delay_ms(100);
         STOPL; BR;
         for(n=0; n<5000; n++)
         {
            if (!arena_r || arena_l) {BL; BR;};
         };
         FL; BR;
         delay_ms(100);
         STOPL; STOPR;
      }
 
      if (arena_l)
      {
         BL; BR;
         delay_ms(100);
         BL; STOPR;
         for(n=0; n<5000; n++)
         {
            if (!arena_l || arena_r) {BL; BR;};
         };
         BL; FR;
         delay_ms(100);
         STOPL; STOPR;
      }
 
      if (sr>10)     // Nepritel vpravo
      {
         FL; FR;                 // popojed rovne
         for(n=0; n<5000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
         };
         FL; BR;                     // otoc se na nej
         for(n=0; n<10000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
            if (f>5)
            {
               FL; FR;               // vytlac ho
            };
            if (sl>5) {BL; FR;};
            if (sr>5) {FL; BR;};
         };
      }
 
      if (sl>10)     // Nepritel vlevo
      {
         FL; FR;                 // popojed rovne
         for(n=0; n<5000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
         };
         BL; FR;                    // otoc se na nej
         for(n=0; n<10000; n++)
         {
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
            if (f>5)
            {
               FL; FR;              // vytlac ho
            };
            if (sl>5) {BL; FR;};
            if (sr>5) {FL; BR;};
         };
      }
 
      if (f>10)      // Nepritel vpredu
      {
         BL; FR;
         delay_ms(110);
         FL; BR;
         delay_ms(50);
         STOPL; STOPR;
      }
 
      if (b>10)      // Nepritel vzadu
      {
         BL; FR;
         delay_ms(110);
         FL; BR;
         delay_ms(50);
         STOPL; STOPR;
      }
 
   } // while(true)
}

Rev 242	Rev 243
1	`// Program pro MiniSumo na R-Day 2006`	1	`// Program pro MiniSumo na R-Day 2006`
2	`//"$Id: cholerik.c 242 2007-04-19 18:12:00Z kakl $"`	2	`//"$Id: cholerik.c 243 2007-04-19 19:57:53Z kakl $"`
3		3
4	`#include "cholerik.h"`	4	`#include "cholerik.h"`
5		5
6	`#define THRESHOLD 0x8 // rozhodovaci uroven pro okraj areny 0-15 (od 0.25 do 0.75 Vdd)`	6	`#define THRESHOLD 15 // rozhodovaci uroven pro okraj areny`
-		7	`// 0-15 (od 0.25 do 0.75 Vdd)`
-		8	`// 0 jasnejsi; 15 tmavsi`
7		9
8	`//motory //Napred vypnout potom zapnout!`	10	`//motory //Napred vypnout potom zapnout!`
9	`#define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred`	11	`#define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred`
10	`#define FL output_low(PIN_B7); output_high(PIN_B6)`	12	`#define FL output_low(PIN_B7); output_high(PIN_B6)`
11	`#define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad`	13	`#define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad`
12	`#define BL output_low(PIN_B6); output_high(PIN_B7)`	14	`#define BL output_low(PIN_B6); output_high(PIN_B7)`
13	`#define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav`	15	`#define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav`
14	`#define STOPL output_low(PIN_B6);output_low(PIN_B7)`	16	`#define STOPL output_low(PIN_B6);output_low(PIN_B7)`
15		17
16	`//cidla`	18	`//cidla`
17	`#define SIDE_R !input(PIN_A7) // Sensory na soupere`	19	`#define SIDE_R !input(PIN_A7) // Sensory na soupere`
18	`#define SIDE_L !input(PIN_A4)`	20	`#define SIDE_L !input(PIN_A4)`
19	`#define FRONT !input(PIN_A6)`	21	`#define FRONT !input(PIN_A6)`
20	`#define BACK !input(PIN_B3)`	22	`#define BACK !input(PIN_B3)`
21	`#define GRAVITY !input(PIN_A1)`	23	`#define GRAVITY !input(PIN_A1)`
22	`// PIN_A3 Prave cidlo na okraj areny (C1OUT)`	24	`// PIN_A3 Prave cidlo na okraj areny (C1OUT)`
23	`// PIN_A2 Leve cidlo na okraj areny (C2OUT)`	25	`// PIN_A2 Leve cidlo na okraj areny (C2OUT)`
24		26
25	`#DEFINE SOUND_HI PIN_B1 // komplementarni vystupy pro piezo pipak`	27	`#DEFINE SOUND_HI PIN_B1 // komplementarni vystupy pro piezo pipak`
26	`#DEFINE SOUND_LO PIN_B2`	28	`#DEFINE SOUND_LO PIN_B2`
27		29
28	`// makro pro PWM`	30	`// makro pro PWM`
29	`#define GO(motor, direction, power) if(get_timer0()<=power) \`	31	`#define GO(motor, direction, power) if(get_timer0()<=power) \`
30	`{direction##motor;} else {stop##motor;}`	32	`{direction##motor;} else {stop##motor;}`
31		33
32	`unsigned int8 majak=0;`	34	`unsigned int8 majak=0;`
33	`unsigned int8 sl=0;`	35	`unsigned int8 sl=0;`
34	`unsigned int8 sr=0;`	36	`unsigned int8 sr=0;`
35	`unsigned int8 b=0;`	37	`unsigned int8 b=0;`
36	`unsigned int8 f=0;`	38	`unsigned int8 f=0;`
37	`unsigned int8 g=0;`	39	`unsigned int8 g=0;`
38	`int1 arena_l=FALSE;`	40	`int1 arena_l=FALSE;`
39	`int1 arena_r=FALSE;`	41	`int1 arena_r=FALSE;`
40	`int1 diag=FALSE;`	42	`int1 diag=FALSE;`
41		43
42	`#int_TIMER0`	44	`#int_TIMER0`
43	`TIMER0_isr()`	45	`TIMER0_isr()`
44	`{`	46	`{`
45	`int1 stav;`	47	`int1 stav;`
46		48
47	`stav = ((majak & 0b1) == 0b1);`	49	`stav = ((majak & 0b1) == 0b1);`
48	`if (((SIDE_R && stav) \|\| (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;};`	50	`if (((SIDE_R && stav) \|\| (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;};`
49	`if (((SIDE_L && stav) \|\| (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;};`	51	`if (((SIDE_L && stav) \|\| (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;};`
50	`if (((BACK && stav) \|\| (!BACK && !stav))) {if (b<255) b++;} else {b=0;};`	52	`if (((BACK && stav) \|\| (!BACK && !stav))) {if (b<255) b++;} else {b=0;};`
51	`if (((FRONT && stav) \|\| (!FRONT && !stav))) {if (f<255) f++;} else {f=0;};`	53	`if (((FRONT && stav) \|\| (!FRONT && !stav))) {if (f<255) f++;} else {f=0;};`
52	`majak++;`	54	`majak++;`
53	`stav = ((majak & 0b1) == 0b1);`	55	`stav = ((majak & 0b1) == 0b1);`
54		56
55	`if (!C1OUT) arena_r=TRUE; else arena_r=FALSE;`	57	`if (!C1OUT) arena_r=TRUE; else arena_r=FALSE;`
56	`if (!C2OUT) arena_l=TRUE; else arena_l=FALSE;`	58	`if (!C2OUT) arena_l=TRUE; else arena_l=FALSE;`
57		59
58	`if (stav)`	60	`if (stav)`
59	`{`	61	`{`
60	`set_pwm1_duty(27); // 1:1`	62	`set_pwm1_duty(27); // 1:1`
61	`}`	63	`}`
62	`else`	64	`else`
63	`{`	65	`{`
64	`set_pwm1_duty(55); // 1:0`	66	`set_pwm1_duty(55); // 1:0`
65	`};`	67	`};`
66	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`	68	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`
67	`if (GRAVITY) {if (g<255) g++;} else g=0;`	69	`if (GRAVITY) {if (g<255) g++;} else g=0;`
68	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`	70	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`
69	`if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se`	71	`if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se`
70	`}`	72	`}`
71		73
72	`// Primitivni Pipani`	74	`// Primitivni Pipani`
73	`void beep(unsigned int16 period, unsigned int16 length)`	75	`void beep(unsigned int16 period, unsigned int16 length)`
74	`{`	76	`{`
75	`unsigned int16 nn;`	77	`unsigned int16 nn;`
76		78
77	`disable_interrupts(GLOBAL);`	79	`disable_interrupts(GLOBAL);`
78	`for(nn=length; nn>0; nn--)`	80	`for(nn=length; nn>0; nn--)`
79	`{`	81	`{`
80	`output_high(SOUND_HI);output_low(SOUND_LO);`	82	`output_high(SOUND_HI);output_low(SOUND_LO);`
81	`delay_us(period);`	83	`delay_us(period);`
82	`output_high(SOUND_LO);output_low(SOUND_HI);`	84	`output_high(SOUND_LO);output_low(SOUND_HI);`
83	`delay_us(period);`	85	`delay_us(period);`
84	`}`	86	`}`
85	`enable_interrupts(GLOBAL);`	87	`enable_interrupts(GLOBAL);`
86	`}`	88	`}`
87		89
88	`/** DIAG ******************************************************************/`	90	`/** DIAG ******************************************************************/`
89	`inline void diagnostika()`	91	`inline void diagnostika()`
90	`{`	92	`{`
91	`unsigned int16 n;`	93	`unsigned int16 n;`
92		94
93	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`	95	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`
94	`if (GRAVITY)`	96	`if (GRAVITY)`
95	`{`	97	`{`
96	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`	98	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`
97	`diag=TRUE;`	99	`diag=TRUE;`
98	`enable_interrupts(INT_TIMER0);`	100	`enable_interrupts(INT_TIMER0);`
99	`enable_interrupts(GLOBAL);`	101	`enable_interrupts(GLOBAL);`
100	`while (true) // Diagnostika cidel`	102	`while (true) // Diagnostika cidel`
101	`{`	103	`{`
102	`if (g>100) beep(800,100);`	104	`if (g>100) beep(800,100);`
103	`Delay_ms(50);`	105	`Delay_ms(50);`
104	`if (arena_l) {beep(1000,200); delay_ms(10);beep(1000,200);};`	106	`if (arena_l) {beep(1000,200); delay_ms(10);beep(1000,200);};`
105	`Delay_ms(50);`	107	`Delay_ms(50);`
106	`if (arena_r) {beep(2000,300); delay_ms(10);beep(2000,300);};`	108	`if (arena_r) {beep(2000,300); delay_ms(10);beep(2000,300);};`
107	`Delay_ms(50);`	109	`Delay_ms(50);`
108		110
109	`if (sr>10) beep(3000,400);`	111	`if (sr>10) beep(3000,400);`
110	`Delay_ms(50);`	112	`Delay_ms(50);`
111	`if (f>10) beep(4000,500);`	113	`if (f>10) beep(4000,500);`
112	`Delay_ms(50);`	114	`Delay_ms(50);`
113	`if (sl>10) beep(5000,500);`	115	`if (sl>10) beep(5000,500);`
114	`Delay_ms(50);`	116	`Delay_ms(50);`
115	`if (b>10) beep(6000,600);`	117	`if (b>10) beep(6000,600);`
116	`Delay_ms(50);`	118	`Delay_ms(50);`
117	`if((g>100) && arena_l && arena_r) break; // Preklopen na zada a bily papir na obou cidlech na okraj areny`	119	`if((g>100) && arena_l && arena_r) break; // Preklopen na zada a bily papir na obou cidlech na okraj areny`
118	`};`	120	`};`
119		121
120	`while(TRUE) // Diagnostika podvozku`	122	`while(TRUE) // Diagnostika podvozku`
121	`{`	123	`{`
122	`for (n=500; n<800; n+=100)`	124	`for (n=500; n<800; n+=100)`
123	`{`	125	`{`
124	`beep(n,n); //beep UP`	126	`beep(n,n); //beep UP`
125	`};`	127	`};`
126	`Delay_ms(1000);`	128	`Delay_ms(1000);`
127	`//zastav vse`	129	`//zastav vse`
128	`STOPL; STOPR;`	130	`STOPL; STOPR;`
129	`//pravy pas`	131	`//pravy pas`
130	`FR; Delay_ms(1000); STOPR; Delay_ms(1000);`	132	`FR; Delay_ms(1000); STOPR; Delay_ms(1000);`
131	`BR; Delay_ms(1000); STOPR; Delay_ms(1000);`	133	`BR; Delay_ms(1000); STOPR; Delay_ms(1000);`
132	`Beep(880,100); Delay_ms(1000);`	134	`Beep(880,100); Delay_ms(1000);`
133	`//levy pas`	135	`//levy pas`
134	`FL; Delay_ms(1000); STOPL; Delay_ms(1000);`	136	`FL; Delay_ms(1000); STOPL; Delay_ms(1000);`
135	`BL; Delay_ms(1000); STOPL; Delay_ms(1000);`	137	`BL; Delay_ms(1000); STOPL; Delay_ms(1000);`
136	`Beep(880,100); Delay_ms(1000);`	138	`Beep(880,100); Delay_ms(1000);`
137	`//oba pasy`	139	`//oba pasy`
138	`FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);`	140	`FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);`
139	`BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);`	141	`BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);`
140	`};`	142	`};`
141	`};`	143	`};`
142	`}`	144	`}`
143		145
144	`/** MAIN ******************************************************************/`	146	`/** MAIN ******************************************************************/`
145	`void main()`	147	`void main()`
146	`{`	148	`{`
147	`unsigned int16 n; // for FOR`	149	`unsigned int16 n; // for FOR`
148		150
149	`STOPL; STOPR; // zastavi motory`	151	`STOPL; STOPR; // zastavi motory`
150		152
151	`setup_oscillator(OSC_8MHZ\|OSC_INTRC); // CPU clock 8MHz`	153	`setup_oscillator(OSC_8MHZ\|OSC_INTRC); // CPU clock 8MHz`
152	`setup_adc_ports(NO_ANALOGS); // komparatory vypnuty`	154	`setup_adc_ports(NO_ANALOGS); // komparatory vypnuty`
153	`setup_adc(ADC_OFF);`	155	`setup_adc(ADC_OFF);`
154	`setup_spi(FALSE);`	156	`setup_spi(FALSE);`
155	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_4); // Casovac pro SW PWM a cteni cidel`	157	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_4); // Casovac pro SW PWM a cteni cidel`
156	`setup_timer_1(T1_DISABLED);`	158	`setup_timer_1(T1_DISABLED);`
157	`setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz`	159	`setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz`
158	`setup_ccp1(CCP_PWM); // HW PWM ON`	160	`setup_ccp1(CCP_PWM); // HW PWM ON`
159	`set_pwm1_duty(27); // 1:1`	161	`set_pwm1_duty(27); // 1:1`
160	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`	162	`setup_comparator(A3_VR_A2_VR); // inicializace komparatoru`
161	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`	163	`setup_comparator(NC_NC_NC_NC); // inicializace komparatoru`
162	`setup_vref(VREF_HIGH\|THRESHOLD); // 16 kroku od 0.25 do 0.75 Vdd`	164	`setup_vref(VREF_HIGH\|THRESHOLD); // 16 kroku od 0.25 do 0.75 Vdd`
163		165
164	`Beep(1000,200); //double beep`	166	`Beep(1000,200); //double beep`
165	`Delay_ms(50);`	167	`Delay_ms(50);`
166	`Beep(1000,200);`	168	`Beep(1000,200);`
167	`diagnostika();`	169	`diagnostika();`
168		170
169	`enable_interrupts(INT_TIMER0);`	171	`enable_interrupts(INT_TIMER0);`
170	`enable_interrupts(GLOBAL);`	172	`enable_interrupts(GLOBAL);`
171		173
172	`for (n=1;n<=5;n++) // 5s do zacatku souboje`	174	`for (n=1;n<=5;n++) // 5s do zacatku souboje`
173	`{`	175	`{`
174	`Delay_ms(720);`	176	`Delay_ms(720);`
175	`Beep(1000,200);`	177	`Beep(1000,200);`
176	`}`	178	`}`
177		179
178	`/----- Main Loop ------------------------------------------------------------/`	180	`/----- Main Loop ------------------------------------------------------------/`
179	`while(true) // hlavni smycka`	181	`while(true) // hlavni smycka`
180	`{`	182	`{`
181	`LOOP:`	183	`LOOP:`
182		184
183	`GO(L, F, 150); GO(R, F, 150);`	185	`GO(L, F, 150); GO(R, F, 150);`
184		186
185	`if (arena_r)`	187	`if (arena_r)`
186	`{`	188	`{`
187	`BL; BR;`	189	`BL; BR;`
188	`delay_ms(100);`	190	`delay_ms(100);`
189	`STOPL; BR;`	191	`STOPL; BR;`
190	`for(n=0; n<5000; n++)`	192	`for(n=0; n<5000; n++)`
191	`{`	193	`{`
192	`if (!arena_r \|\| arena_l) {BL; BR;};`	194	`if (!arena_r \|\| arena_l) {BL; BR;};`
193	`};`	195	`};`
194	`FL; BR;`	196	`FL; BR;`
195	`delay_ms(100);`	197	`delay_ms(100);`
196	`STOPL; STOPR;`	198	`STOPL; STOPR;`
197	`}`	199	`}`
198		200
199	`if (arena_l)`	201	`if (arena_l)`
200	`{`	202	`{`
201	`BL; BR;`	203	`BL; BR;`
202	`delay_ms(100);`	204	`delay_ms(100);`
203	`BL; STOPR;`	205	`BL; STOPR;`
204	`for(n=0; n<5000; n++)`	206	`for(n=0; n<5000; n++)`
205	`{`	207	`{`
206	`if (!arena_l \|\| arena_r) {BL; BR;};`	208	`if (!arena_l \|\| arena_r) {BL; BR;};`
207	`};`	209	`};`
208	`BL; FR;`	210	`BL; FR;`
209	`delay_ms(100);`	211	`delay_ms(100);`
210	`STOPL; STOPR;`	212	`STOPL; STOPR;`
211	`}`	213	`}`
212		214
213	`if (sr>10) // Nepritel vpravo`	215	`if (sr>10) // Nepritel vpravo`
214	`{`	216	`{`
215	`FL; FR; // popojed rovne`	217	`FL; FR; // popojed rovne`
216	`for(n=0; n<5000; n++)`	218	`for(n=0; n<5000; n++)`
217	`{`	219	`{`
218	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`	220	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`
219	`};`	221	`};`
220	`FL; BR; // otoc se na nej`	222	`FL; BR; // otoc se na nej`
221	`for(n=0; n<10000; n++)`	223	`for(n=0; n<10000; n++)`
222	`{`	224	`{`
223	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`	225	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`
224	`if (f>5)`	226	`if (f>5)`
225	`{`	227	`{`
226	`FL; FR; // vytlac ho`	228	`FL; FR; // vytlac ho`
227	`};`	229	`};`
228	`if (sl>5) {BL; FR;};`	230	`if (sl>5) {BL; FR;};`
229	`if (sr>5) {FL; BR;};`	231	`if (sr>5) {FL; BR;};`
230	`};`	232	`};`
231	`}`	233	`}`
232		234
233	`if (sl>10) // Nepritel vlevo`	235	`if (sl>10) // Nepritel vlevo`
234	`{`	236	`{`
235	`FL; FR; // popojed rovne`	237	`FL; FR; // popojed rovne`
236	`for(n=0; n<5000; n++)`	238	`for(n=0; n<5000; n++)`
237	`{`	239	`{`
238	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`	240	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`
239	`};`	241	`};`
240	`BL; FR; // otoc se na nej`	242	`BL; FR; // otoc se na nej`
241	`for(n=0; n<10000; n++)`	243	`for(n=0; n<10000; n++)`
242	`{`	244	`{`
243	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`	245	`if (arena_l \|\| arena_r) {BL; BR; delay_ms(100); goto LOOP;};`
244	`if (f>5)`	246	`if (f>5)`
245	`{`	247	`{`
246	`FL; FR; // vytlac ho`	248	`FL; FR; // vytlac ho`
247	`};`	249	`};`
248	`if (sl>5) {BL; FR;};`	250	`if (sl>5) {BL; FR;};`
249	`if (sr>5) {FL; BR;};`	251	`if (sr>5) {FL; BR;};`
250	`};`	252	`};`
251	`}`	253	`}`
252		254
253	`if (f>10) // Nepritel vpredu`	255	`if (f>10) // Nepritel vpredu`
254	`{`	256	`{`
255	`BL; FR;`	257	`BL; FR;`
256	`delay_ms(110);`	258	`delay_ms(110);`
257	`FL; BR;`	259	`FL; BR;`
258	`delay_ms(50);`	260	`delay_ms(50);`
259	`STOPL; STOPR;`	261	`STOPL; STOPR;`
260	`}`	262	`}`
261		263
262	`if (b>10) // Nepritel vzadu`	264	`if (b>10) // Nepritel vzadu`
263	`{`	265	`{`
264	`BL; FR;`	266	`BL; FR;`
265	`delay_ms(110);`	267	`delay_ms(110);`
266	`FL; BR;`	268	`FL; BR;`
267	`delay_ms(50);`	269	`delay_ms(50);`
268	`STOPL; STOPR;`	270	`STOPL; STOPR;`
269	`}`	271	`}`
270		272
271	`} // while(true)`	273	`} // while(true)`
272	`}`	274	`}`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/cholerik/cholerik.c – Rev 242 → 243