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