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
745 cizelu 15 #define SDIN PIN_D4 // seriovy vstup
16 #define SDOUT input(PIN_B2) // seriovy vystup
17 #define SCLK PIN_D5 // takt
727 cizelu 18  
741 cizelu 19 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset
745 cizelu 20 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru
741 cizelu 21 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru
730 cizelu 22  
745 cizelu 23 int olsa_line[96]={0}; // radkovy senzor
741 cizelu 24  
700 cizelu 25 //naraznik
708 cizelu 26 #define BUMPL input(PIN_D6)
27 #define BUMPR input(PIN_D7)
700 cizelu 28  
29 //nouzove senzory
708 cizelu 30 #define LINEL 0
31 #define LINER 1
745 cizelu 32 #define TRESHOLD 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
708 cizelu 33 int8 line_l;
34 int8 line_r;
700 cizelu 35  
36 // motory
708 cizelu 37 #define LMF PIN_D0
38 #define LMB PIN_D1
39 #define RMF PIN_D2
40 #define RMB PIN_D3
700 cizelu 41  
42 //PODPROGRAMY
43 //SENZORY
730 cizelu 44 //OLSA01A
745 cizelu 45 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
730 cizelu 46 {
741 cizelu 47 int8 ct;
48 for(ct=0;ct<=count;ct++)
730 cizelu 49 {
745 cizelu 50 delay_us(1); // doba pro ustaleni
730 cizelu 51 output_high(SCLK);
745 cizelu 52 delay_us(1); // doba pro ustaleni
730 cizelu 53 output_low(SCLK);
54 }
55 }
56  
745 cizelu 57 void olsa_pulse() // vytvori jeden impulz
730 cizelu 58 {
745 cizelu 59 delay_us(1); // doba pro ustaleni
741 cizelu 60 output_high(SCLK);
745 cizelu 61 delay_us(1); // doba pro ustaleni
741 cizelu 62 output_low(SCLK);
63 }
64  
65 void olsa_send(int info[]) // USART komunikace s modulem OLSA01A - poslani zpravy
66 {
67 int8 *ip = &info; // ukazatel na pole s informaci
68 int i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
69  
70 output_low(SDIN); // start bit
71 olsa_pulse();
72 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
730 cizelu 73 {
741 cizelu 74 i=info[ip]; // ziskani hodnoty z pole
75 if(i==1) // vyhodnoceni obsahu informace - nastav 1
730 cizelu 76 {
77 output_high(SDIN);
78 }
741 cizelu 79 else // vyhodnoceni obsahu informace - nastav 0
730 cizelu 80 {
81 output_low(SDIN);
82 }
741 cizelu 83 olsa_pulse();
730 cizelu 84 }
741 cizelu 85 output_high(SDIN); // stop bit
86 olsa_pulse();
730 cizelu 87 }
88  
741 cizelu 89 void olsa_reset() // hlavni RESET - provadi se po zapnuti
90 {
91 output_low(SDIN);
92 output_low(SCLK);
93 olsa_pulses(30); // reset radkoveho senzoru
94 output_high(SDIN);
95 olsa_pulses(10); // start bit - synchronizace
96 olsa_send(main_reset);
97 olsa_pulses(5);
98 olsa_send(set_mode_rg);
99 olsa_send(clear_mode_rg);
100 }
101  
102 //ZACHRANNE SENZORY
708 cizelu 103 void read_blue_sensors() // cteni nouzovych senzoru
104 {
105 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 106 delay_us(10);
745 cizelu 107 line_l=read_adc();
708 cizelu 108 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 109 delay_us(10);
110 line_r=read_adc();
708 cizelu 111 }
730 cizelu 112  
700 cizelu 113 //PIPAK
716 cizelu 114 void beep(int16 period,int16 length)
730 cizelu 115 {
716 cizelu 116 int16 bp; //promenna pro nastaveni delky
700 cizelu 117  
118 for(bp=length;bp>0;bp--)
730 cizelu 119 {
120 output_high(SOUND_HI);
121 output_low(SOUND_LO);
122 delay_us(period);
123 output_high(SOUND_LO);
124 output_low(SOUND_HI);
125 delay_us(period);
126 }
127 }
708 cizelu 128 //MOTORY
129 void l_motor_fwd(int8 speedl) // levy motor dopredu
730 cizelu 130 {
708 cizelu 131 output_high(LMF);
132 output_low(LMB);
133 set_pwm2_duty(speedl);
730 cizelu 134 }
700 cizelu 135  
708 cizelu 136 void l_motor_bwd(int8 speedl) // levy motor dozadu
730 cizelu 137 {
708 cizelu 138 output_high(LMB);
139 output_low(LMF);
140 set_pwm2_duty(speedl);
730 cizelu 141 }
708 cizelu 142  
143 void r_motor_fwd(int8 speedr) // pravy motor dopredu
730 cizelu 144 {
708 cizelu 145 output_high(RMF);
146 output_low(RMB);
147 set_pwm1_duty(speedr);
730 cizelu 148 }
708 cizelu 149  
150 void r_motor_bwd(int8 speedr) // pravy motor dozadu
730 cizelu 151 {
708 cizelu 152 output_high(RMB);
153 output_low(RMF);
154 set_pwm1_duty(speedr);
730 cizelu 155 }
708 cizelu 156  
157 void l_motor_off() // levy motor vypnut
730 cizelu 158 {
708 cizelu 159 output_low(LMF);
160 output_low(LMB);
161 set_pwm2_duty(0);
730 cizelu 162 }
163  
708 cizelu 164 void r_motor_off() // pravy motor vypnut
730 cizelu 165 {
708 cizelu 166 output_low(RMF);
167 output_low(RMB);
168 set_pwm1_duty(0);
730 cizelu 169 }
708 cizelu 170  
171 void motor_test() // test motoru
730 cizelu 172 {
708 cizelu 173 int8 i;
716 cizelu 174 beep(100,200);
708 cizelu 175 printf("TEST MOTORU\n");
176 delay_ms(1000);
716 cizelu 177 printf("LEVY MOTOR DOPREDU\n");
708 cizelu 178 for(i=0;i<255;i++)
730 cizelu 179 {
708 cizelu 180 l_motor_fwd(i);
181 printf("RYCHLOST: %u\n",i);
716 cizelu 182 delay_ms(5);
730 cizelu 183 }
708 cizelu 184 for(i=255;i>0;i--)
730 cizelu 185 {
708 cizelu 186 l_motor_fwd(i);
187 printf("RYCHLOST: %u\n",i);
716 cizelu 188 delay_ms(5);
730 cizelu 189 }
716 cizelu 190 printf("LEVY MOTOR DOZADU\n");
708 cizelu 191 for(i=0;i<255;i++)
730 cizelu 192 {
708 cizelu 193 l_motor_bwd(i);
194 printf("RYCHLOST: %u\n",i);
716 cizelu 195 delay_ms(5);
730 cizelu 196 }
708 cizelu 197 for(i=255;i>0;i--)
730 cizelu 198 {
708 cizelu 199 l_motor_bwd(i);
200 printf("RYCHLOST: %u\n",i);
716 cizelu 201 delay_ms(5);
730 cizelu 202 }
716 cizelu 203 printf("PRAVY MOTOR DOPREDU\n");
708 cizelu 204 for(i=0;i<255;i++)
730 cizelu 205 {
708 cizelu 206 r_motor_fwd(i);
716 cizelu 207 printf("RYCHLOST: %u\n",i);
208 delay_ms(5);
730 cizelu 209 }
708 cizelu 210 for(i=255;i>0;i--)
730 cizelu 211 {
708 cizelu 212 r_motor_fwd(i);
213 printf("RYCHLOST: %u\n",i);
716 cizelu 214 delay_ms(5);
730 cizelu 215 }
716 cizelu 216 printf("PRAVY MOTOR DOZADU\n");
708 cizelu 217 for(i=0;i<255;i++)
730 cizelu 218 {
708 cizelu 219 r_motor_bwd(i);
220 printf("RYCHLOST: %u\n",i);
716 cizelu 221 delay_ms(5);
730 cizelu 222 }
708 cizelu 223 for(i=255;i>0;i--)
730 cizelu 224 {
708 cizelu 225 r_motor_bwd(i);
226 printf("RYCHLOST: %u\n",i);
716 cizelu 227 delay_ms(5);
730 cizelu 228 }
229 printf("KONEC TESTU MOTORU \N");
708 cizelu 230 }
231  
232 void diagnostika()
233 {
234 read_blue_sensors();
235 printf("LEVA: %u \t",line_l);
236 delay_ms(20);
237 printf("PRAVA: %u \t",line_r);
238 delay_ms(20);
239 printf("L_NARAZ: %u \t",BUMPL);
240 delay_ms(20);
241 printf("P_NARAZ: %u \n",BUMPR);
242 delay_ms(20);
243 if(BUMPL&&BUMPR)
244 {
245 motor_test();
246 }
247 }
248  
700 cizelu 249 // HLAVNI SMYCKA
250 void main()
730 cizelu 251 {
708 cizelu 252 printf("POWER ON \n");
700 cizelu 253 // NASTAVENI > provede se pouze pri zapnuti
708 cizelu 254 setup_adc_ports(sAN0-sAN1-sAN2);
741 cizelu 255 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
700 cizelu 256 setup_spi(SPI_SS_DISABLED);
257 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
258 setup_timer_1(T1_DISABLED);
741 cizelu 259 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
700 cizelu 260 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
261 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
262 setup_comparator(NC_NC_NC_NC);
708 cizelu 263 setup_vref(FALSE);
701 cizelu 264 output_high(LED1); // zhasne LED1
265 output_high(LED2); // zhasne LED2
741 cizelu 266 l_motor_off(); // vypne levy motor
267 r_motor_off(); // vypne pravy motor
268 olsa_reset(); // reset logiky radkoveho senzoru
716 cizelu 269 beep(500,200); // pipni pri startu
708 cizelu 270 printf("OK! \n");
716 cizelu 271 delay_ms(500);
708 cizelu 272 printf("VYBRAT MOD... \n");
700 cizelu 273 while(true)
274 {
275 }
730 cizelu 276 }