Subversion Repositories svnkaklik

Rev

Go to most recent revision | 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
}*/