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