| 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 |
|