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<=7;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<=7;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++)
for(n=1;n<=4;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()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {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()<TRESHOLD) break;
   }
}
else
{
   set_adc_channel(LMAX);    // Levy UV sensor
   for(j=0;j<10000;j++)
   {
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
      if (read_adc()<TRESHOLD) 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()>TRESHOLD); 
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()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<TRESHOLD) {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 214	Rev 215
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<=10;n++)`	50	`for(n=1;n<=7;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<=10;n++)`	60	`for(n=1;n<=7;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	`for(n=1;n<=4;n++)`
71	`{`	72	`{`
72	`while(get_timer1()<(odocounter+n));`	73	`while(get_timer1()<(odocounter+n));`
73	`SetServoQ((CASAVR-CASMIN)-i);`	74	`SetServoQ((CASAVR-CASMIN)-i);`
74	`set_pwm1_duty(130-i);`	75	`set_pwm1_duty(130-i);`
75	`i-=16;`	76	`i-=16;`
76	`};`	77	`};`
77		78
78	`disp(6);`	79	`disp(6);`
79	`set_pwm1_duty(80); // vpred`	80	`set_pwm1_duty(80); // vpred`
80	`set_pwm2_duty(80);`	81	`set_pwm2_duty(80);`
81	`output_low(MOT_L);`	82	`output_low(MOT_L);`
82	`output_low(MOT_R);`	83	`output_low(MOT_R);`
83	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	84	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
84	`for(n=1;n<=3;n++)`	85	`for(n=1;n<=3;n++)`
85	`{`	86	`{`
86	`while(get_timer1()<(odocounter+n));`	87	`while(get_timer1()<(odocounter+n));`
87	`SetServoQ((CASAVR-CASMIN));`	88	`SetServoQ((CASAVR-CASMIN));`
88	`};`	89	`};`
89		90
90	`disp(7);`	91	`disp(7);`
91	`while(true)`	92	`while(true)`
92	`{`	93	`{`
93	`set_adc_channel(LMAX); // Levy UV sensor`	94	`set_adc_channel(LMAX); // Levy UV sensor`
94	`delay_us(40);`	95	`delay_us(40);`
95	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	96	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; }; // Prejeli jsme caru vlevo`
96	`set_adc_channel(RMAX); // Pravy UV sensor`	97	`set_adc_channel(RMAX); // Pravy UV sensor`
97	`delay_us(40);`	98	`delay_us(40);`
98	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	99	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; }; // Prejeli jsme caru vpravo`
99	`}`	100	`}`
100		101
101		102
102	`if(cas==CASMIN)`	103	`if(cas==CASMIN)`
103	`{`	104	`{`
104	`set_adc_channel(RMAX); // Pravy UV sensor`	105	`set_adc_channel(RMAX); // Pravy UV sensor`
105	`for(j=0;j<10000;j++)`	106	`for(j=0;j<10000;j++)`
106	`{`	107	`{`
107	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`	108	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`
108	`if (read_adc()<128) break;`	109	`if (read_adc()<TRESHOLD) break;`
109	`}`	110	`}`
110	`}`	111	`}`
111	`else`	112	`else`
112	`{`	113	`{`
113	`set_adc_channel(LMAX); // Pravy UV sensor`	114	`set_adc_channel(LMAX); // Levy UV sensor`
114	`for(j=0;j<10000;j++)`	115	`for(j=0;j<10000;j++)`
115	`{`	116	`{`
116	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`	117	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`
117	`if (read_adc()<128) break;`	118	`if (read_adc()<TRESHOLD) break;`
118	`}`	119	`}`
119	`}`	120	`}`
120		121
121	`set_adc_channel(RMAX); // Pravy UV sensor`	122	`set_adc_channel(RMAX); // Pravy UV sensor`
122		123
123	`set_pwm1_duty(0); // zabrzdi`	124	`set_pwm1_duty(0); // zabrzdi`
124	`set_pwm2_duty(0);`	125	`set_pwm2_duty(0);`
125	`output_high(MOT_L);`	126	`output_high(MOT_L);`
126	`output_high(MOT_R);`	127	`output_high(MOT_R);`
127	`SetServo(CASAVR-CASMIN); // doprostred`	128	`SetServo(CASAVR-CASMIN); // doprostred`
128	`delay_ms(100);`	129	`delay_ms(100);`
129	`brzda();`	130	`brzda();`
130	`// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care`	131	`// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care`
131		132
132	`SetServo(CASMIN); // max. doleva L`	133	`SetServo(CASMIN); // max. doleva L`
133	`set_pwm1_duty(0); // vzad`	134	`set_pwm1_duty(0); // vzad`
134	`set_pwm2_duty(0);`	135	`set_pwm2_duty(0);`
135	`output_low(MOT_L);`	136	`output_low(MOT_L);`
136	`output_high(MOT_R);`	137	`output_high(MOT_R);`
137	`while (read_adc()>128);`	138	`while (read_adc()>TRESHOLD);`
138	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	139	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
139	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	140	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
140	`set_pwm1_duty(0); // reverz (zabrzdi)`	141	`set_pwm1_duty(0); // reverz (zabrzdi)`
141	`set_pwm2_duty(255);`	142	`set_pwm2_duty(255);`
142	`output_low(MOT_L);`	143	`output_low(MOT_L);`
143	`output_low(MOT_R);`	144	`output_low(MOT_R);`
144	`SetServo(CASAVR-CASMIN); // doprostred`	145	`SetServo(CASAVR-CASMIN); // doprostred`
145	`delay_ms(100);`	146	`delay_ms(100);`
146	`brzda();`	147	`brzda();`
147		148
148	`set_pwm1_duty(255); // max. vpred`	149	`set_pwm1_duty(255); // max. vpred`
149	`set_pwm2_duty(255);`	150	`set_pwm2_duty(255);`
150	`output_low(MOT_L);`	151	`output_low(MOT_L);`
151	`output_low(MOT_R);`	152	`output_low(MOT_R);`
152	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	153	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
153	`while(get_timer1()<(odocounter+2)) // Ujed kousek`	154	`while(get_timer1()<(odocounter+2)) // Ujed kousek`
154	`{`	155	`{`
155	`set_adc_channel(LMAX); // Levy UV sensor`	156	`set_adc_channel(LMAX); // Levy UV sensor`
156	`delay_us(40);`	157	`delay_us(40);`
157	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	158	`if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`
158	`set_adc_channel(RMAX); // Pravy UV sensor`	159	`set_adc_channel(RMAX); // Pravy UV sensor`
159	`delay_us(40);`	160	`delay_us(40);`
160	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	161	`if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
161	`cas=CASAVR-CASMIN; // Cara je rovne`	162	`cas=CASAVR-CASMIN; // Cara je rovne`
162	`};`	163	`};`
163		164
164	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`	165	`stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`
165	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	166	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
166	`//rr=60; // Nerozumna rychlost pro rozjeti`	167	`//rr=60; // Nerozumna rychlost pro rozjeti`

Subversion Repositories svnkaklik

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