WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/camerus.c



//********* Robot Camerus pro IstRobot 2007 ************
//"$Id: camerus.c 255 2007-04-27 13:14:34Z kakl $"
//*****************************************************
 
#include ".\camerus.h"
 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#define RR_CIHLA     50       // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 30       // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 0x20     // Jak daleko pred problemem se zacne brzdit
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
#define ODODO_CIHLA       0xFFF
#define ODODO_TUNEL       0xFFF
#define ODODO_PRERUSENI   0xFFF

 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
//!!!   setup_timer_2(T2_DIV_BY_4,255,1);        // Casovac PWM motoru
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
 

   i2c_stop();
   delay_ms(1000);   // Nech hodnotu chvili na displayi
 
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
   delay_ms(1);
   rr=read_adc()>>2; // 0-63  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
   rr+=27;  // 27-90
//!!!   rr=read_adc()>>1; // 0-128 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
   rrold=rr;
 
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
                        // a aby se to rozjelo jeste dneska
   stav=start;          // Jsme na startu

      // Elektronicky diferencial 2. cast
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
      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
 
//!!! pro zatuhle prevodovky
//      r1<<=1;     // Rychlost je dvojnasobna
//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
 
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
      {
         set_pwm1_duty(r1);
         set_pwm2_duty(r2);

Rev 254	Rev 255
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);`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 256 – Rev 254 → 255