Rev 918 Rev 919
Line 8... Line 8...
8 #define RIGHT 1 8 #define RIGHT 1
9 #define BLACK 0 9 #define BLACK 0
10 #define WHITE 255 10 #define WHITE 255
11   11  
12 // regulator 12 // regulator
13 #define CONP 2 // konstanta pro proporcionalni regulator 13 #define CONP 2 // konstanta pro proporcionalni regulator (2)
14 #define CONI 45 // konstanta pro integracni regulator *0,01 14 #define CONI 45 // konstanta pro integracni regulator *0,01 (45)
15 #define COND 20 // konstanta pro derivacni regulator *0,01 15 #define COND 20 // konstanta pro derivacni regulator *0,01 (20)
16   16  
17 #define SPD_LO 136 // zaklad pro vypocet rychlosti pomalejsiho motoru 17 #define SPD_LO 130 // zaklad pro vypocet rychlosti pomalejsiho motoru (130)
18 #define SPD_HI 156 // zaklad pro vypocetrychlosti rychlejsiho motoru 18 #define SPD_HI 155 // zaklad pro vypocetrychlosti rychlejsiho motoru (155)
19 #define SPD_MAX 230 // rychlost motoru po rovince 19 #define SPD_MAX 240 // rychlost po rovince (240)
20   20  
21 int8 reg_out; 21 int8 reg_out;
22 int8 err1; // odchylka prvni hodnoty 22 int8 err1; // odchylka prvni hodnoty
23 int8 err2; 23 int8 err2;
24 int8 err3; 24 int8 err3;
25 int8 err4; 25 int8 err4;
26 int8 err5; // odchylka posledni hodnoty 26 int8 err5; // odchylka posledni hodnoty
27 int8 errp; // prumer chyb 27 int8 errp; // prumer chyb
28   28  
29 // mezera 29 // mezera
30 #define SPACE 10 30 #define SPACE 8 // jak dlouho robot smi nic nevidet (8)
31   31  
32 // univerzalni LED diody 32 // univerzalni LED diody
33 #define LED1 PIN_E1 33 #define LED1 PIN_E1
34 #define LED2 PIN_E0 34 #define LED2 PIN_E0
35   35  
Line 42... Line 42...
42 // radkovy senzor 42 // radkovy senzor
43 #define SDIN PIN_D4 // seriovy vstup 43 #define SDIN PIN_D4 // seriovy vstup
44 #define SDOUT input(PIN_C5) // seriovy vystup 44 #define SDOUT input(PIN_C5) // seriovy vystup
45 #define SCLK PIN_D5 // takt 45 #define SCLK PIN_D5 // takt
46   46  
47 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary 47 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary (4)
48 #define OLSA_LEV 0x10 // rozhodovaci uroven (cca 10 odpovida cerne) 48 #define OLSA_LEV 0x10 // rozhodovaci uroven (cca 10 odpovida cerne) (0x10 nebo 0x60)
49   49  
50 // pro komunikaci s OLSA, prvni se posila LSB 50 // pro komunikaci s OLSA, prvni se posila LSB
51 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 51 int MAIN_RESET[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
52 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F 52 int SET_MODE_RG[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
53 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00 53 int CLEAR_MODE_RG[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
54   54  
55 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40 55 int LEFT_OFFSET[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
56 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42 56 int MID_OFFSET[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
57 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44 57 int RIGHT_OFFSET[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
58 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81 58 int OFFSET[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
59   59  
60 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41 60 int LEFT_GAIN[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
61 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43 61 int MID_GAIN[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
62 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45 62 int RIGHT_GAIN[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
63 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5 63 int GAIN[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
64   64  
65 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08 65 int START_INT[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
66 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10 66 int STOP_INT[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
67 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02 67 int READOUT[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
68   68  
69 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50) 69 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
70 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 70 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
71 int8 *lp; // ukazatel pro levou polovinu radky 71 int8 *lp; // ukazatel pro levou polovinu radky
72 int8 *rp; // ukazatel pro levou polovinu radky 72 int8 *rp; // ukazatel pro levou polovinu radky
Line 79... Line 79...
79 // ================================= NARAZNIK ================================== 79 // ================================= NARAZNIK ==================================
80 #define BUMPL input(PIN_D6) 80 #define BUMPL input(PIN_D6)
81 #define BUMPR input(PIN_D7) 81 #define BUMPR input(PIN_D7)
82   82  
83 // ============================= NOUZOVE SENZORY =============================== 83 // ============================= NOUZOVE SENZORY ===============================
84 #define LINEL 0 84 #define LINEL 0 // analogovy kanal pro levy senzor
85 #define LINER 1 85 #define LINER 1 // analogovy kanal pro pravy senzor
86 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) 86 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
87 int8 line_l; 87 int8 line_l; // uklada hodnotu leveho senzoru
88 int8 line_r; 88 int8 line_r; // uklada hodnotu praveho senzoru
-   89  
-   90 // ================================ DALKOMER ===================================
-   91 #define SHARP 2 // analogovy kanal pro SHARP
-   92 #define BLOCK 300 // rozhodovaci uroven, kdy je prekazka
-   93  
-   94 int8 sharp_lev; // uklada hodnotu sharp
89   95  
90 // ================================== MOTORY =================================== 96 // ================================== MOTORY ===================================
91 #define LMF PIN_D0 97 #define LMF PIN_D0
92 #define LMB PIN_D1 98 #define LMB PIN_D1
93 #define RMF PIN_D2 99 #define RMF PIN_D2
Line 144... Line 150...
144 output_low(SDIN); 150 output_low(SDIN);
145 output_low(SCLK); 151 output_low(SCLK);
146 olsa_pulses(30); // reset radkoveho senzoru 152 olsa_pulses(30); // reset radkoveho senzoru
147 output_high(SDIN); 153 output_high(SDIN);
148 olsa_pulses(10); // start bit - synchronizace 154 olsa_pulses(10); // start bit - synchronizace
149 olsa_send(main_reset); 155 olsa_send(MAIN_RESET);
150 olsa_pulses(5); 156 olsa_pulses(5);
151 olsa_send(set_mode_rg); 157 olsa_send(SET_MODE_RG);
152 olsa_send(clear_mode_rg); 158 olsa_send(CLEAR_MODE_RG);
153 } 159 }
154 160
155 void olsa_setup() // kompletni nastaveni, provadi se po resetu 161 void olsa_setup() // kompletni nastaveni, provadi se po resetu
156 { 162 {
157 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk) 163 olsa_send(LEFT_OFFSET); // nastaveni leveho segmentu (offset a zisk)
158 olsa_send(offset); 164 olsa_send(OFFSET);
159 olsa_send(left_gain); 165 olsa_send(LEFT_GAIN);
160 olsa_send(gain); 166 olsa_send(GAIN);
161 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk) 167 olsa_send(MID_OFFSET); // nastaveni prostredniho segmentu (offset a zisk)
162 olsa_send(offset); 168 olsa_send(OFFSET);
163 olsa_send(mid_gain); 169 olsa_send(MID_GAIN);
164 olsa_send(gain); 170 olsa_send(GAIN);
165 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk) 171 olsa_send(RIGHT_OFFSET); // nastaveni praveho segmentu (offset a zisk)
166 olsa_send(offset); 172 olsa_send(OFFSET);
167 olsa_send(right_gain); 173 olsa_send(RIGHT_GAIN);
168 olsa_send(gain); 174 olsa_send(GAIN);
169 } 175 }
170 176
171 void olsa_integration() // snimani pixelu 177 void olsa_integration() // snimani pixelu
172 { 178 {
173 olsa_send(start_int); // zacatek integrace senzoru 179 olsa_send(START_INT); // zacatek integrace senzoru
174 olsa_pulses(22); 180 olsa_pulses(22);
175 olsa_send(stop_int); // konec integrace senzoru 181 olsa_send(STOP_INT); // konec integrace senzoru
176 olsa_pulses(5); 182 olsa_pulses(5);
177 } 183 }
178   184  
179 void read_olsa() 185 void read_olsa()
180 { 186 {
Line 183... Line 189...
183 int8 pixel; // hodnota precteneho pixelu 189 int8 pixel; // hodnota precteneho pixelu
184 cpixel=0; 190 cpixel=0;
185 lp=0; 191 lp=0;
186 rp=0; 192 rp=0;
187 olsa_integration(); 193 olsa_integration();
188 olsa_send(readout); 194 olsa_send(READOUT);
189 do // precte 102 pixelu 195 do // precte 102 pixelu
190 { 196 {
191 if(!SDOUT) // zacatek prenosu - zachycen start bit 197 if(!SDOUT) // zacatek prenosu - zachycen start bit
192 { 198 {
193 pixel=0; 199 pixel=0;
Line 553... Line 559...
553 if(BUMPR) // spusteni hledani pravym naraznikem 559 if(BUMPR) // spusteni hledani pravym naraznikem
554 { 560 {
555 output_low(LED2); 561 output_low(LED2);
556 output_high(LED1); 562 output_high(LED1);
557 beep(100,500); 563 beep(100,500);
558 delay_ms(1000); -  
559 while(true) 564 while(true)
560 { 565 {
561 old_position=position; // zaznamena predhozi polohu cary 566 old_position=position; // zaznamena predhozi polohu cary
562 read_olsa(); // precte a ulozi hodnoty z olsa 567 read_olsa(); // precte a ulozi hodnoty z olsa
563 olsa_position(); // vyhodnoti pozici cary 568 olsa_position(); // vyhodnoti pozici cary
Line 580... Line 585...
580 if(line_r==BLACK) // cara videna pravym modrym senzorem 585 if(line_r==BLACK) // cara videna pravym modrym senzorem
581 { 586 {
582 position=99; 587 position=99;
583 } 588 }
584 } 589 }
585 calc_error(); // vypocet chyby 590 calc_error();
586 calc_regulator(); // vypocet regulatoru 591 calc_regulator();
587 //printf("regulator: %u\r\n",reg_out); 592 //printf("regulator: %u\r\n",reg_out);
588 if(position<50) 593 if(position<50)
589 { 594 {
590 lm_speed=SPD_LO-reg_out; 595 lm_speed=SPD_LO-reg_out;
591 rm_speed=SPD_HI+reg_out; 596 rm_speed=SPD_HI+reg_out;