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

Subversion Repositories svnkaklik

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