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