Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1 
#include "D:\@KAKLIK\programy\PIC_C\robot\tank\main.h"
2 
 
3 
//motory
4 
#define FR         output_high(PIN_B4);output_low(PIN_B5)  // Vpred
5 
#define FL         output_high(PIN_B6);output_low(PIN_B7)
6 
#define BR         output_high(PIN_B5);output_low(PIN_B4)  // Vzad
7 
#define BL         output_high(PIN_B7);output_low(PIN_B6)
8 
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)
9 
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
10 
 
11 
//cidla
12 
#define RSENSOR    1   // Senzory na caru
13 
#define LSENSOR    0
14 
//#define BUMPER     SENSOR_2   // Senzor na cihlu
15 
 
16 
//human interface
17 
#DEFINE LED        PIN_A2
18 
#DEFINE SOUND_HI   PIN_B2
19 
#DEFINE SOUND_LO   PIN_B3
20 
#include "sound.c"
21 
 
22 
//program
23 
#define MEZERA     38         // jak se muze jet dlouho bez cary
24 
#define COUVEJ     700         // kolik se ma couvat po detekci diry
25 
#define CIKCAK     40         // kolik se ma jet cik/cak
26 
#define PRES_DIRU  350         // jak predpokladame velkou diru
27 
#define TRESHOLD   50
28 
 
29 
#define L 0  // left
30 
#define R 1  // right
31 
#define S 2  // straight
32 
 
33 
int movement;     // smer minuleho pohybu
34 
int line;         // na ktere strane byla detekovana cara
35 
int dira;         // pocitadlo pro nalezeni preruseni cary
36 
int n;            // pomocna promena pro cyklus FOR
37 
 
38 
void cik_cak()   // Hledani cary
39 
{
40 
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
41 
  switch(movement)  // zmenime smer zataceni
42 
  {
43 
   case L:
44 
        FL;STOPR;          // doprava
45 
        movement=R;        // poznamenej kam jedem
46 
        line=L;            // kdyz prejedem, tak bude cara vlevo
47 
     break;
48 
   case R:
49 
        FR;STOPL;           // doleva
50 
        movement=L;        // poznamenej kam jedem
51 
        line=R;            // kdyz prejedem, tak bude cara vpravo
52 
     break;
53 
  }
54 
 
55 
  while(true)   // jed cik-cak, dokud nenajdes caru
56 
  {
57 
    set_adc_channel(RSENSOR);
58 
    if (TRESHOLD > read_adc())   // je cara ?
59 
    {
60 
         STOPL;
61 
         STOPR;
62 
         break;
63 
    };
64 
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
65 
    {
66 
       n=0;
67 
       STOPL;
68 
       STOPR;
69 
       switch(movement)     // zmenime smer zataceni
70 
       {
71 
         case L:
72 
              FL;          // doprava
73 
              movement=R;        // poznamenej kam jedem
74 
              line=L;            // kdyz prejedem, tak bude cara vlevo
75 
           break;
76 
         case R:
77 
              FR;           // doleva
78 
              movement=L;        // poznamenej kam jedem
79 
              line=R;            // kdyz prejedem, tak bude cara vpravo
80 
           break;
81 
       }
82 
    }
83 
  }
84 
}
85 
void diagnostika()
86 
{
87 
   STOPL;                     //zastav vse
88 
   STOPR;
89 
 
90 
   FR;                        //pravy pas
91 
   Delay_ms(1000);
92 
   STOPR;
93 
   Delay_ms(1000);
94 
   BR;
95 
   Delay_ms(1000);
96 
   STOPR;
97 
   Delay_ms(1000);
98 
 
99 
   FL;                        //levy pas
100 
   Delay_ms(1000);
101 
   STOPL;
102 
   Delay_ms(1000);
103 
   BL;
104 
   Delay_ms(1000);
105 
   STOPL;
106 
   Delay_ms(1000);
107 
 
108 
   FL;                        //oba pasy
109 
   FR;
110 
   Delay_ms(1000);
111 
   STOPL;
112 
   STOPR;
113 
   Delay_ms(1000);
114 
   BL;
115 
   BR;
116 
   Delay_ms(1000);
117 
   STOPL;
118 
   STOPR;
119 
   Delay_ms(1000);
120 
 
121 
}
122 
 
123 
void main()
124 
{
125 
  SoundBeep(640,200);                        //beep
126 
  Delay_ms(50);
127 
  SoundBeep(640,200);
128 
 
129 
  setup_adc_ports(RA0_RA1_RA3_ANALOG);
130 
  setup_adc(ADC_CLOCK_DIV_2);
131 
  Delay_ms(1000); // 1s
132 
 
133 
  movement=R;
134 
  line=S;
135 
  dira=0;
136 
 
137 
  //cik_cak();    // toc se, abys nasel caru
138 
  FL; FR;         // vpred
139 
 
140 
                                                   // Sledovani cary
141 
  while(true)
142 
  {
143 
snimani:
144 
    set_adc_channel(RSENSOR);
145 
    Delay_us(10);
146 
    if (TRESHOLD > read_adc())    // Cara pod pravym senzorem
147 
    {
148 
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
149 
      line=R;    // zaznamenej, kdes videl caru
150 
      FL;FR;
151 
      goto snimani;
152 
    }
153 
 
154 
    set_adc_channel(LSENSOR);
155 
    Delay_us(10);
156 
    if (TRESHOLD > read_adc())    // Cara pod levym senzorem
157 
    {
158 
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
159 
      line=L;    // zaznamenej, kdes videl caru
160 
      FL;FR;
161 
      goto snimani;
162 
    }
163 
 
164 
    // oba senzory mimo caru
165 
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
166 
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
167 
    {
168 
      STOPL;
169 
      STOPR;
170 
      Delay_ms(100);
171 
                           // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
172 
      if(line==L)
173 
      {
174 
        // doleva
175 
        FR;
176 
        movement=L;    // poznamenej kam jedes
177 
      }
178 
      if(line==R)
179 
      {
180 
        // doprava
181 
        FL;
182 
        movement=R;    // poznamenej kam jedes
183 
      }
184 
      dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
185 
      goto snimani; // co nejrychleji se vrat na snimani cary
186 
    }
187 
 
188 
    /*if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
189 
    {
190 
      dira=0;
191 
      switch(movement)  // kam se jelo ?
192 
      {
193 
       case L:
194 
            BR;      // couvej doprava
195 
            Delay_ms(COUVEJ);
196 
            movement=R;
197 
         break;
198 
       case R:
199 
            BL;       // couvej doleva
200 
            Delay_ms(COUVEJ);
201 
            movement=L;
202 
         break;
203 
      }
204 
      STOP();                    // konec couvani
205 
      FL; FR;                  // rovne pres diru
206 
      Delay_ms(PRES_DIRU);
207 
      STOP();
208 
      cik_cak();                // najdi caru
209 
      FR; FL;                  // vpred
210 
    } // dira*/
211 
 
212 
  } // while(true)
213 
 
214 
}
215 
 
216 
/*void objizdka()
217 
{
218 
  BL; BR;                 // zacouvej
219 
  Delay_ms(150);
220 
  STOP();
221 
 
222 
  cik_cak();               // vyrovnej se na caru
223 
  cik_cak();               // radeji 3x, aby se vyrovnaly setrvacne sily
224 
  cik_cak();
225 
 
226 
  BL; FR; Delay_ms(220);               // vlevo
227 
  STOP();
228 
  FL; FR; Delay_ms(650);               // rovne
229 
  STOP();
230 
  FL; BR; Delay_ms(220);               // vpravo
231 
  STOP();
232 
  FL; FR; Delay_ms(770);               // rovne
233 
  STOP();
234 
  FL; BR; Delay_ms(210);               // vpravo
235 
  STOP();
236 
  FL; FR;
237 
  Delay_ms(200);                       // kousek rovne
238 
  // rovne, dokud nenarazis na caru
239 
  while(input(RSENSOR));     // pravej senzor, protoze cihla je vpravo
240 
  STOP();
241 
  BL; FR;                         // toc se doleva, dokud nenarazis na caru
242 
  while(input(LSENSOR));     // levej senzor, protoze cara je urcite vlevo
243 
  STOP();
244 
  line=R;      // caru predpokladame vpravo, kdybysme ji prejeli
245 
  dira=0;
246 
  FL; FR;                         // vpred
247 
}*/