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
11 #DEFINE SOUND_HI PIN_B1
12 #DEFINE SOUND_LO PIN_B2
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
46 void beep(unsigned int16 period, unsigned int16 length)
47 {
708 cizelu 48 unsigned int16 bp; //promenna pro nastaveni delky
700 cizelu 49  
50 for(bp=length;bp>0;bp--)
51 {
708 cizelu 52 output_high(SOUND_HI);output_low(SOUND_LO);
53 delay_us(period);
54 output_high(SOUND_LO);output_low(SOUND_HI);
55 delay_us(period);
700 cizelu 56 }
708 cizelu 57 }
58 //MOTORY
59 void l_motor_fwd(int8 speedl) // levy motor dopredu
60 {
61 output_high(LMF);
62 output_low(LMB);
63 set_pwm2_duty(speedl);
700 cizelu 64 }
65  
708 cizelu 66 void l_motor_bwd(int8 speedl) // levy motor dozadu
67 {
68 output_high(LMB);
69 output_low(LMF);
70 set_pwm2_duty(speedl);
71 }
72  
73 void r_motor_fwd(int8 speedr) // pravy motor dopredu
74 {
75 output_high(RMF);
76 output_low(RMB);
77 set_pwm1_duty(speedr);
78 }
79  
80 void r_motor_bwd(int8 speedr) // pravy motor dozadu
81 {
82 output_high(RMB);
83 output_low(RMF);
84 set_pwm1_duty(speedr);
85 }
86  
87 void l_motor_off() // levy motor vypnut
88 {
89 output_low(LMF);
90 output_low(LMB);
91 set_pwm2_duty(0);
92 }
93  
94 void r_motor_off() // pravy motor vypnut
95 {
96 output_low(RMF);
97 output_low(RMB);
98 set_pwm1_duty(0);
99 }
100  
101 void motor_test() // test motoru
102 {
103 int8 i;
104 printf("TEST MOTORU\n");
105 delay_ms(1000);
106 for(i=0;i<255;i++)
107 {
108 l_motor_fwd(i);
109 printf("RYCHLOST: %u\n",i);
110 delay_ms(10);
111 }
112 for(i=255;i>0;i--)
113 {
114 l_motor_fwd(i);
115 printf("RYCHLOST: %u\n",i);
116 delay_ms(10);
117 }
118 for(i=0;i<255;i++)
119 {
120 l_motor_bwd(i);
121 printf("RYCHLOST: %u\n",i);
122 delay_ms(10);
123 }
124  
125 for(i=255;i>0;i--)
126 {
127 l_motor_bwd(i);
128 printf("RYCHLOST: %u\n",i);
129 delay_ms(10);
130 }
131 for(i=0;i<255;i++)
132 {
133 r_motor_fwd(i);
134 delay_ms(10);
135 }
136  
137 for(i=255;i>0;i--)
138 {
139 r_motor_fwd(i);
140 printf("RYCHLOST: %u\n",i);
141 delay_ms(10);
142 }
143 for(i=0;i<255;i++)
144 {
145 r_motor_bwd(i);
146 printf("RYCHLOST: %u\n",i);
147 delay_ms(10);
148 }
149  
150 for(i=255;i>0;i--)
151 {
152 r_motor_bwd(i);
153 printf("RYCHLOST: %u\n",i);
154 delay_ms(10);
155 }
156 l_motor_off();
157 r_motor_off();
158 }
159  
160 void diagnostika()
161 {
162 read_blue_sensors();
163 printf("LEVA: %u \t",line_l);
164 delay_ms(20);
165 printf("PRAVA: %u \t",line_r);
166 delay_ms(20);
167 printf("L_NARAZ: %u \t",BUMPL);
168 delay_ms(20);
169 printf("P_NARAZ: %u \n",BUMPR);
170 delay_ms(20);
171 if(BUMPL&&BUMPR)
172 {
173 motor_test();
174 }
175 }
176  
700 cizelu 177 // HLAVNI SMYCKA
178 void main()
179 {
708 cizelu 180 printf("POWER ON \n");
700 cizelu 181 // NASTAVENI > provede se pouze pri zapnuti
708 cizelu 182 setup_adc_ports(sAN0-sAN1-sAN2);
700 cizelu 183 setup_adc(ADC_CLOCK_INTERNAL);
184 setup_spi(SPI_SS_DISABLED);
185 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
186 setup_timer_1(T1_DISABLED);
187 setup_timer_2(T2_DIV_BY_16,255,1); //casovac pro PWM
188 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
189 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
190 setup_comparator(NC_NC_NC_NC);
708 cizelu 191 setup_vref(FALSE);
701 cizelu 192 output_high(LED1); // zhasne LED1
193 output_high(LED2); // zhasne LED2
708 cizelu 194 l_motor_off(); // vypne oba motory
195 r_motor_off(); // vypne oba motory
196 printf("OK! \n");
197 printf("VYBRAT MOD... \n");
700 cizelu 198 while(true)
199 {
708 cizelu 200 bl++; // primitivni blikani - oznacuje vypber modu
201 if(bl>4096)
700 cizelu 202 {
701 cizelu 203 output_low(LED1);
708 cizelu 204 output_high(LED2);
700 cizelu 205 }
708 cizelu 206 else
700 cizelu 207 {
708 cizelu 208 output_high(LED1);
701 cizelu 209 output_low(LED2);
708 cizelu 210 }
211 if(bl>8192)
212 {
213 bl=0;
214 }
215 if(BUMPL)
216 {
217 printf("MOD: DIAGNOSTIKA\n");
218 output_low(LED1);
219 output_high(LED2);
220 delay_ms(1000);
221 while(true)
222 {
223 diagnostika();
224 }
225 }
226 if(BUMPR)
227 {
228 printf("MOD: STOPOVANI\n");
229 output_low(LED2);
230 output_high(LED1);
231 delay_ms(1000);
232 while(true)
233 {
700 cizelu 234  
708 cizelu 235 }
236 }
237 }
700 cizelu 238 }