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 ML         OUT_A   // Motory
4 
#define MR         OUT_C
5 
#define FL         OnFwd(ML)  // Vpred
6 
#define FR         OnRev(MR)
7 
#define BL         OnRev(ML)  // Vzad
8 
#define BR         OnFwd(MR)
9 
#define RSENSOR    SENSOR_3   // Sensory na caru
10 
#define LSENSOR    SENSOR_1
11 
#define BUMPER     SENSOR_2   // Dalkovy sensor
12 
 
13 
#define L 0  // left
14 
#define R 1  // right
15 
#define S 2  // straight
16 
 
17 
int movement;     // smer minuleho pohybu
18 
int line;         // na ktere strane byla detekovana cara
19 
int dira;         // pocitadlo pro nalezeni preruseni cary
20 
 
21 
void Straight()   // rovne
22 
{
23 
    FL;
24 
    FR;
25 
    movement=S;
26 
}
27 
 
28 
void Left()    // doleva
29 
{
30 
    Off(ML);
31 
    FR;
32 
    movement=L;
33 
}
34 
 
35 
void Right()   // doprava
36 
{
37 
    FL;
38 
    Off(MR);
39 
    movement=R;
40 
}
41 
 
42 
task main()
43 
{
44 
  PlaySound (SOUND_DOUBLE_BEEP);
45 
  Wait(100); // 1s
46 
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
47 
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
48 
  // sensor na prekazku
49 
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
50 
  SetTxPower(TX_POWER_HI);    // aby daleko videl
51 
 
52 
  SetPower (ML,OUT_FULL);     // vykon motoru
53 
  SetPower (MR,OUT_FULL);
54 
  movement=S;
55 
  line=S;
56 
  dira=0;
57 
 
58 
  start cara;
59 
  start cihla;
60 
}
61 
 
62 
task cara()              // sledovani cary
63 
{
64 
  Straight();
65 
  while(true)
66 
  {
67 
       if (THRESHOLD < RSENSOR)    // Cara vpravo
68 
       {
69 
          line=R;    // cara zrejme bude na druhe strane
70 
          dira=0;    // nuluj pocitadlo diry
71 
          switch(movement)
72 
          {
73 
            case L:
74 
               Right();
75 
              break;
76 
            default:
77 
              Straight();
78 
          };
79 
       }
80 
       else
81 
       if (THRESHOLD < LSENSOR)    // Cara vlevo
82 
       {
83 
          line=L;    // zaznamenej, kdes videl caru
84 
          dira=0;    // nuluj pocitadlo diry
85 
          switch(movement)
86 
          {
87 
            case R:
88 
               Left();
89 
              break;
90 
            default:
91 
              Straight();
92 
          };
93 
       }
94 
       else   // sensory mimo caru
95 
       {
96 
         if (20 < dira++)  // nejedeme uz moc dlouho bez cary?
97 
         {
98 
           PlaySound (SOUND_FAST_UP);
99 
           switch(movement)
100 
           {
101 
             case L:
102 
                line=R;   // cara zrejme bude na druhe strane
103 
                Off(ML);  //couvej
104 
                BR;
105 
               break;
106 
             case R:
107 
                line=L;   // cara zrejme bude na druhe strane
108 
                BL;       // couvej
109 
                Off(MR);
110 
               break;
111 
           };
112 
           Wait(50);   // zpet, ale ne uplne a kousek rovne
113 
           Straight();
114 
           Wait(5);
115 
           dira=0;    // myslime si, ze diru jsme vyresili
116 
         };
117 
 
118 
         switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
119 
         {
120 
           case L:
121 
                Left();
122 
             break;
123 
           case R:
124 
                Right();
125 
             break;
126 
         };
127 
       }
128 
  } // while(true)
129 
}
130 
 
131 
 
132 
task cihla() // dalkovy sensor
133 
{
134 
  int lastlevel;
135 
 
136 
  lastlevel = 0;
137 
  while(true)
138 
  {
139 
    SendMessage(0);
140 
    if(lastlevel > BUMPER)
141 
    {
142 
      PlaySound(SOUND_DOWN);
143 
      stop cara;
144 
      Off(ML);           // zatim pouze zastav
145 
      Off(MR);
146 
      Wait(300);
147 
      start cara;
148 
    }
149 
    lastlevel = BUMPER;
150 
    lastlevel -= FTHRESHOLD;
151 
  }
152 
}
153 
 
154