| Line 1... |
Line 1... |
| 1 |
//********* Robot Camerus pro IstRobot 2007 ************
|
1 |
//********* Robot Camerus pro IstRobot 2007 ************
|
| 2 |
//"$Id: camerus.c 249 2007-04-23 12:26:15Z kakl $"
|
2 |
//"$Id: camerus.c 252 2007-04-24 08:53:01Z 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 25 // 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 TUHOS 100 // Jak dlouho se bude couvat po narazu na naraznik
|
| 13 |
#define ODODO_CIHLA 0xFFF
|
14 |
#define ODODO_CIHLA 0xFFF
|
| 14 |
#define ODODO_TUNEL 0xFFF
|
15 |
#define ODODO_TUNEL 0xFFF
|
| 15 |
#define ODODO_PRERUSENI 0xFFF
|
16 |
#define ODODO_PRERUSENI 0xFFF
|
| 16 |
|
17 |
|
| 17 |
// Adresy IIC periferii
|
18 |
// Adresy IIC periferii
|
| Line 306... |
Line 307... |
| 306 |
|
307 |
|
| 307 |
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek
|
308 |
setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek
|
| 308 |
setup_adc(ADC_CLOCK_INTERNAL);
|
309 |
setup_adc(ADC_CLOCK_INTERNAL);
|
| 309 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce
|
310 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce
|
| 310 |
setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola
|
311 |
setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola
|
| - |
|
312 |
//!!! setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru
|
| 311 |
setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru
|
313 |
setup_timer_2(T2_DIV_BY_4,255,1); // Casovac PWM motoru
|
| 312 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
|
314 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
|
| 313 |
setup_ccp2(CCP_PWM); // RC2
|
315 |
setup_ccp2(CCP_PWM); // RC2
|
| 314 |
setup_comparator(NC_NC_NC_NC);
|
316 |
setup_comparator(NC_NC_NC_NC);
|
| 315 |
setup_vref(FALSE);
|
317 |
setup_vref(FALSE);
|
| 316 |
|
318 |
|
| Line 445... |
Line 447... |
| 445 |
i2c_stop();
|
447 |
i2c_stop();
|
| 446 |
delay_ms(1000); // Nech hodnotu chvili na displayi
|
448 |
delay_ms(1000); // Nech hodnotu chvili na displayi
|
| 447 |
|
449 |
|
| 448 |
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
|
450 |
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
|
| 449 |
delay_ms(1);
|
451 |
delay_ms(1);
|
| - |
|
452 |
//!!! rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
| 450 |
rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
453 |
rr=read_adc()>>3; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
| 451 |
rrold=rr;
|
454 |
rrold=rr;
|
| 452 |
|
455 |
|
| 453 |
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
|
456 |
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
|
| 454 |
// a aby se to rozjelo jeste dneska
|
457 |
// a aby se to rozjelo jeste dneska
|
| 455 |
stav=start; // Jsme na startu
|
458 |
stav=start; // Jsme na startu
|
| Line 546... |
Line 549... |
| 546 |
if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
|
549 |
if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
|
| 547 |
if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
|
550 |
if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
|
| 548 |
|
551 |
|
| 549 |
// Elektronicky diferencial 2. cast
|
552 |
// Elektronicky diferencial 2. cast
|
| 550 |
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
|
553 |
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
|
| 551 |
if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); // rozsah 1 az 92 pro rr=0
|
- |
|
| 552 |
// rozsah 1 az 154 pro rr=63
|
554 |
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
|
| 553 |
|
555 |
|
| - |
|
556 |
//!!! pro zatuhle prevodovky
|
| 554 |
// r1<<=1; // Rychlost je dvojnasobna
|
557 |
r1<<=1; // Rychlost je dvojnasobna
|
| 555 |
// r2<<=1; // Rozsah 2 az 184 pro rr=0
|
558 |
r2<<=1; // Rozsah 2 az 184 pro rr=0
|
| 556 |
|
559 |
|
| 557 |
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
|
560 |
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
|
| 558 |
{
|
561 |
{
|
| 559 |
set_pwm1_duty(r1);
|
562 |
set_pwm1_duty(r1);
|
| 560 |
set_pwm2_duty(r2);
|
563 |
set_pwm2_duty(r2);
|
| Line 609... |
Line 612... |
| 609 |
{
|
612 |
{
|
| 610 |
if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
|
613 |
if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
|
| 611 |
{
|
614 |
{
|
| 612 |
bum();
|
615 |
bum();
|
| 613 |
SaveLog(log-1); // Zapis Black Boxu do EEPROM
|
616 |
SaveLog(log-1); // Zapis Black Boxu do EEPROM
|
| - |
|
617 |
delay_ms(TUHOS); //!!! Zatuhle prevodovky
|
| 614 |
set_pwm1_duty(160); // pomalu vpred
|
618 |
set_pwm1_duty(200); // pomalu vpred
|
| 615 |
set_pwm2_duty(160);
|
619 |
set_pwm2_duty(200);
|
| 616 |
output_low(MOT_L);
|
620 |
output_low(MOT_L);
|
| 617 |
output_low(MOT_R);
|
621 |
output_low(MOT_R);
|
| 618 |
cas=CASAVR-CASMIN;
|
622 |
cas=CASAVR-CASMIN;
|
| 619 |
};
|
623 |
};
|
| 620 |
set_adc_channel(LMAX); // Levy UV sensor
|
624 |
set_adc_channel(LMAX); // Levy UV sensor
|