Subversion Repositories svnkaklik

Rev

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 
    SetPower (MLEFT,OUT_FULL);
21 
    SetPower (MRIGHT,OUT_FULL);
22 
    OnRev(MLEFT);
23 
    OnFwd(MRIGHT);
24 
    derivace=5;
25 
    movement=S;
26 
}
27 
 
28 
void Left()    // doleva
29 
{
30 
    SetPower (MLEFT,OUT_LOW);
31 
    SetPower (MRIGHT,OUT_FULL);
32 
    Off(MLEFT);
33 
    OnFwd(MRIGHT);
34 
    derivace=3;
35 
    movement=L;
36 
}
37 
 
38 
void Right()   // doprava
39 
{
40 
    SetPower (MLEFT,OUT_FULL);
41 
    SetPower (MRIGHT,OUT_LOW);
42 
    OnRev(MLEFT);
43 
    Off(MRIGHT);
44 
    derivace=3;
45 
    movement=R;
46 
}
47 
 
48 
task main()
49 
{
50 
  PlaySound (SOUND_DOUBLE_BEEP);
51 
  Wait(100); // 1s
52 
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
53 
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
54 
  // sensor na prekazku
55 
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
56 
  SetTxPower(TX_POWER_HI);    // aby daleko videl
57 
 
58 
  movement=S;
59 
  line=S;
60 
  dira=0;
61 
 
62 
//  start cara;
63 
  start cihla;
64 
 
65 
  Straight();
66 
  while(true)
67 
  {
68 
       if (THRESHOLD < RSENSOR)
69 
       {
70 
           line=R;
71 
           dira=0;
72 
           Right();
73 
           if (THRESHOLD < RSENSOR)
74 
           {
75 
             while(THRESHOLD < RSENSOR);
76 
           }
77 
           else
78 
 
79 
       };
80 
       if (THRESHOLD < LSENSOR)
81 
       {
82 
         if (L==movement)
83 
         {
84 
            Straight();
85 
         }
86 
         else
87 
         {
88 
           line=L;
89 
           dira=0;
90 
           Left();
91 
         };
92 
       };
93 
  }
94 
}
95 
 
96 
 
97 
task cara()
98 
{
99 
Left();
100 
 while(true)
101 
 {
102 
          if (8 < dira++)
103 
          {
104 
            PlaySound(SOUND_DOUBLE_BEEP);
105 
            if (L==line) line=R; else line=L;
106 
            Off(MLEFT);
107 
            Off(MRIGHT);
108 
            Wait(500);
109 
            dira=0;
110 
          };
111 
 
112 
   switch(line)
113 
   {
114 
      case S:
115 
          break;
116 
      case L:
117 
            Left();
118 
          break;
119 
 
120 
      case R:
121 
            Right();
122 
          break;
123 
   };
124 
//   line=S;
125 
   Wait(5);
126 
   Straight();
127 
   Wait(5);
128 
 }
129 
}
130 
 
131 
task cihla() // dalkovy sensor
132 
{
133 
  int lastlevel;
134 
 
135 
  lastlevel = 0;
136 
  while(true)
137 
  {
138 
    SendMessage(0);
139 
    if(lastlevel > BUMPER)
140 
    {
141 
      PlaySound(SOUND_FAST_UP);
142 
      stop cara;
143 
      Off(MLEFT);
144 
      Off(MRIGHT);
145 
      Wait(300);
146 
      start cara;
147 
    }
148 
    lastlevel = BUMPER;
149 
    lastlevel -= FTHRESHOLD;
150 
  }
151 
}
152 
 
153