Rev 727 Rev 730
Line 10... Line 10...
10 //piezo pipak 10 //piezo pipak
11 #DEFINE SOUND_HI PIN_B4 11 #DEFINE SOUND_HI PIN_B4
12 #DEFINE SOUND_LO PIN_B5 12 #DEFINE SOUND_LO PIN_B5
13   13  
14 //radkovy senzor 14 //radkovy senzor
15 #define SDTAM PIN_B1 15 #define SDIN PIN_D4 // seriovy vstup
16 #define SDSEM input(PIN_B2) 16 #define SDOUT input(PIN_B2) // seriovy vystup
17 #define SCLK PIN_B3 17 #define SCLK PIN_D5 // takt
-   18  
-   19 int test[8]={1,0,0,0,0,0,0,0}; // pole pro test
18   20  
19 //naraznik 21 //naraznik
20 #define BUMPL input(PIN_D6) 22 #define BUMPL input(PIN_D6)
21 #define BUMPR input(PIN_D7) 23 #define BUMPR input(PIN_D7)
22   24  
Line 31... Line 33...
31 #define LMF PIN_D0 33 #define LMF PIN_D0
32 #define LMB PIN_D1 34 #define LMB PIN_D1
33 #define RMF PIN_D2 35 #define RMF PIN_D2
34 #define RMB PIN_D3 36 #define RMB PIN_D3
35   37  
36 int16 bl; -  
37 //PODPROGRAMY 38 //PODPROGRAMY
38 //SENZORY 39 //SENZORY
-   40  
-   41 //OLSA01A
-   42 void pulse(int pocet) // vytvori impulzy pro ridici logiku
-   43 {
-   44 int8 pc;
-   45 for(pc=0;pc<=pocet;pc++)
-   46 {
-   47 output_high(SCLK);
-   48 output_low(SCLK);
-   49 }
-   50 }
-   51  
-   52 void send(int info[]) // USART komunikace s modulem OLSA01A
-   53 {
-   54 int8 *ip = &info; // ukazatel na pole s informaci
-   55 int i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
-   56 for(ip=0;ip<8;ip++)
-   57 {
-   58 i=info[ip]; // ziskani hodnoty z pole
-   59 if(i==1) // vyhodnoceni obsahu informace - nastav 1
-   60 {
-   61 output_high(SDIN);
-   62 }
-   63 else // vyhodnoceni obsahu informace - nastav 0
-   64 {
-   65 output_low(SDIN);
-   66 }
-   67 delay_us(1); // vytvor impulz - delay je pouze pro zabraneni hazardnich stavu
-   68 output_high(SCLK);
-   69 delay_us(1);
-   70 output_low(SCLK);
-   71 }
-   72 }
-   73  
39 void read_blue_sensors() // cteni nouzovych senzoru 74 void read_blue_sensors() // cteni nouzovych senzoru
40 { 75 {
41 set_adc_channel(LINEL); // cti levy nouzovy senzor 76 set_adc_channel(LINEL); // cti levy nouzovy senzor
42 delay_us(10); 77 delay_us(10);
43 line_l=read_adc(); 78 line_l=read_adc();
44 79
45 set_adc_channel(LINER); // cti pravy nouzovy senzor 80 set_adc_channel(LINER); // cti pravy nouzovy senzor
46 delay_us(10); 81 delay_us(10);
47 line_r=read_adc(); 82 line_r=read_adc();
48 } 83 }
-   84
-   85  
49   86  
50 //PIPAK 87 //PIPAK
51 void beep(int16 period,int16 length) 88 void beep(int16 period,int16 length)
52 { 89 {
53 int16 bp; //promenna pro nastaveni delky 90 int16 bp; //promenna pro nastaveni delky
54 91
55 for(bp=length;bp>0;bp--) 92 for(bp=length;bp>0;bp--)
56 { 93 {
57 output_high(SOUND_HI); 94 output_high(SOUND_HI);
58 output_low(SOUND_LO); 95 output_low(SOUND_LO);
59 delay_us(period); 96 delay_us(period);
60 output_high(SOUND_LO); 97 output_high(SOUND_LO);
61 output_low(SOUND_HI); 98 output_low(SOUND_HI);
62 delay_us(period); 99 delay_us(period);
63 } 100 }
64 } 101 }
65 //MOTORY 102 //MOTORY
66 void l_motor_fwd(int8 speedl) // levy motor dopredu 103 void l_motor_fwd(int8 speedl) // levy motor dopredu
67 { 104 {
68 output_high(LMF); 105 output_high(LMF);
69 output_low(LMB); 106 output_low(LMB);
70 set_pwm2_duty(speedl); 107 set_pwm2_duty(speedl);
71 } 108 }
72   109  
73 void l_motor_bwd(int8 speedl) // levy motor dozadu 110 void l_motor_bwd(int8 speedl) // levy motor dozadu
74 { 111 {
75 output_high(LMB); 112 output_high(LMB);
76 output_low(LMF); 113 output_low(LMF);
77 set_pwm2_duty(speedl); 114 set_pwm2_duty(speedl);
78 } 115 }
79   116  
80 void r_motor_fwd(int8 speedr) // pravy motor dopredu 117 void r_motor_fwd(int8 speedr) // pravy motor dopredu
81 { 118 {
82 output_high(RMF); 119 output_high(RMF);
83 output_low(RMB); 120 output_low(RMB);
84 set_pwm1_duty(speedr); 121 set_pwm1_duty(speedr);
85 } 122 }
86   123  
87 void r_motor_bwd(int8 speedr) // pravy motor dozadu 124 void r_motor_bwd(int8 speedr) // pravy motor dozadu
88 { 125 {
89 output_high(RMB); 126 output_high(RMB);
90 output_low(RMF); 127 output_low(RMF);
91 set_pwm1_duty(speedr); 128 set_pwm1_duty(speedr);
92 } 129 }
93   130  
94 void l_motor_off() // levy motor vypnut 131 void l_motor_off() // levy motor vypnut
95 { 132 {
96 output_low(LMF); 133 output_low(LMF);
97 output_low(LMB); 134 output_low(LMB);
98 set_pwm2_duty(0); 135 set_pwm2_duty(0);
99 } 136 }
100   137
101 void r_motor_off() // pravy motor vypnut 138 void r_motor_off() // pravy motor vypnut
102 { 139 {
103 output_low(RMF); 140 output_low(RMF);
104 output_low(RMB); 141 output_low(RMB);
105 set_pwm1_duty(0); 142 set_pwm1_duty(0);
106 } 143 }
107   144  
108 void motor_test() // test motoru 145 void motor_test() // test motoru
109 { 146 {
110 int8 i; 147 int8 i;
111 beep(100,200); 148 beep(100,200);
112 printf("TEST MOTORU\n"); 149 printf("TEST MOTORU\n");
113 delay_ms(1000); 150 delay_ms(1000);
114 printf("LEVY MOTOR DOPREDU\n"); 151 printf("LEVY MOTOR DOPREDU\n");
115 for(i=0;i<255;i++) 152 for(i=0;i<255;i++)
116 { 153 {
117 l_motor_fwd(i); 154 l_motor_fwd(i);
118 printf("RYCHLOST: %u\n",i); 155 printf("RYCHLOST: %u\n",i);
119 delay_ms(5); 156 delay_ms(5);
120 } 157 }
121 for(i=255;i>0;i--) 158 for(i=255;i>0;i--)
122 { 159 {
123 l_motor_fwd(i); 160 l_motor_fwd(i);
124 printf("RYCHLOST: %u\n",i); 161 printf("RYCHLOST: %u\n",i);
125 delay_ms(5); 162 delay_ms(5);
126 } 163 }
127 -  
128 printf("LEVY MOTOR DOZADU\n"); 164 printf("LEVY MOTOR DOZADU\n");
129 for(i=0;i<255;i++) 165 for(i=0;i<255;i++)
130 { 166 {
131 l_motor_bwd(i); 167 l_motor_bwd(i);
132 printf("RYCHLOST: %u\n",i); 168 printf("RYCHLOST: %u\n",i);
133 delay_ms(5); 169 delay_ms(5);
134 } 170 }
135 -  
136 for(i=255;i>0;i--) 171 for(i=255;i>0;i--)
137 { 172 {
138 l_motor_bwd(i); 173 l_motor_bwd(i);
139 printf("RYCHLOST: %u\n",i); 174 printf("RYCHLOST: %u\n",i);
140 delay_ms(5); 175 delay_ms(5);
141 } 176 }
142 -  
143 printf("PRAVY MOTOR DOPREDU\n"); 177 printf("PRAVY MOTOR DOPREDU\n");
144 for(i=0;i<255;i++) 178 for(i=0;i<255;i++)
145 { 179 {
146 r_motor_fwd(i); 180 r_motor_fwd(i);
147 printf("RYCHLOST: %u\n",i); 181 printf("RYCHLOST: %u\n",i);
148 delay_ms(5); 182 delay_ms(5);
149 } 183 }
150 -  
151 for(i=255;i>0;i--) 184 for(i=255;i>0;i--)
152 { 185 {
153 r_motor_fwd(i); 186 r_motor_fwd(i);
154 printf("RYCHLOST: %u\n",i); 187 printf("RYCHLOST: %u\n",i);
155 delay_ms(5); 188 delay_ms(5);
156 } 189 }
157 -  
158 printf("PRAVY MOTOR DOZADU\n"); 190 printf("PRAVY MOTOR DOZADU\n");
159 for(i=0;i<255;i++) 191 for(i=0;i<255;i++)
160 { 192 {
161 r_motor_bwd(i); 193 r_motor_bwd(i);
162 printf("RYCHLOST: %u\n",i); 194 printf("RYCHLOST: %u\n",i);
163 delay_ms(5); 195 delay_ms(5);
164 } 196 }
165 -  
166 for(i=255;i>0;i--) 197 for(i=255;i>0;i--)
167 { 198 {
168 r_motor_bwd(i); 199 r_motor_bwd(i);
169 printf("RYCHLOST: %u\n",i); 200 printf("RYCHLOST: %u\n",i);
170 delay_ms(5); 201 delay_ms(5);
171 } 202 }
172 printf("KONEC TESTU MOTORU \N"); 203 printf("KONEC TESTU MOTORU \N");
173 } 204 }
174   205  
175 void diagnostika() 206 void diagnostika()
176 { 207 {
177 read_blue_sensors(); 208 read_blue_sensors();
178 printf("LEVA: %u \t",line_l); 209 printf("LEVA: %u \t",line_l);
Line 189... Line 220...
189 } 220 }
190 } 221 }
191   222  
192 // HLAVNI SMYCKA 223 // HLAVNI SMYCKA
193 void main() 224 void main()
194 { 225 {
195 printf("POWER ON \n"); 226 printf("POWER ON \n");
196 // NASTAVENI > provede se pouze pri zapnuti 227 // NASTAVENI > provede se pouze pri zapnuti
197 setup_adc_ports(sAN0-sAN1-sAN2); 228 setup_adc_ports(sAN0-sAN1-sAN2);
198 setup_adc(ADC_CLOCK_INTERNAL); 229 setup_adc(ADC_CLOCK_INTERNAL);
199 setup_spi(SPI_SS_DISABLED); 230 setup_spi(SPI_SS_DISABLED);
Line 211... Line 242...
211 beep(500,200); // pipni pri startu 242 beep(500,200); // pipni pri startu
212 printf("OK! \n"); 243 printf("OK! \n");
213 delay_ms(500); 244 delay_ms(500);
214 printf("VYBRAT MOD... \n"); 245 printf("VYBRAT MOD... \n");
215 while(true) 246 while(true)
216 { -  
217 -  
218 bl++; // primitivni blikani - oznacuje vypber modu -  
219 if(bl>4096) -  
220 { -  
221 output_low(LED1); -  
222 output_high(LED2); -  
223 } -  
224 else -  
225 { 247 {
226 output_high(LED1); -  
227 output_low(LED2); 248 send(test);
228 } 249 }
229 if(bl>8192) -  
230 { -  
231 bl=0; -  
232 } -  
233 if(BUMPL) -  
234 { -  
235 printf("MOD: DIAGNOSTIKA\n"); -  
236 output_low(LED1); -  
237 output_high(LED2); -  
238 beep(100,200); -  
239 delay_ms(1000); -  
240 while(true) -  
241 { -  
242 diagnostika(); -  
243 } -  
244 } -  
245 if(BUMPR) -  
246 { -  
247 printf("MOD: STOPOVANI\n"); -  
248 output_low(LED2); -  
249 output_high(LED1); -  
250 beep(100,200); -  
251 delay_ms(1000); -  
252 while(true) -  
253 { -  
254   -  
255 } -  
256 } -  
257 } 250 }
258 } -