Subversion Repositories svnkaklik

Rev

Rev 222 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 222 Rev 223
Line 2... Line 2...
2
 
2
 
3
int8 n;
3
int8 n;
4
int8 r1,r2,rr;
4
int8 r1,r2,rr;
5
int1 flag;
5
int1 flag;
6
 
6
 
7
SetServo(CASMIN-3);   // max. doleva, zatoc kolmo na caru
7
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
8
set_pwm1_duty(0);
8
set_pwm1_duty(0);
9
set_pwm2_duty(140);
9
set_pwm2_duty(140);
10
output_low(MOT_L);    // vpred
10
output_low(MOT_L);    // vpred
11
output_low(MOT_R);
11
output_low(MOT_R);
12
disp(1);
12
disp(0x81);
13
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
13
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
14
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
14
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
15
 
15
 
16
set_pwm1_duty(80);
16
set_pwm1_duty(80);
17
set_pwm2_duty(80);
17
set_pwm2_duty(80);
18
set_adc_channel(LMAX);
18
set_adc_channel(LMAX);
19
disp(2);
19
disp(0x82);
20
cas=CASAVR-CASMIN;
20
cas=CASAVR-CASMIN;
21
odocounter=get_timer1();
21
odocounter=get_timer1();
22
flag=1;
22
flag=1;
23
rr=20;
23
rr=20;
24
while(true)
24
while(true)
Line 49... Line 49...
49
 
49
 
50
      while(input(HREF));
50
      while(input(HREF));
51
   };
51
   };
52
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
52
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
53
   {
53
   {
54
      if(flag==1) {disp(3); flag=0;};
54
      if(flag==1) {disp(0x83); flag=0;};
55
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};};
55
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
56
   };
56
   };
57
};
57
};
58
 
58
 
59
disp(4);
59
disp(0x84);
60
while(read_adc()<THR_L);   // Prejed caru
-
 
61
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
60
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
-
 
61
SetServo(CASAVR);    // Primy smer
62
while((read_adc()<THR_L) || (get_timer1()<(odocounter+1))); // Popojed za caru
62
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru
63
 
63
 
64
SetServo(CASMIN);   // max. doleva                 L
64
SetServo(CASMIN);   // max. doleva                 L
65
set_pwm1_duty(0);
65
set_pwm1_duty(20);
66
set_pwm2_duty(140);
66
set_pwm2_duty(170);
-
 
67
output_high(MOT_L);    // levym kolem brzdi
67
while (read_adc()>THR_L);
68
output_low(MOT_R);
68
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
69
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
-
 
70
while (true)  // Znovu se musime dotknout cary
-
 
71
{
-
 
72
   if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
-
 
73
   {
-
 
74
      while(read_adc()<THR_L); // Prejedeme caru
-
 
75
      cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
-
 
76
      break;
-
 
77
   };
-
 
78
   set_adc_channel(RMAX);    // Pravy UV sensor
-
 
79
   delay_us(40);
69
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
80
   if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
-
 
81
   {
-
 
82
      cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
-
 
83
      break;
-
 
84
   };
-
 
85
   set_adc_channel(LMAX);    // Levy UV sensor
-
 
86
   delay_us(40);
-
 
87
}
70
set_pwm1_duty(0);    // vypni motory
88
set_pwm1_duty(0);    // vypni motory
71
set_pwm2_duty(0);
89
set_pwm2_duty(0);
72
/*
-
 
73
set_pwm1_duty(0);    // reverz (zabrzdi)
-
 
74
set_pwm2_duty(0);
-
 
75
output_low(MOT_L);
-
 
76
output_high(MOT_R);
-
 
77
*/
-
 
78
 
90
 
79
SetServo(CASAVR-CASMIN);   // doprostred
91
SetServo(CASAVR-CASMIN);   // doprostred
80
 
92
 
81
disp(5);
93
disp(0x85);
82
set_pwm1_duty(255);    // max. vpred
94
set_pwm1_duty(255);    // max. vpred
83
set_pwm2_duty(255);
95
set_pwm2_duty(255);
84
output_low(MOT_L);
96
output_low(MOT_L);
85
output_low(MOT_R);
97
output_low(MOT_R);
86
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
98
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
Line 90... Line 102...
90
   delay_us(40);
102
   delay_us(40);
91
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
103
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
92
   set_adc_channel(RMAX);    // Pravy UV sensor
104
   set_adc_channel(RMAX);    // Pravy UV sensor
93
   delay_us(40);
105
   delay_us(40);
94
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
106
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
95
   cas=CASAVR-CASMIN;    // Cara je rovne
107
//   cas=CASAVR-CASMIN;    // Cara je rovne
96
};
108
};