WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/objizdka_R.c


// **** Objeti cihly vpravo **** RRRR
int8 n,i;
int16 j;
 
set_adc_channel(RMAX);
 
SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(0);   // vzad
set_pwm2_duty(0);
output_low(MOT_L);
output_high(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost
set_pwm1_duty(0);    // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(215);
brzda();
 
disp(1);
SetServo((CASAVR-CASMIN));   // rovne              S
set_pwm1_duty(160);  // vpred
set_pwm2_duty(160);
output_low(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
for(n=1;n<=7;n++)
{
  while(get_timer1()<(odocounter+n)); // Popojed
  SetServoQ((CASAVR-CASMIN));
};
 
i=0;
 
disp(2);
set_pwm1_duty(130);
set_pwm2_duty(140);
odocounter=get_timer1();
for(n=1;n<=10;n++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i+=8;
};
 
disp(3);
odocounter=get_timer1();
for(n=1;n<=10;n++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i-=8;
};
 
disp(4);
odocounter=get_timer1();
for(n=1;n<=10;n++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i+=8;
};
 
disp(5);
odocounter=get_timer1();
for(n=1;n<=5;n++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i-=16;
};
 
disp(6);
set_pwm1_duty(80);  // vpred
set_pwm2_duty(80);
output_low(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
for(n=1;n<=3;n++)
{
  while(get_timer1()<(odocounter+n));
  SetServoQ((CASAVR-CASMIN));
};
 
disp(7);
while(true)
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
}
 
 
if(cas==CASMIN)
{
   set_adc_channel(RMAX);    // Pravy UV sensor
   for(j=0;j<10000;j++)
   {
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
      if (read_adc()<128) break;
   }
}
else
{
   set_adc_channel(LMAX);    // Pravy UV sensor
   for(j=0;j<10000;j++)
   {
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
      if (read_adc()<128) break;
   }
}
 
set_adc_channel(RMAX);    // Pravy UV sensor
 
set_pwm1_duty(0);    // zabrzdi
set_pwm2_duty(0);
output_high(MOT_L);
output_high(MOT_R);
SetServo(CASAVR-CASMIN);   // doprostred
delay_ms(100);
brzda();
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
 
SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(0);   // vzad
set_pwm2_duty(0);
output_low(MOT_L);
output_high(MOT_R);
while (read_adc()>128);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(0);    // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
SetServo(CASAVR-CASMIN);   // doprostred
delay_ms(100);
brzda();
 
set_pwm1_duty(255);    // max. vpred
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+2)) // Ujed kousek
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
   cas=CASAVR-CASMIN;    // Cara je rovne
};
 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
//rr=60;   // Nerozumna rychlost pro rozjeti

Rev 213	Rev 214
1	`// ** Objeti cihly vpravo ** RRRR`	1	`// ** Objeti cihly vpravo ** RRRR`
2	`int8 n,i;`	2	`int8 n,i;`
3	`int16 j;`	3	`int16 j;`
4		4
5	`set_adc_channel(RMAX);`	5	`set_adc_channel(RMAX);`
6		6
7	`SetServo(CASMIN); // max. doleva L`	7	`SetServo(CASMIN); // max. doleva L`
8	`set_pwm1_duty(0); // vzad`	8	`set_pwm1_duty(0); // vzad`
9	`set_pwm2_duty(0);`	9	`set_pwm2_duty(0);`
10	`output_low(MOT_L);`	10	`output_low(MOT_L);`
11	`output_high(MOT_R);`	11	`output_high(MOT_R);`
12	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	12	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
13	`while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost`	13	`while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost`
14	`set_pwm1_duty(0); // reverz (zabrzdi)`	14	`set_pwm1_duty(0); // reverz (zabrzdi)`
15	`set_pwm2_duty(255);`	15	`set_pwm2_duty(255);`
16	`output_low(MOT_L);`	16	`output_low(MOT_L);`
17	`output_low(MOT_R);`	17	`output_low(MOT_R);`
18	`delay_ms(215);`	18	`delay_ms(215);`
19	`brzda();`	19	`brzda();`
20		20
21	`disp(1);`	21	`disp(1);`
22	`SetServo((CASAVR-CASMIN)); // rovne S`	22	`SetServo((CASAVR-CASMIN)); // rovne S`
23	`set_pwm1_duty(160); // vpred`	23	`set_pwm1_duty(160); // vpred`
24	`set_pwm2_duty(160);`	24	`set_pwm2_duty(160);`
25	`output_low(MOT_L);`	25	`output_low(MOT_L);`
26	`output_low(MOT_R);`	26	`output_low(MOT_R);`
27	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	27	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
28	`for(n=1;n<=7;n++)`	28	`for(n=1;n<=7;n++)`
29	`{`	29	`{`
30	`while(get_timer1()<(odocounter+n)); // Popojed`	30	`while(get_timer1()<(odocounter+n)); // Popojed`
31	`SetServoQ((CASAVR-CASMIN));`	31	`SetServoQ((CASAVR-CASMIN));`
32	`};`	32	`};`
33		33
34	`i=0;`	34	`i=0;`
35		35
36	`disp(2);`	36	`disp(2);`
37	`set_pwm1_duty(130);`	37	`set_pwm1_duty(130);`
38	`set_pwm2_duty(140);`	38	`set_pwm2_duty(140);`
39	`odocounter=get_timer1();`	39	`odocounter=get_timer1();`
40	`for(n=1;n<=10;n++)`	40	`for(n=1;n<=10;n++)`
41	`{`	41	`{`
42	`while(get_timer1()<(odocounter+n));`	42	`while(get_timer1()<(odocounter+n));`
43	`SetServoQ((CASAVR-CASMIN)-i);`	43	`SetServoQ((CASAVR-CASMIN)-i);`
44	`set_pwm1_duty(130-i);`	44	`set_pwm1_duty(130-i);`
45	`i+=8;`	45	`i+=8;`
46	`};`	46	`};`
47		47
48	`disp(3);`	48	`disp(3);`
49	`odocounter=get_timer1();`	49	`odocounter=get_timer1();`
50	`for(n=1;n<=6;n++)`	50	`for(n=1;n<=10;n++)`
51	`{`	51	`{`
52	`while(get_timer1()<(odocounter+n));`	52	`while(get_timer1()<(odocounter+n));`
53	`SetServoQ((CASAVR-CASMIN)-i);`	53	`SetServoQ((CASAVR-CASMIN)-i);`
54	`set_pwm1_duty(130-i);`	54	`set_pwm1_duty(130-i);`
55	`i-=8;`	55	`i-=8;`
56	`};`	56	`};`
57		57
58	`disp(4);`	58	`disp(4);`
59	`odocounter=get_timer1();`	59	`odocounter=get_timer1();`
60	`for(n=1;n<=6;n++)`	60	`for(n=1;n<=10;n++)`
61	`{`	61	`{`
62	`while(get_timer1()<(odocounter+n));`	62	`while(get_timer1()<(odocounter+n));`
63	`SetServoQ((CASAVR-CASMIN)-i);`	63	`SetServoQ((CASAVR-CASMIN)-i);`
64	`set_pwm1_duty(130-i);`	64	`set_pwm1_duty(130-i);`
65	`i+=8;`	65	`i+=8;`
66	`};`	66	`};`
67		67
68	`disp(5);`	68	`disp(5);`
69	`odocounter=get_timer1();`	69	`odocounter=get_timer1();`
70	`for(n=1;n<=5;n++)`	70	`for(n=1;n<=5;n++)`
71	`{`	71	`{`
72	`while(get_timer1()<(odocounter+n));`	72	`while(get_timer1()<(odocounter+n));`
73	`SetServoQ((CASAVR-CASMIN)-i);`	73	`SetServoQ((CASAVR-CASMIN)-i);`
74	`set_pwm1_duty(130-i);`	74	`set_pwm1_duty(130-i);`
75	`i-=16;`	75	`i-=16;`
76	`};`	76	`};`
77		77
78	`disp(6);`	78	`disp(6);`
79	`set_pwm1_duty(80); // vpred`	79	`set_pwm1_duty(80); // vpred`
80	`set_pwm2_duty(80);`	80	`set_pwm2_duty(80);`
81	`output_low(MOT_L);`	81	`output_low(MOT_L);`
82	`output_low(MOT_R);`	82	`output_low(MOT_R);`
83	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	83	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
84	`for(n=1;n<=3;n++)`	84	`for(n=1;n<=3;n++)`
85	`{`	85	`{`
86	`while(get_timer1()<(odocounter+n));`	86	`while(get_timer1()<(odocounter+n));`
87	`SetServoQ((CASAVR-CASMIN));`	87	`SetServoQ((CASAVR-CASMIN));`
88	`};`	88	`};`
89		89
90	`disp(7);`	90	`disp(7);`
91	`while(true)`	91	`while(true)`
92	`{`	92	`{`
93	`set_adc_channel(LMAX); // Levy UV sensor`	93	`set_adc_channel(LMAX); // Levy UV sensor`
94	`delay_us(40);`	94	`delay_us(40);`
95	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	95	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`
96	`set_adc_channel(RMAX); // Pravy UV sensor`	96	`set_adc_channel(RMAX); // Pravy UV sensor`
97	`delay_us(40);`	97	`delay_us(40);`
98	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	98	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
99	`}`	99	`}`
100		100
101		101
102	`if(cas==CASMIN)`	102	`if(cas==CASMIN)`
103	`{`	103	`{`
104	`set_adc_channel(RMAX); // Pravy UV sensor`	104	`set_adc_channel(RMAX); // Pravy UV sensor`
105	`for(j=0;j<10000;j++)`	105	`for(j=0;j<10000;j++)`
106	`{`	106	`{`
107	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`	107	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`
108	`if (read_adc()<128) break;`	108	`if (read_adc()<128) break;`
109	`}`	109	`}`
110	`}`	110	`}`
111	`else`	111	`else`
112	`{`	112	`{`
113	`set_adc_channel(LMAX); // Pravy UV sensor`	113	`set_adc_channel(LMAX); // Pravy UV sensor`
114	`for(j=0;j<10000;j++)`	114	`for(j=0;j<10000;j++)`
115	`{`	115	`{`
116	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`	116	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`
117	`if (read_adc()<128) break;`	117	`if (read_adc()<128) break;`
118	`}`	118	`}`
119	`}`	119	`}`
120		120
121	`set_adc_channel(RMAX); // Pravy UV sensor`	121	`set_adc_channel(RMAX); // Pravy UV sensor`
122		122
123	`set_pwm1_duty(0); // zabrzdi`	123	`set_pwm1_duty(0); // zabrzdi`
124	`set_pwm2_duty(0);`	124	`set_pwm2_duty(0);`
125	`output_high(MOT_L);`	125	`output_high(MOT_L);`
126	`output_high(MOT_R);`	126	`output_high(MOT_R);`
127	`SetServo(CASAVR-CASMIN); // doprostred`	127	`SetServo(CASAVR-CASMIN); // doprostred`
128	`delay_ms(100);`	128	`delay_ms(100);`
129	`brzda();`	129	`brzda();`
130	`// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care`	130	`// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care`
131		131
132	`SetServo(CASMIN); // max. doleva L`	132	`SetServo(CASMIN); // max. doleva L`
133	`set_pwm1_duty(0); // vzad`	133	`set_pwm1_duty(0); // vzad`
134	`set_pwm2_duty(0);`	134	`set_pwm2_duty(0);`
135	`output_low(MOT_L);`	135	`output_low(MOT_L);`
136	`output_high(MOT_R);`	136	`output_high(MOT_R);`
137	`while (read_adc()>128);`	137	`while (read_adc()>128);`
138	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	138	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
139	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	139	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
140	`set_pwm1_duty(0); // reverz (zabrzdi)`	140	`set_pwm1_duty(0); // reverz (zabrzdi)`
141	`set_pwm2_duty(255);`	141	`set_pwm2_duty(255);`
142	`output_low(MOT_L);`	142	`output_low(MOT_L);`
143	`output_low(MOT_R);`	143	`output_low(MOT_R);`
144	`SetServo(CASAVR-CASMIN); // doprostred`	144	`SetServo(CASAVR-CASMIN); // doprostred`
145	`delay_ms(100);`	145	`delay_ms(100);`
146	`brzda();`	146	`brzda();`
147		147
148	`set_pwm1_duty(255); // max. vpred`	148	`set_pwm1_duty(255); // max. vpred`
149	`set_pwm2_duty(255);`	149	`set_pwm2_duty(255);`
150	`output_low(MOT_L);`	150	`output_low(MOT_L);`
151	`output_low(MOT_R);`	151	`output_low(MOT_R);`
152	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	152	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
153	`while(get_timer1()<(odocounter+2)) // Ujed kousek`	153	`while(get_timer1()<(odocounter+2)) // Ujed kousek`
154	`{`	154	`{`
155	`set_adc_channel(LMAX); // Levy UV sensor`	155	`set_adc_channel(LMAX); // Levy UV sensor`
156	`delay_us(40);`	156	`delay_us(40);`
157	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	157	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`
158	`set_adc_channel(RMAX); // Pravy UV sensor`	158	`set_adc_channel(RMAX); // Pravy UV sensor`
159	`delay_us(40);`	159	`delay_us(40);`
160	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	160	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
161	`cas=CASAVR-CASMIN; // Cara je rovne`	161	`cas=CASAVR-CASMIN; // Cara je rovne`
162	`};`	162	`};`
163		163
164	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`	164	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`
165	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	165	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
166	`//rr=60; // Nerozumna rychlost pro rozjeti`	166	`//rr=60; // Nerozumna rychlost pro rozjeti`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_R.c – Rev 213 → 214