WebSVN – svnkaklik – Diff – /roboti/istrobot/callis/Kopie (2) - callis_1.nqc


#define THRESHOLD  44     // rozhodovaci uroven mezi cernou a bilou
#define FTHRESHOLD 100    // Rozhodovaci uroven dalkoveho sensoru
#define MLEFT     OUT_C   // Motory
#define MRIGHT    OUT_A
#define RSENSOR   SENSOR_1   // Sensory na caru
#define LSENSOR   SENSOR_3
#define BUMPER    SENSOR_2   // Dalkovy sensor
 
#define L 0  // left
#define R 1  // right
#define S 2  // straight
 
int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
int dira;         // pocitadlo pro nalezeni preruseni cary
int derivace;     // jak moc se bude zachovavat predesly smer
 
void Straight()   // rovne
{
    OnRev(MLEFT);
    OnFwd(MRIGHT);
    movement=S;
}
 
void Left()    // doleva
{
    Off(MLEFT);
    OnFwd(MRIGHT);
    movement=L;
}
 
void Right()   // doprava
{
    OnRev(MLEFT);
    Off(MRIGHT);
    movement=R;
}
 
task main()
{
  PlaySound (SOUND_DOUBLE_BEEP);
  Wait(100); // 1s
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
  // sensor na prekazku
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
  SetTxPower(TX_POWER_HI);    // aby daleko videl
 
  movement=S;
  line=S;
  dira=0;
 
  start cara;
  start cihla;
  SetPower (MLEFT,OUT_FULL);
  SetPower (MRIGHT,OUT_FULL);
}
 
task cara()
{
  Straight();
  while(true)
  {
       if (THRESHOLD < RSENSOR)
       {
          line=R;
          dira=0;
          switch(movement)
          {
            case L:
               Right();
              break;
            default:
              Straight();
          };
       }
       else
       if (THRESHOLD < LSENSOR)
       {
          line=L;
          dira=0;
          switch(movement)
          {
            case R:
               Left();
              break;
            default:
              Straight();
          };
       }
       else
       {
       if (20 < dira++)
       {
         PlaySound (SOUND_FAST_UP);
         switch(movement)
         {
            case L:
               line=R;
               Off(MLEFT);     //couvej
               OnRev(MRIGHT);
              break;
            case R:
               line=L;
               OnFwd(MLEFT);    // couvej
               Off(MRIGHT);
             break;
         };
         Wait(50);
         Straight();
         Wait(5);
         dira=0;
       };
 
       switch(line)
       {
          case L:
               Left();
            break;
          case R:
               Right();
            break;
         };
       }
  } // while(true)
}
 
 
task cihla() // dalkovy sensor
{
  int lastlevel;
 
  lastlevel = 0;
  while(true)
  {
    SendMessage(0);
    if(lastlevel > BUMPER)
    {
      PlaySound(SOUND_DOWN);
      stop cara;
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // do leva
      OnRev(MRIGHT);
      Wait(30);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(70);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnFwd(MLEFT);           // do prava
      OnFwd(MRIGHT);
      Wait(25);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(80);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnFwd(MLEFT);           // do prava
      OnFwd(MRIGHT);
      Wait(25);
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(5);
      OnRev(MLEFT);           // rovne
      OnFwd(MRIGHT);
      Wait(60);
      OnRev(MLEFT);           // do leva
      OnRev(MRIGHT);
      Wait(20);
      while(THRESHOLD > RSENSOR); // toc se dokud neni cara
      Off(MLEFT);           // stop
      Off(MRIGHT);
      Wait(30);
      line=R;
      start cara;
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}
 
 

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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/callis/Kopie (2) - callis_1.nqc @ 260 – Rev 1 → 5