Subversion Repositories svnkaklik

Rev

Rev 260 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 260 Rev 360
Line 1... Line 1...
1
//********* Robot Camerus pro IstRobot 2007 ************
1
//********* Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 260 2007-04-27 13:27:40Z kakl $"
2
//"$Id: camerus.c 360 2007-09-09 15:07:21Z 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 9... Line 9...
9
// Rychlostni konstanty
9
// Rychlostni konstanty
10
#define RR_CIHLA     50       // Rozumna rychlost pro objizdeni cihly
10
#define RR_CIHLA     50       // Rozumna rychlost pro objizdeni cihly
11
#define RR_PRERUSENI 50       // Rozumna rychlost pro priblizeni se k preruseni
11
#define RR_PRERUSENI 50       // Rozumna rychlost pro priblizeni se k preruseni
12
#define BRZDNA_DRAHA 0x15     // Jak daleko pred problemem se zacne brzdit
12
#define BRZDNA_DRAHA 0x15     // Jak daleko pred problemem se zacne brzdit
13
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
13
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
14
#define ODODO_CIHLA       0xD0
14
#define ODODO_PROBLEM1     0xFFF
15
#define ODODO_TUNEL       0xFFF
15
#define ODODO_PROBLEM2     0xFFF
16
#define ODODO_PRERUSENI   0xFFF//0xB4
16
#define ODODO_PROBLEM3     0xFFF
17
 
17
 
18
// Adresy IIC periferii
18
// Adresy IIC periferii
19
#define COMPAS_ADR   0xC0
19
#define COMPAS_ADR   0xC0
20
#define CAMERA_ADR   0xDC
20
#define CAMERA_ADR   0xDC
21
#define SONAR_ADR    0xE0
21
#define SONAR_ADR    0xE0
Line 80... Line 80...
80
int8 bb_f[MAXLOG];  // Cerna skrinka priznak (typ zaznamu)
80
int8 bb_f[MAXLOG];  // Cerna skrinka priznak (typ zaznamu)
81
int8 log;         // Pocitadlo pro cernou skrinku
81
int8 log;         // Pocitadlo pro cernou skrinku
82
int8 rr;          // Promenna na ulozeni Rozumne rychlost
82
int8 rr;          // Promenna na ulozeni Rozumne rychlost
83
int8 rrold;
83
int8 rrold;
84
int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati
84
int16 odo_preruseni, odo_cihla, odo_tunel; // Problemy na trati
-
 
85
int16 odo_problem1, odo_problem2, odo_problem3; // Problemy na trati
85
 
86
 
86
// Zobrazeni jednoho byte na modulu LEDbar
87
// Zobrazeni jednoho byte na modulu LEDbar
87
inline void disp(int8 x)
88
inline void disp(int8 x)
88
{
89
{
89
   int n;
90
   int n;
Line 461... Line 462...
461
   stav=start;          // Jsme na startu
462
   stav=start;          // Jsme na startu
462
   set_timer1(0);       // Vynuluj citac odometrie
463
   set_timer1(0);       // Vynuluj citac odometrie
463
   log=0;               // Zacatek logu v cerne skrince
464
   log=0;               // Zacatek logu v cerne skrince
464
   last_log_odo=0;      // Posledni zaznam odometrie do logu
465
   last_log_odo=0;      // Posledni zaznam odometrie do logu
465
 
466
 
466
//   ReadBlackBox();    // Vycteni zaznamu z Black Boxu
467
   ReadBlackBox();    // Vycteni zaznamu z Black Boxu
467
 
468
 
468
   odo_cihla=ODODO_CIHLA-BRZDNA_DRAHA;
469
   odo_problem1=ODODO_PROBLEM1-BRZDNA_DRAHA;
469
   odo_tunel=ODODO_TUNEL-BRZDNA_DRAHA;
470
   odo_problem2=ODODO_PROBLEM2-BRZDNA_DRAHA;
470
   odo_preruseni=ODODO_PRERUSENI-BRZDNA_DRAHA;
471
   odo_problem3=ODODO_PROBLEM3-BRZDNA_DRAHA;
471
 
472
 
472
   // ........................... Hlavni smycka ................................
473
   // ........................... Hlavni smycka ................................
473
   while(true)
474
   while(true)
474
   {
475
   {
475
      int8 pom;
476
      int8 pom;
Line 571... Line 572...
571
//ODODO
572
//ODODO
572
      ododo=get_timer1();
573
      ododo=get_timer1();
573
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
574
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
574
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
575
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
575
      if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
576
      if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
-
 
577
      if((ododo>odo_problem1)&&(ododo<(odo_problem1+8))) rr=RR_PRERUSENI;
-
 
578
      if((ododo>odo_problem2)&&(ododo<(odo_problem2+8))) rr=RR_PRERUSENI;
-
 
579
      if((ododo>odo_problem3)&&(ododo<(odo_problem3+8))) rr=RR_PRERUSENI;
576
 
580
 
577
      // Elektronicky diferencial 2. cast
581
      // Elektronicky diferencial 2. cast
578
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
582
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
579
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0                                                                   // rozsah 1 az 154 pro rr=63
583
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0                                                                   // rozsah 1 az 154 pro rr=63
580
 
584