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