Line 1... |
Line 1... |
1 |
//********* Robot Camerus pro IstRobot 2007 ************
|
1 |
//********* Robot Camerus pro IstRobot 2007 ************
|
2 |
//"$Id: camerus.c 254 2007-04-27 13:13:08Z kakl $"
|
2 |
//"$Id: camerus.c 255 2007-04-27 13:14:34Z 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 25 // Rozumna rychlost pro objizdeni cihly
|
10 |
#define RR_CIHLA 50 // Rozumna rychlost pro objizdeni cihly
|
11 |
#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni
|
11 |
#define RR_PRERUSENI 30 // 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 TUHOS 100 // Jak dlouho se bude couvat po narazu na naraznik
|
14 |
#define ODODO_CIHLA 0xFFF
|
14 |
#define ODODO_CIHLA 0xFFF
|
15 |
#define ODODO_TUNEL 0xFFF
|
15 |
#define ODODO_TUNEL 0xFFF
|
16 |
#define ODODO_PRERUSENI 0xFFF
|
16 |
#define ODODO_PRERUSENI 0xFFF
|
Line 307... |
Line 307... |
307 |
|
307 |
|
308 |
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
|
309 |
setup_adc(ADC_CLOCK_INTERNAL);
|
309 |
setup_adc(ADC_CLOCK_INTERNAL);
|
310 |
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
|
311 |
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
|
312 |
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
|
313 |
//!!! setup_timer_2(T2_DIV_BY_4,255,1); // Casovac PWM motoru
|
314 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
|
314 |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory
|
315 |
setup_ccp2(CCP_PWM); // RC2
|
315 |
setup_ccp2(CCP_PWM); // RC2
|
316 |
setup_comparator(NC_NC_NC_NC);
|
316 |
setup_comparator(NC_NC_NC_NC);
|
317 |
setup_vref(FALSE);
|
317 |
setup_vref(FALSE);
|
318 |
|
318 |
|
Line 447... |
Line 447... |
447 |
i2c_stop();
|
447 |
i2c_stop();
|
448 |
delay_ms(1000); // Nech hodnotu chvili na displayi
|
448 |
delay_ms(1000); // Nech hodnotu chvili na displayi
|
449 |
|
449 |
|
450 |
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
|
450 |
set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---
|
451 |
delay_ms(1);
|
451 |
delay_ms(1);
|
452 |
//!!! rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
452 |
rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
- |
|
453 |
rr+=27; // 27-90
|
453 |
rr=read_adc()>>3; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
454 |
//!!! rr=read_adc()>>1; // 0-128 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
|
454 |
rrold=rr;
|
455 |
rrold=rr;
|
455 |
|
456 |
|
456 |
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
|
457 |
cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh
|
457 |
// a aby se to rozjelo jeste dneska
|
458 |
// a aby se to rozjelo jeste dneska
|
458 |
stav=start; // Jsme na startu
|
459 |
stav=start; // Jsme na startu
|
Line 552... |
Line 553... |
552 |
// Elektronicky diferencial 2. cast
|
553 |
// Elektronicky diferencial 2. cast
|
553 |
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
|
554 |
if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni
|
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
|
555 |
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
|
555 |
|
556 |
|
556 |
//!!! pro zatuhle prevodovky
|
557 |
//!!! pro zatuhle prevodovky
|
557 |
r1<<=1; // Rychlost je dvojnasobna
|
558 |
// r1<<=1; // Rychlost je dvojnasobna
|
558 |
r2<<=1; // Rozsah 2 az 184 pro rr=0
|
559 |
// r2<<=1; // Rozsah 2 az 184 pro rr=0
|
559 |
|
560 |
|
560 |
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
|
561 |
if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda
|
561 |
{
|
562 |
{
|
562 |
set_pwm1_duty(r1);
|
563 |
set_pwm1_duty(r1);
|
563 |
set_pwm2_duty(r2);
|
564 |
set_pwm2_duty(r2);
|