Rev 708 Rev 716
Line 6... Line 6...
6 //univerzalni LED diody 6 //univerzalni LED diody
7 #define LED1 PIN_A4 7 #define LED1 PIN_A4
8 #define LED2 PIN_A5 8 #define LED2 PIN_A5
9   9  
10 //piezo pipak 10 //piezo pipak
11 #DEFINE SOUND_HI PIN_B1 11 #DEFINE SOUND_HI PIN_B4
12 #DEFINE SOUND_LO PIN_B2 12 #DEFINE SOUND_LO PIN_B5
13   13  
14 //naraznik 14 //naraznik
15 #define BUMPL input(PIN_D6) 15 #define BUMPL input(PIN_D6)
16 #define BUMPR input(PIN_D7) 16 #define BUMPR input(PIN_D7)
17   17  
Line 41... Line 41...
41 delay_us(10); 41 delay_us(10);
42 line_r=read_adc(); 42 line_r=read_adc();
43 } 43 }
44   44  
45 //PIPAK 45 //PIPAK
46 void beep(unsigned int16 period, unsigned int16 length) 46 void beep(int16 period,int16 length)
47 { 47 {
48 unsigned int16 bp; //promenna pro nastaveni delky 48 int16 bp; //promenna pro nastaveni delky
49 49
50 for(bp=length;bp>0;bp--) 50 for(bp=length;bp>0;bp--)
51 { 51 {
52 output_high(SOUND_HI);output_low(SOUND_LO); 52 output_high(SOUND_HI);
-   53 output_low(SOUND_LO);
53 delay_us(period); 54 delay_us(period);
54 output_high(SOUND_LO);output_low(SOUND_HI); 55 output_high(SOUND_LO);
-   56 output_low(SOUND_HI);
55 delay_us(period); 57 delay_us(period);
56 } 58 }
57 } 59 }
58 //MOTORY 60 //MOTORY
59 void l_motor_fwd(int8 speedl) // levy motor dopredu 61 void l_motor_fwd(int8 speedl) // levy motor dopredu
Line 99... Line 101...
99 } 101 }
100   102  
101 void motor_test() // test motoru 103 void motor_test() // test motoru
102 { 104 {
103 int8 i; 105 int8 i;
-   106 beep(100,200);
104 printf("TEST MOTORU\n"); 107 printf("TEST MOTORU\n");
105 delay_ms(1000); 108 delay_ms(1000);
-   109 printf("LEVY MOTOR DOPREDU\n");
106 for(i=0;i<255;i++) 110 for(i=0;i<255;i++)
107 { 111 {
108 l_motor_fwd(i); 112 l_motor_fwd(i);
109 printf("RYCHLOST: %u\n",i); 113 printf("RYCHLOST: %u\n",i);
110 delay_ms(10); 114 delay_ms(5);
111 } 115 }
112 for(i=255;i>0;i--) 116 for(i=255;i>0;i--)
113 { 117 {
114 l_motor_fwd(i); 118 l_motor_fwd(i);
115 printf("RYCHLOST: %u\n",i); 119 printf("RYCHLOST: %u\n",i);
116 delay_ms(10); 120 delay_ms(5);
117 } 121 }
-   122
-   123 printf("LEVY MOTOR DOZADU\n");
118 for(i=0;i<255;i++) 124 for(i=0;i<255;i++)
119 { 125 {
120 l_motor_bwd(i); 126 l_motor_bwd(i);
121 printf("RYCHLOST: %u\n",i); 127 printf("RYCHLOST: %u\n",i);
122 delay_ms(10); 128 delay_ms(5);
123 } 129 }
124 130
125 for(i=255;i>0;i--) 131 for(i=255;i>0;i--)
126 { 132 {
127 l_motor_bwd(i); 133 l_motor_bwd(i);
128 printf("RYCHLOST: %u\n",i); 134 printf("RYCHLOST: %u\n",i);
129 delay_ms(10); 135 delay_ms(5);
130 } 136 }
-   137
-   138 printf("PRAVY MOTOR DOPREDU\n");
131 for(i=0;i<255;i++) 139 for(i=0;i<255;i++)
132 { 140 {
133 r_motor_fwd(i); 141 r_motor_fwd(i);
-   142 printf("RYCHLOST: %u\n",i);
134 delay_ms(10); 143 delay_ms(5);
135 } 144 }
136 145
137 for(i=255;i>0;i--) 146 for(i=255;i>0;i--)
138 { 147 {
139 r_motor_fwd(i); 148 r_motor_fwd(i);
140 printf("RYCHLOST: %u\n",i); 149 printf("RYCHLOST: %u\n",i);
141 delay_ms(10); 150 delay_ms(5);
142 } 151 }
-   152
-   153 printf("PRAVY MOTOR DOZADU\n");
143 for(i=0;i<255;i++) 154 for(i=0;i<255;i++)
144 { 155 {
145 r_motor_bwd(i); 156 r_motor_bwd(i);
146 printf("RYCHLOST: %u\n",i); 157 printf("RYCHLOST: %u\n",i);
147 delay_ms(10); 158 delay_ms(5);
148 } 159 }
149 160
150 for(i=255;i>0;i--) 161 for(i=255;i>0;i--)
151 { 162 {
152 r_motor_bwd(i); 163 r_motor_bwd(i);
153 printf("RYCHLOST: %u\n",i); 164 printf("RYCHLOST: %u\n",i);
154 delay_ms(10); 165 delay_ms(5);
155 } 166 }
156 l_motor_off(); -  
157 r_motor_off(); 167 printf("KONEC TESTU MOTORU \N");
158 } 168 }
159   169  
160 void diagnostika() 170 void diagnostika()
161 { 171 {
162 read_blue_sensors(); 172 read_blue_sensors();
Line 191... Line 201...
191 setup_vref(FALSE); 201 setup_vref(FALSE);
192 output_high(LED1); // zhasne LED1 202 output_high(LED1); // zhasne LED1
193 output_high(LED2); // zhasne LED2 203 output_high(LED2); // zhasne LED2
194 l_motor_off(); // vypne oba motory 204 l_motor_off(); // vypne oba motory
195 r_motor_off(); // vypne oba motory 205 r_motor_off(); // vypne oba motory
-   206 beep(500,200); // pipni pri startu
196 printf("OK! \n"); 207 printf("OK! \n");
-   208 delay_ms(500);
197 printf("VYBRAT MOD... \n"); 209 printf("VYBRAT MOD... \n");
198 while(true) 210 while(true)
199 { 211 {
200 bl++; // primitivni blikani - oznacuje vypber modu 212 bl++; // primitivni blikani - oznacuje vypber modu
201 if(bl>4096) 213 if(bl>4096)
Line 215... Line 227...
215 if(BUMPL) 227 if(BUMPL)
216 { 228 {
217 printf("MOD: DIAGNOSTIKA\n"); 229 printf("MOD: DIAGNOSTIKA\n");
218 output_low(LED1); 230 output_low(LED1);
219 output_high(LED2); 231 output_high(LED2);
-   232 beep(100,200);
220 delay_ms(1000); 233 delay_ms(1000);
221 while(true) 234 while(true)
222 { 235 {
223 diagnostika(); 236 diagnostika();
224 } 237 }
Line 226... Line 239...
226 if(BUMPR) 239 if(BUMPR)
227 { 240 {
228 printf("MOD: STOPOVANI\n"); 241 printf("MOD: STOPOVANI\n");
229 output_low(LED2); 242 output_low(LED2);
230 output_high(LED1); 243 output_high(LED1);
-   244 beep(100,200);
231 delay_ms(1000); 245 delay_ms(1000);
232 while(true) 246 while(true)
233 { 247 {
234   248  
235 } 249 }