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=5;
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=5;
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 
  while(true)
66 
  {
67 
       if (THRESHOLD < RSENSOR) {line=R; dira=0;};
68 
       if (THRESHOLD < LSENSOR) {line=L; dira=0;};
69 
  }
70 
}
71 
 
72 
 
73 
task cara()
74 
{
75 
Left();
76 
 while(true)
77 
 {
78 
   switch(line)
79 
   {
80 
      case S:
81 
          if (3 < dira++)
82 
          {
83 
            PlaySound(SOUND_DOUBLE_BEEP);
84 
          };
85 
          break;
86 
      case L:
87 
          switch(movement)
88 
          {
89 
//            case S:
90 
            case R:
91 
                    Left();
92 
                 break;
93 
            case L:
94 
                    Right();
95 
                 break;
96 
          };
97 
          break;
98 
 
99 
      case R:
100 
          switch(movement)
101 
          {
102 
//            case S:
103 
            case L:
104 
                    Right();
105 
                 break;
106 
            case R:
107 
                    Left();
108 
                 break;
109 
          };
110 
          break;
111 
   };
112 
   line=S;
113 
   Wait(10);
114 
 }
115 
}
116 
 
117 
task cihla() // dalkovy sensor
118 
{
119 
  int lastlevel;
120 
 
121 
  lastlevel = 0;
122 
  while(true)
123 
  {
124 
    SendMessage(0);
125 
    if(lastlevel > BUMPER)
126 
    {
127 
      PlaySound(SOUND_FAST_UP);
128 
      stop cara;
129 
      Off(MLEFT);
130 
      Off(MRIGHT);
131 
      Wait(300);
132 
      start cara;
133 
    }
134 
    lastlevel = BUMPER;
135 
    lastlevel -= FTHRESHOLD;
136 
  }
137 
}
138 
 
139