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



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
while(get_timer1()<(odocounter+5)); // Popojed definovanou vzdalenost
 
disp(2);
SetServoQ((CASAVR-CASMIN)-20);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
 
set_pwm1_duty(120);
set_pwm2_duty(140);
 
disp(3);
SetServoQ((CASAVR-CASMIN)-30);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(100);
 
disp(4);
SetServoQ((CASAVR-CASMIN)-60);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(80);
 
disp(5);
SetServoQ((CASAVR-CASMIN)-40);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(80);
 
disp(6);
SetServoQ((CASAVR-CASMIN)-70);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(80);
 
disp(7);
SetServoQ((CASAVR-CASMIN)-60);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(100);
 
disp(8);
SetServoQ((CASAVR-CASMIN)-30);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(120);
 
disp(9);
SetServoQ((CASAVR-CASMIN));		// Rovne
set_pwm1_duty(80);  // vpred
set_pwm2_duty(80);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenos

output_high(MOT_L);
output_high(MOT_R);
delay_ms(300);
brzda();
 
disp(10);
SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(0);   // vzad
set_pwm2_duty(10);
output_low(MOT_L);
output_high(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost
{
   if(read_adc()<128) break; // Neprejeli jsme caru?
};
 
disp(11);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly
set_pwm1_duty(0);    // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
delay_ms(200);
brzda();
 
disp(12);
SetServo((CASAVR-CASMIN));   // rovne              S
set_pwm1_duty(255);  // Rozjezd na plny vykon vpred
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);

   delay_us(40);
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
   cas=CASAVR-CASMIN;    // Cara je rovne
};
 
disp(13);
//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 204	Rev 207
Line 14...	Line 14...
14	`output_low(MOT_L);`	14	`output_low(MOT_L);`
15	`output_low(MOT_R);`	15	`output_low(MOT_R);`
16	`delay_ms(215);`	16	`delay_ms(215);`
17	`brzda();`	17	`brzda();`
18		18
-		19	`disp(1);`
-		20
19	`SetServo((CASAVR-CASMIN)); // rovne S`	21	`SetServo((CASAVR-CASMIN)); // rovne S`
20	`set_pwm1_duty(160); // vpred`	22	`set_pwm1_duty(160); // vpred`
21	`set_pwm2_duty(160);`	23	`set_pwm2_duty(160);`
22	`output_low(MOT_L);`	24	`output_low(MOT_L);`
23	`output_low(MOT_R);`	25	`output_low(MOT_R);`
24	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	26	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
25	`while(get_timer1()<(odocounter+5)); // Popojed definovanou vzdalenost`	27	`while(get_timer1()<(odocounter+5)); // Popojed definovanou vzdalenost`
26		28
-		29	`disp(2);`
27	`SetServoQ((CASAVR-CASMIN)-20);`	30	`SetServoQ((CASAVR-CASMIN)-20);`
28	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	31	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
29	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	32	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
30	`set_pwm1_duty(140);`	-
31	`set_pwm1_duty(120);`	33	`set_pwm1_duty(120);`
-		34	`set_pwm2_duty(140);`
32		35
-		36	`disp(3);`
33	`SetServoQ((CASAVR-CASMIN)-30);`	37	`SetServoQ((CASAVR-CASMIN)-30);`
34	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	38	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
35	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	39	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
36	`set_pwm1_duty(100);`	40	`set_pwm1_duty(100);`
37		41
-		42	`disp(4);`
38	`SetServoQ((CASAVR-CASMIN)-60);`	43	`SetServoQ((CASAVR-CASMIN)-60);`
39	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	44	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
40	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	45	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
41	`set_pwm1_duty(80);`	46	`set_pwm1_duty(80);`
42		47
-		48	`disp(5);`
43	`SetServoQ((CASAVR-CASMIN)-40);`	49	`SetServoQ((CASAVR-CASMIN)-40);`
44	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	50	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
45	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	51	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
46	`set_pwm1_duty(80);`	52	`set_pwm1_duty(80);`
47		53
-		54	`disp(6);`
48	`SetServoQ((CASAVR-CASMIN)-70);`	55	`SetServoQ((CASAVR-CASMIN)-70);`
49	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	56	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
50	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	57	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
51	`set_pwm1_duty(80);`	58	`set_pwm1_duty(80);`
52		59
-		60	`disp(7);`
53	`SetServoQ((CASAVR-CASMIN)-60);`	61	`SetServoQ((CASAVR-CASMIN)-60);`
54	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	62	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
55	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	63	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
56	`set_pwm1_duty(100);`	64	`set_pwm1_duty(100);`
57		65
-		66	`disp(8);`
58	`SetServoQ((CASAVR-CASMIN)-30);`	67	`SetServoQ((CASAVR-CASMIN)-30);`
59	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	68	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
60	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	69	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
61	`set_pwm1_duty(120);`	70	`set_pwm1_duty(120);`
62		71
-		72	`disp(9);`
63	`SetServoQ((CASAVR-CASMIN)); // Rovne`	73	`SetServoQ((CASAVR-CASMIN)); // Rovne`
64	`set_pwm1_duty(80); // vpred`	74	`set_pwm1_duty(80); // vpred`
65	`set_pwm2_duty(80);`	75	`set_pwm2_duty(80);`
66	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	76	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
67	`while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenos`	77	`while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenos`
Line 73...	Line 83...
73	`output_high(MOT_L);`	83	`output_high(MOT_L);`
74	`output_high(MOT_R);`	84	`output_high(MOT_R);`
75	`delay_ms(300);`	85	`delay_ms(300);`
76	`brzda();`	86	`brzda();`
77		87
-		88	`disp(10);`
78	`SetServo(CASMIN); // max. doleva L`	89	`SetServo(CASMIN); // max. doleva L`
79	`set_pwm1_duty(0); // vzad`	90	`set_pwm1_duty(0); // vzad`
80	`set_pwm2_duty(10);`	91	`set_pwm2_duty(10);`
81	`output_low(MOT_L);`	92	`output_low(MOT_L);`
82	`output_high(MOT_R);`	93	`output_high(MOT_R);`
83	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	94	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
84	`while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost`	95	`while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost`
85	`{`	96	`{`
86	`if(read_adc()<128) break; // Neprejeli jsme caru?`	97	`if(read_adc()<128) break; // Neprejeli jsme caru?`
87	`};`	98	`};`
-		99
-		100	`disp(11);`
88	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	101	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
89	`while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly`	102	`while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly`
90	`set_pwm1_duty(0); // reverz (zabrzdi)`	103	`set_pwm1_duty(0); // reverz (zabrzdi)`
91	`set_pwm2_duty(255);`	104	`set_pwm2_duty(255);`
92	`output_low(MOT_L);`	105	`output_low(MOT_L);`
93	`output_low(MOT_R);`	106	`output_low(MOT_R);`
94	`delay_ms(200);`	107	`delay_ms(200);`
95	`brzda();`	108	`brzda();`
96		109
-		110	`disp(12);`
97	`SetServo((CASAVR-CASMIN)); // rovne S`	111	`SetServo((CASAVR-CASMIN)); // rovne S`
98	`set_pwm1_duty(255); // Rozjezd na plny vykon vpred`	112	`set_pwm1_duty(255); // Rozjezd na plny vykon vpred`
99	`set_pwm2_duty(255);`	113	`set_pwm2_duty(255);`
100	`output_low(MOT_L);`	114	`output_low(MOT_L);`
101	`output_low(MOT_R);`	115	`output_low(MOT_R);`
Line 109...	Line 123...
109	`delay_us(40);`	123	`delay_us(40);`
110	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	124	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
111	`cas=CASAVR-CASMIN; // Cara je rovne`	125	`cas=CASAVR-CASMIN; // Cara je rovne`
112	`};`	126	`};`
113		127
-		128	`disp(13);`
114	`stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`	129	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`
115	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	130	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
116	`rr=60; // Nerozumna rychlost pro rozjeti`	131	`rr=60; // Nerozumna rychlost pro rozjeti`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_R.c @ 210 – Rev 204 → 207