Subversion Repositories svnkaklik

Rev

Rev 246 | Go to most recent revision | Show entire file | Ignore 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);
222 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
 222 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
Line 292... Line 292...
292 
   set_pwm2_duty(0);
 292 
   set_pwm2_duty(0);
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
- 
 
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 
 
 297 
 
316 
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
 298 
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
317 
   {
 299 
   {
318 
      int8 n;
 300 
      int8 n;
319 
 
 301 
 
320 
      for(n=0;n<255;n++) write_eeprom(n,0);
 302 
      for(n=0;n<255;n++) write_eeprom(n,0);
321 
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
 303 
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
322 
      bb_h[0]=0;
 304 
      bb_h[0]=0;
323 
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
 305 
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
- 
 
 306 
      for(n=0;n<=4;n++)
- 
 
 307 
      {
- 
 
 308 
         disp(0x55);       // Blikni pro potvrzeni
- 
 
 309 
         delay_ms(200);
- 
 
 310 
         disp(0xAA);
- 
 
 311 
         delay_ms(200);
- 
 
 312 
      };
324 
      while(true)
 313 
      while(true)
325 
      {
 314 
      {
326 
         if(!IRRX)
 315 
         if(!IRRX)
327 
         {
 316 
         {
- 
 
 317 
            int8 ble;
- 
 
 318
328 
            disp(0x55);       // Blikni pro potvrzeni
 319 
            i2c_start();     // Cteni kompasu
- 
 
 320 
            i2c_write(COMPAS_ADR);
- 
 
 321 
            i2c_write(0x1);
- 
 
 322 
            i2c_stop();
- 
 
 323 
            i2c_start();
- 
 
 324 
            i2c_write(COMPAS_ADR+1);
- 
 
 325 
            ble=i2c_read(0);
329 
            delay_ms(200);
 326 
            i2c_stop();
330 
            disp(0xAA);
 327 
            disp(ble);
331 
            delay_ms(200);
 328 
            delay_ms(200);
332 
         }
 329 
         }
333 
         else
 330 
         else
334 
         {
 331 
         {
335 
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
 332 
            disp(0x81);       // Diagnostika celniho IR senzoru na prekazku
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;