Subversion Repositories svnkaklik

Rev

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

Rev 246 Rev 247
Line 1... Line 1...
1
//********* Robot Camerus pro IstRobot 2007 ************
1
//********* Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 246 2007-04-22 15:43:09Z kakl $"
2
//"$Id: camerus.c 247 2007-04-22 19:31:58Z 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
8
 
8
 
9
// Rychlostni konstanty
9
// Rychlostni konstanty
10
#define RR_CIHLA     30       // Rozumna rychlost pro objizdeni cihly
10
#define RR_CIHLA     30       // Rozumna rychlost pro objizdeni cihly
11
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
11
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
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 0xFFF
14
#define ODODO2 0x5E
14
#define ODODO2 0xFFF
15
 
15
 
16
// Adresy IIC periferii
16
// Adresy IIC periferii
17
#define COMPAS_ADR   0xC0
17
#define COMPAS_ADR   0xC0
18
#define CAMERA_ADR   0xDC
18
#define CAMERA_ADR   0xDC
19
#define SONAR_ADR    0xE0
19
#define SONAR_ADR    0xE0
Line 211... Line 211...
211
 
211
 
212
//---------------------------- INT --------------------------------
212
//---------------------------- INT --------------------------------
213
#int_EXT
213
#int_EXT
214
EXT_isr()   // Preruseni od prekazky
214
EXT_isr()   // Preruseni od prekazky
215
{
215
{
216
   int8 bearing, bearing_offset, delta_bearing;
216
   unsigned int8 bearing, bearing_offset, delta_bearing;
217
   
217
 
218
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
218
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
219
   set_pwm2_duty(0);
219
   set_pwm2_duty(0);
220
   output_high(MOT_L);
220
   output_high(MOT_L);
221
   output_low(MOT_R);
221
   output_low(MOT_R);
Line 293... Line 293...
293
   output_low(MOT_L);   // Nastav smer vpred
293
   output_low(MOT_L);   // Nastav smer vpred
294
   output_low(MOT_R);
294
   output_low(MOT_R);
295
 
295
 
296
   disp(0);    // Zhasni LEDbar
296
   disp(0);    // Zhasni LEDbar
297
   
297
 
-
 
298
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
-
 
299
   {
-
 
300
      int8 n;
-
 
301
 
-
 
302
      for(n=0;n<255;n++) write_eeprom(n,0);
-
 
303
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
-
 
304
      bb_h[0]=0;
-
 
305
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
-
 
306
      for(n=0;n<=4;n++)
298
/*!!!!
307
      {
-
 
308
         disp(0x55);       // Blikni pro potvrzeni
-
 
309
         delay_ms(200);
-
 
310
         disp(0xAA);
-
 
311
         delay_ms(200);
-
 
312
      };
299
while(true)
313
      while(true)
300
{
314
      {
-
 
315
         if(!IRRX)
-
 
316
         {
301
   int8 ble;
317
            int8 ble;
302
 
318
         
303
   i2c_start();     // Cteni kompasu
319
            i2c_start();     // Cteni kompasu
304
   i2c_write(COMPAS_ADR);
320
            i2c_write(COMPAS_ADR);
305
   i2c_write(0x1);
321
            i2c_write(0x1);
Line 309... Line 325...
309
   ble=i2c_read(0);
325
            ble=i2c_read(0);
310
   i2c_stop();
326
            i2c_stop();
311
   disp(ble);
327
            disp(ble);
312
   delay_ms(200);
328
            delay_ms(200);
313
}
329
         }
314
//*/           
-
 
315
 
-
 
316
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
-
 
317
   {
-
 
318
      int8 n;
-
 
319
 
-
 
320
      for(n=0;n<255;n++) write_eeprom(n,0);
-
 
321
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
-
 
322
      bb_h[0]=0;
-
 
323
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
-
 
324
      while(true)
-
 
325
      {
-
 
326
         if(!IRRX)
-
 
327
         {
-
 
328
            disp(0x55);       // Blikni pro potvrzeni
-
 
329
            delay_ms(200);
-
 
330
            disp(0xAA);
-
 
331
            delay_ms(200);
-
 
332
         }
-
 
333
         else
330
         else
334
         {
331
         {
335
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
332
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
336
            delay_ms(200);
333
            delay_ms(200);
337
         }
334
         }
Line 636... Line 633...
636
      {
633
      {
637
         if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
634
         if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
638
         {
635
         {
639
            bum();
636
            bum();
640
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
637
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
641
            set_pwm1_duty(140);   // pomalu vpred
638
            set_pwm1_duty(160);   // pomalu vpred
642
            set_pwm2_duty(140);
639
            set_pwm2_duty(160);
643
            output_low(MOT_L);
640
            output_low(MOT_L);
644
            output_low(MOT_R);
641
            output_low(MOT_R);
645
         };
642
         };
646
         set_adc_channel(LMAX);    // Levy UV sensor
643
         set_adc_channel(LMAX);    // Levy UV sensor
647
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
644
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;