Rev Author Line No. Line
708 cizelu 1 #include "main.h"
700 cizelu 2  
708 cizelu 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
4 // BAUD RATE = 4800
5  
700 cizelu 6 //univerzalni LED diody
708 cizelu 7 #define LED1 PIN_A4
8 #define LED2 PIN_A5
700 cizelu 9  
10 //piezo pipak
716 cizelu 11 #DEFINE SOUND_HI PIN_B4
12 #DEFINE SOUND_LO PIN_B5
700 cizelu 13  
727 cizelu 14 //radkovy senzor
15 #define SDTAM PIN_B1
16 #define SDSEM input(PIN_B2)
17 #define SCLK PIN_B3
18  
700 cizelu 19 //naraznik
708 cizelu 20 #define BUMPL input(PIN_D6)
21 #define BUMPR input(PIN_D7)
700 cizelu 22  
23 //nouzove senzory
708 cizelu 24 #define LINEL 0
25 #define LINER 1
26 #define TRESHOLD 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
27 int8 line_l;
28 int8 line_r;
700 cizelu 29  
30 // motory
708 cizelu 31 #define LMF PIN_D0
32 #define LMB PIN_D1
33 #define RMF PIN_D2
34 #define RMB PIN_D3
700 cizelu 35  
708 cizelu 36 int16 bl;
700 cizelu 37 //PODPROGRAMY
38 //SENZORY
708 cizelu 39 void read_blue_sensors() // cteni nouzovych senzoru
40 {
41 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 42 delay_us(10);
708 cizelu 43 line_l=read_adc();
44  
45 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 46 delay_us(10);
47 line_r=read_adc();
708 cizelu 48 }
700 cizelu 49  
50 //PIPAK
716 cizelu 51 void beep(int16 period,int16 length)
700 cizelu 52 {
716 cizelu 53 int16 bp; //promenna pro nastaveni delky
700 cizelu 54  
55 for(bp=length;bp>0;bp--)
56 {
716 cizelu 57 output_high(SOUND_HI);
58 output_low(SOUND_LO);
708 cizelu 59 delay_us(period);
716 cizelu 60 output_high(SOUND_LO);
61 output_low(SOUND_HI);
708 cizelu 62 delay_us(period);
700 cizelu 63 }
708 cizelu 64 }
65 //MOTORY
66 void l_motor_fwd(int8 speedl) // levy motor dopredu
67 {
68 output_high(LMF);
69 output_low(LMB);
70 set_pwm2_duty(speedl);
700 cizelu 71 }
72  
708 cizelu 73 void l_motor_bwd(int8 speedl) // levy motor dozadu
74 {
75 output_high(LMB);
76 output_low(LMF);
77 set_pwm2_duty(speedl);
78 }
79  
80 void r_motor_fwd(int8 speedr) // pravy motor dopredu
81 {
82 output_high(RMF);
83 output_low(RMB);
84 set_pwm1_duty(speedr);
85 }
86  
87 void r_motor_bwd(int8 speedr) // pravy motor dozadu
88 {
89 output_high(RMB);
90 output_low(RMF);
91 set_pwm1_duty(speedr);
92 }
93  
94 void l_motor_off() // levy motor vypnut
95 {
96 output_low(LMF);
97 output_low(LMB);
98 set_pwm2_duty(0);
99 }
100  
101 void r_motor_off() // pravy motor vypnut
102 {
103 output_low(RMF);
104 output_low(RMB);
105 set_pwm1_duty(0);
106 }
107  
108 void motor_test() // test motoru
109 {
110 int8 i;
716 cizelu 111 beep(100,200);
708 cizelu 112 printf("TEST MOTORU\n");
113 delay_ms(1000);
716 cizelu 114 printf("LEVY MOTOR DOPREDU\n");
708 cizelu 115 for(i=0;i<255;i++)
116 {
117 l_motor_fwd(i);
118 printf("RYCHLOST: %u\n",i);
716 cizelu 119 delay_ms(5);
708 cizelu 120 }
121 for(i=255;i>0;i--)
122 {
123 l_motor_fwd(i);
124 printf("RYCHLOST: %u\n",i);
716 cizelu 125 delay_ms(5);
708 cizelu 126 }
716 cizelu 127  
128 printf("LEVY MOTOR DOZADU\n");
708 cizelu 129 for(i=0;i<255;i++)
130 {
131 l_motor_bwd(i);
132 printf("RYCHLOST: %u\n",i);
716 cizelu 133 delay_ms(5);
708 cizelu 134 }
135  
136 for(i=255;i>0;i--)
137 {
138 l_motor_bwd(i);
139 printf("RYCHLOST: %u\n",i);
716 cizelu 140 delay_ms(5);
708 cizelu 141 }
716 cizelu 142  
143 printf("PRAVY MOTOR DOPREDU\n");
708 cizelu 144 for(i=0;i<255;i++)
145 {
146 r_motor_fwd(i);
716 cizelu 147 printf("RYCHLOST: %u\n",i);
148 delay_ms(5);
708 cizelu 149 }
150  
151 for(i=255;i>0;i--)
152 {
153 r_motor_fwd(i);
154 printf("RYCHLOST: %u\n",i);
716 cizelu 155 delay_ms(5);
708 cizelu 156 }
716 cizelu 157  
158 printf("PRAVY MOTOR DOZADU\n");
708 cizelu 159 for(i=0;i<255;i++)
160 {
161 r_motor_bwd(i);
162 printf("RYCHLOST: %u\n",i);
716 cizelu 163 delay_ms(5);
708 cizelu 164 }
165  
166 for(i=255;i>0;i--)
167 {
168 r_motor_bwd(i);
169 printf("RYCHLOST: %u\n",i);
716 cizelu 170 delay_ms(5);
708 cizelu 171 }
716 cizelu 172 printf("KONEC TESTU MOTORU \N");
708 cizelu 173 }
174  
175 void diagnostika()
176 {
177 read_blue_sensors();
178 printf("LEVA: %u \t",line_l);
179 delay_ms(20);
180 printf("PRAVA: %u \t",line_r);
181 delay_ms(20);
182 printf("L_NARAZ: %u \t",BUMPL);
183 delay_ms(20);
184 printf("P_NARAZ: %u \n",BUMPR);
185 delay_ms(20);
186 if(BUMPL&&BUMPR)
187 {
188 motor_test();
189 }
190 }
191  
700 cizelu 192 // HLAVNI SMYCKA
193 void main()
194 {
708 cizelu 195 printf("POWER ON \n");
700 cizelu 196 // NASTAVENI > provede se pouze pri zapnuti
708 cizelu 197 setup_adc_ports(sAN0-sAN1-sAN2);
700 cizelu 198 setup_adc(ADC_CLOCK_INTERNAL);
199 setup_spi(SPI_SS_DISABLED);
200 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
201 setup_timer_1(T1_DISABLED);
202 setup_timer_2(T2_DIV_BY_16,255,1); //casovac pro PWM
203 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
204 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
205 setup_comparator(NC_NC_NC_NC);
708 cizelu 206 setup_vref(FALSE);
701 cizelu 207 output_high(LED1); // zhasne LED1
208 output_high(LED2); // zhasne LED2
708 cizelu 209 l_motor_off(); // vypne oba motory
210 r_motor_off(); // vypne oba motory
716 cizelu 211 beep(500,200); // pipni pri startu
708 cizelu 212 printf("OK! \n");
716 cizelu 213 delay_ms(500);
708 cizelu 214 printf("VYBRAT MOD... \n");
700 cizelu 215 while(true)
216 {
727 cizelu 217  
708 cizelu 218 bl++; // primitivni blikani - oznacuje vypber modu
219 if(bl>4096)
700 cizelu 220 {
701 cizelu 221 output_low(LED1);
708 cizelu 222 output_high(LED2);
700 cizelu 223 }
708 cizelu 224 else
700 cizelu 225 {
708 cizelu 226 output_high(LED1);
701 cizelu 227 output_low(LED2);
708 cizelu 228 }
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);
716 cizelu 238 beep(100,200);
708 cizelu 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);
716 cizelu 250 beep(100,200);
708 cizelu 251 delay_ms(1000);
252 while(true)
253 {
700 cizelu 254  
708 cizelu 255 }
727 cizelu 256 }
708 cizelu 257 }
700 cizelu 258 }