Subversion Repositories svnkaklik

Rev

Rev 239 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log

Rev 239 Rev 246
Line 1... Line 1...
1 
//********* Robot Camerus pro IstRobot 2007 ************
 1 
//********* Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 239 2007-04-18 21:51:25Z kakl $"
 2 
//"$Id: camerus.c 246 2007-04-22 15:43:09Z kakl $"
3 
//*****************************************************
 3 
//*****************************************************
4 
 
 4 
 
5 
#include ".\camerus.h"
 5 
#include ".\camerus.h"
6 
 
 6 
 
7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
Line 12... Line 12...
12 
#define BRZDNA_DRAHA 0x20        // Jak daleko pred problemem se zacne brzdit
 12 
#define BRZDNA_DRAHA 0x20        // Jak daleko pred problemem se zacne brzdit
13 
#define ODODO1 0x37
13 
#define ODODO1 0x37
14 
#define ODODO2 0x5E
14 
#define ODODO2 0x5E
15 
 
 15 
 
16 
// Adresy IIC periferii
 16 
// Adresy IIC periferii
- 
 
 17 
#define COMPAS_ADR   0xC0
17 
#define CAMERA_ADR   0xC0
 18 
#define CAMERA_ADR   0xDC
18 
#define SONAR_ADR    0xE0
 19 
#define SONAR_ADR    0xE0
19 
 
 20 
 
20 
// A/D vstupy
 21 
// A/D vstupy
21 
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
 22 
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
22 
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
 23 
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
Line 210... Line 211...
210 
 
 211 
 
211 
//---------------------------- INT --------------------------------
 212 
//---------------------------- INT --------------------------------
212 
#int_EXT
 213 
#int_EXT
213 
EXT_isr()   // Preruseni od prekazky
 214 
EXT_isr()   // Preruseni od prekazky
214 
{
 215 
{
- 
 
 216 
   int8 bearing, bearing_offset, delta_bearing;
- 
 
 217
215 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
 218 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
216 
   set_pwm2_duty(0);
 219 
   set_pwm2_duty(0);
217 
   output_high(MOT_L);
 220 
   output_high(MOT_L);
218 
   output_low(MOT_R);
 221 
   output_low(MOT_R);
219 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
 222 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
Line 224... Line 227...
224 
      output_low(MOT_L); // je odraz -> vpred
 227 
      output_low(MOT_L); // je odraz -> vpred
225 
      output_low(MOT_R);
 228 
      output_low(MOT_R);
226 
      return;
 229 
      return;
227 
   };
 230 
   };
228 
   output_low(IRTX);    // Zapni LED na detekci prekazky
 231 
   output_low(IRTX);    // Zapni LED na detekci prekazky
- 
 
 232 
 
- 
 
 233 
   i2c_start();     // Cteni kompasu
- 
 
 234 
   i2c_write(COMPAS_ADR);
- 
 
 235 
   i2c_write(0x1);
- 
 
 236 
   i2c_stop();
- 
 
 237 
   i2c_start();
- 
 
 238 
   i2c_write(COMPAS_ADR+1);
- 
 
 239 
   bearing_offset=i2c_read(0);
- 
 
 240 
   i2c_stop();
- 
 
 241 
 
229 
   delay_ms(10);
 242 
   delay_ms(9);
230 
   if (!IRRX)     // stale nas signal?
 243 
   if (!IRRX)     // stale nas signal?
231 
   {
 244 
   {
232 
      output_low(MOT_L); // neni odraz -> vpred
 245 
      output_low(MOT_L); // neni odraz -> vpred
233 
      output_low(MOT_R);
 246 
      output_low(MOT_R);
234 
      return;
 247 
      return;
Line 241... Line 254...
241 
      bb_l[log]=make8(timer_pom,0); // Zaznam do logu
 254 
      bb_l[log]=make8(timer_pom,0); // Zaznam do logu
242 
      bb_h[log]=make8(timer_pom,1);
255 
      bb_h[log]=make8(timer_pom,1);
243 
      bb_f[log]=0xFF;   // Cihla
 256 
      bb_f[log]=0xFF;   // Cihla
244 
      if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
 257 
      if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
245 
   };
 258 
   };
246 
//!!! Vyzkouset, ze nepreteka LOG
 - 
 
247 
 
 259 
 
248 
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
 260 
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
249 
//   if(stav==cihla) return; // Po druhe nic neobjizdej
 261 
//   if(stav==cihla) return; // Po druhe nic neobjizdej
250 
// Pozor na rozjezd
 262 
// Pozor na rozjezd
251 
 
 263 
 
Line 279... Line 291...
279 
   set_pwm1_duty(0);    // Zastav motory
 291 
   set_pwm1_duty(0);    // Zastav motory
280 
   set_pwm2_duty(0);
 292 
   set_pwm2_duty(0);
281 
   output_low(MOT_L);   // Nastav smer vpred
 293 
   output_low(MOT_L);   // Nastav smer vpred
282 
   output_low(MOT_R);
 294 
   output_low(MOT_R);
283 
 
 295 
 
- 
 
 296 
   disp(0);    // Zhasni LEDbar
- 
 
 297
- 
 
 298 
/*!!!!
- 
 
 299 
while(true)
- 
 
 300 
{
- 
 
 301 
   int8 ble;
- 
 
 302 
 
- 
 
 303 
   i2c_start();     // Cteni kompasu
- 
 
 304 
   i2c_write(COMPAS_ADR);
- 
 
 305 
   i2c_write(0x1);
- 
 
 306 
   i2c_stop();
- 
 
 307 
   i2c_start();
- 
 
 308 
   i2c_write(COMPAS_ADR+1);
- 
 
 309 
   ble=i2c_read(0);
- 
 
 310 
   i2c_stop();
- 
 
 311 
   disp(ble);
- 
 
 312 
   delay_ms(200);
- 
 
 313 
}
- 
 
 314 
//*/
- 
 
 315 
 
284 
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
 316 
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
285 
   {
 317 
   {
286 
      int8 n;
 318 
      int8 n;
287 
 
 319 
 
288 
      for(n=0;n<255;n++) write_eeprom(n,0);
 320 
      for(n=0;n<255;n++) write_eeprom(n,0);
289 
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
 321 
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
290 
      bb_h[0]=0;
 322 
      bb_h[0]=0;
291 
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
 323 
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
292 
      while(true)
 324 
      while(true)
293 
      {
 325 
      {
- 
 
 326 
         if(!IRRX)
- 
 
 327 
         {
294 
         disp(0x55);       // Blikni pro potvrzeni
 328 
            disp(0x55);       // Blikni pro potvrzeni
295 
         delay_ms(200);
 329 
            delay_ms(200);
296 
         disp(0xAA);
 330 
            disp(0xAA);
297 
         delay_ms(200);
 331 
            delay_ms(200);
298 
      }
 332 
         }
- 
 
 333 
         else
- 
 
 334 
         {
- 
 
 335 
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
- 
 
 336 
            delay_ms(200);
- 
 
 337 
         }
- 
 
 338 
      }
299 
   }
 339 
   }
300 
 
 340 
 
301 
   output_low(IRTX);   // Zapni LED na detekci prekazky
 341 
   output_low(IRTX);   // Zapni LED na detekci prekazky
302 
 
 342 
 
303 
   NightRider(1);    // Aby se poznalo, ze byl RESET
 343 
   NightRider(1);    // Aby se poznalo, ze byl RESET
Line 337... Line 377...
337 
   }
 377 
   }
338 
*/
 378 
*/
339 
 
 379 
 
340 
   //... Nastaveni kamery ...
 380 
   //... Nastaveni kamery ...
341 
   i2c_start();      // Soft RESET kamery
 381 
   i2c_start();      // Soft RESET kamery
342 
   i2c_write(CAMERA_ADR);        // Pro single slave musi mit vsechny zapisy adresu C0h
 382 
   i2c_write(CAMERA_ADR);        // Adresa kamery
343 
   i2c_write(0x12);        // Adresa registru COMH
 383 
   i2c_write(0x12);        // Adresa registru COMH
344 
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
 384 
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
345 
   i2c_stop();
 385 
   i2c_stop();
346 
 
 386 
 
347 
   i2c_start();      // BW
 387 
   i2c_start();      // BW
Line 350... Line 390...
350 
   i2c_write(0b01000001);
 390 
   i2c_write(0b01000001);
351 
   i2c_stop();
 391 
   i2c_stop();
352 
 
 392 
 
353 
/*
 393 
/*
354 
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
 394 
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
355 
   i2c_write(0xC0);
 395 
   i2c_write(CAMERA_ADR);
356 
   i2c_write(0x05);
 396 
   i2c_write(0x05);
357 
   i2c_write(0xA0);  // 48h
 397 
   i2c_write(0xA0);  // 48h
358 
   i2c_stop();
 398 
   i2c_stop();
359 
 
 399 
 
360 
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
 400 
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
361 
   i2c_write(0xC0);
 401 
   i2c_write(CAMERA_ADR);
362 
   i2c_write(0x2D);
 402 
   i2c_write(0x2D);
363 
   i2c_write(0x04 | 0x03);
 403 
   i2c_write(0x04 | 0x03);
364 
   i2c_stop();
 404 
   i2c_stop();
365 
*/
 405 
*/
366 
 
 406