Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
1 kaklik 1 
#define THRESHOLD  44     // rozhodovaci uroven mezi cernou a bilou
2 
#define FTHRESHOLD 100    // Rozhodovaci uroven dalkoveho sensoru
3 
#define MLEFT     OUT_C   // Motory
4 
#define MRIGHT    OUT_A
5 
#define RSENSOR   SENSOR_1   // Sensory na caru
6 
#define LSENSOR   SENSOR_3
7 
#define BUMPER    SENSOR_2   // Dalkovy sensor
8 
 
9 
#define L 0  // left
10 
#define R 1  // right
11 
#define S 2  // straight
12 
 
13 
int movement;     // smer minuleho pohybu
14 
int line;         // na ktere strane byla detekovana cara
15 
int dira;         // pocitadlo pro nalezeni preruseni cary
16 
int derivace;     // jak moc se bude zachovavat predesly smer
17 
 
18 
void Straight()   // rovne
19 
{
20 
    OnRev(MLEFT);
21 
    OnFwd(MRIGHT);
22 
    movement=S;
23 
}
24 
 
25 
void Left()    // doleva
26 
{
27 
    Off(MLEFT);
28 
    OnFwd(MRIGHT);
29 
    movement=L;
30 
}
31 
 
32 
void Right()   // doprava
33 
{
34 
    OnRev(MLEFT);
35 
    Off(MRIGHT);
36 
    movement=R;
37 
}
38 
 
39 
task main()
40 
{
41 
  PlaySound (SOUND_DOUBLE_BEEP);
42 
  Wait(100); // 1s
43 
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
44 
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
45 
  // sensor na prekazku
46 
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
47 
  SetTxPower(TX_POWER_HI);    // aby daleko videl
48 
 
49 
  movement=S;
50 
  line=S;
51 
  dira=0;
52 
 
53 
  start cara;
54 
  start cihla;
55 
  SetPower (MLEFT,OUT_FULL);
56 
  SetPower (MRIGHT,OUT_FULL);
57 
}
58 
 
59 
task cara()
60 
{
61 
  Straight();
62 
  while(true)
63 
  {
64 
       if (THRESHOLD < RSENSOR)
65 
       {
66 
          line=R;
67 
          dira=0;
68 
          switch(movement)
69 
          {
70 
            case L:
71 
               Right();
72 
              break;
73 
            default:
74 
              Straight();
75 
          };
76 
       }
77 
       else
78 
       if (THRESHOLD < LSENSOR)
79 
       {
80 
          line=L;
81 
          dira=0;
82 
          switch(movement)
83 
          {
84 
            case R:
85 
               Left();
86 
              break;
87 
            default:
88 
              Straight();
89 
          };
90 
       }
91 
       else
92 
       {
93 
       if (20 < dira++)
94 
       {
95 
         PlaySound (SOUND_FAST_UP);
96 
         switch(movement)
97 
         {
98 
            case L:
99 
               line=R;
100 
               Off(MLEFT);     //couvej
101 
               OnRev(MRIGHT);
102 
              break;
103 
            case R:
104 
               line=L;
105 
               OnFwd(MLEFT);    // couvej
106 
               Off(MRIGHT);
107 
             break;
108 
         };
109 
         Wait(50);
110 
         Straight();
111 
         Wait(5);
112 
         dira=0;
113 
       };
114 
 
115 
       switch(line)
116 
       {
117 
          case L:
118 
               Left();
119 
            break;
120 
          case R:
121 
               Right();
122 
            break;
123 
         };
124 
       }
125 
  } // while(true)
126 
}
127 
 
128 
 
129 
task cihla() // dalkovy sensor
130 
{
131 
  int lastlevel;
132 
 
133 
  lastlevel = 0;
134 
  while(true)
135 
  {
136 
    SendMessage(0);
137 
    if(lastlevel > BUMPER)
138 
    {
139 
      PlaySound(SOUND_DOWN);
140 
      stop cara;
141 
      Off(MLEFT);           // stop
142 
      Off(MRIGHT);
143 
      Wait(5);
144 
      OnRev(MLEFT);           // do leva
145 
      OnRev(MRIGHT);
146 
      Wait(30);
147 
      Off(MLEFT);           // stop
148 
      Off(MRIGHT);
149 
      Wait(5);
150 
      OnRev(MLEFT);           // rovne
151 
      OnFwd(MRIGHT);
152 
      Wait(70);
153 
      Off(MLEFT);           // stop
154 
      Off(MRIGHT);
155 
      Wait(5);
156 
      OnFwd(MLEFT);           // do prava
157 
      OnFwd(MRIGHT);
158 
      Wait(25);
159 
      Off(MLEFT);           // stop
160 
      Off(MRIGHT);
161 
      Wait(5);
162 
      OnRev(MLEFT);           // rovne
163 
      OnFwd(MRIGHT);
164 
      Wait(80);
165 
      Off(MLEFT);           // stop
166 
      Off(MRIGHT);
167 
      Wait(5);
168 
      OnFwd(MLEFT);           // do prava
169 
      OnFwd(MRIGHT);
170 
      Wait(25);
171 
      Off(MLEFT);           // stop
172 
      Off(MRIGHT);
173 
      Wait(5);
174 
      OnRev(MLEFT);           // rovne
175 
      OnFwd(MRIGHT);
176 
      Wait(60);
177 
      OnRev(MLEFT);           // do leva
178 
      OnRev(MRIGHT);
179 
      Wait(20);
180 
      while(THRESHOLD > RSENSOR); // toc se dokud neni cara
181 
      Off(MLEFT);           // stop
182 
      Off(MRIGHT);
183 
      Wait(30);
184 
      line=R;
185 
      start cara;
186 
    }
187 
    lastlevel = BUMPER;
188 
    lastlevel -= FTHRESHOLD;
189 
  }
190 
}
191 
 
192