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

Subversion Repositories svnkaklik

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