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



//******** Robot Camerus pro IstRobot 2007 ************
//"$Id: camerus.c 198 2007-03-18 10:13:14Z kakl $"
//*****************************************************
 
#include ".\camerus.h"
 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery

   #bit TMR0IF    = INTCON.2
   #bit INT0IF    = INTCON.1
   #bit RBIF      = INTCON.0
 
enum stavy {start,rozjezd,jizda,cihla,cil};
stavy stav;       // Kde jsme na trati
int8 cas;         // Cas hrany bila/cerna v radce
int8 stred;       // Vystredeni kolecka
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
 
inline void brzda()
{
   int8 n,i;
 

}
 
#int_EXT
EXT_isr()   // Preruseni od prekazky
{
 
 
   set_pwm1_duty(0);    // reverz (zabrzdi)
   set_pwm2_duty(0);
   output_high(MOT_L);  // vzad
   output_high(MOT_R);
   // Ujistime se, ze prijaty signal je z naseho IR vysilace

 
   set_pwm1_duty(140);  // vpred
   set_pwm2_duty(140);
   output_low(MOT_L);
   output_low(MOT_R);
 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
   while(true)
   {
      if(!input(PROXIMITY)) break; // Je cihla blizko?
 
 
      if(get_timer1()>=odocounter+7) return; // nedojeli jsme k cihle, jed dal
   };
 
 
   set_pwm1_duty(0);    // reverz (zabrzdi)
   set_pwm2_duty(0);
   output_high(MOT_L);
   output_high(MOT_R);
   delay_ms(150);

   i=0;
   disp(i);
}
 
 
//---------------------------------- MAIN --------------------------------------
void main()
{
   int8 offset;   // Promena pro ulozeni ovsetu
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
   int8 r1;       // Rychlost motoru 1

   int16 trasa;   // Pocitadlo ujete vzdalenosti
 
   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_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
 
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
 
   set_pwm1_duty(0);   // Zastav motory
   set_pwm2_duty(0);
   output_low(MOT_L);
   output_low(MOT_R);

      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
   }
 
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
   delay_ms(1);
   offset=read_adc();
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
//   offset += 0x70;         // Jas nebude nikdy nizsi
   disp(offset);
   i2c_start();            // Brightness
   i2c_write(0xC0);

         stred=read_adc();
         if(!input(PROXIMITY))
         {
            disp(0x80);
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
            set_timer1(0);         // Vynuluj citac odometrie
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
            set_pwm2_duty(255);
            disp(0x1);
            delay_ms(300);
            stav=jizda;

Rev 197	Rev 198
Line 1...	Line 1...
1	`//****** Robot Camerus pro IstRobot 2007 **********`	1	`//****** Robot Camerus pro IstRobot 2007 **********`
2	`//"$Id: camerus.c 197 2007-03-18 08:30:32Z kakl $"`	2	`//"$Id: camerus.c 198 2007-03-18 10:13:14Z 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 41...	Line 41...
41	`#bit TMR0IF = INTCON.2`	41	`#bit TMR0IF = INTCON.2`
42	`#bit INT0IF = INTCON.1`	42	`#bit INT0IF = INTCON.1`
43	`#bit RBIF = INTCON.0`	43	`#bit RBIF = INTCON.0`
44		44
45	`enum stavy {start,rozjezd,jizda,cihla,cil};`	45	`enum stavy {start,rozjezd,jizda,cihla,cil};`
46	`stavy stav; // Kde jsme na trati`	46	`stavy stav; // Kde jsme na trati`
47	`int8 cas; // Cas hrany bila/cerna v radce`	47	`int8 cas; // Cas hrany bila/cerna v radce`
48	`int8 stred; // vystredeni kolecka`	48	`int8 stred; // Vystredeni kolecka`
-		49	`int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie`
49		50
50	`inline void brzda()`	51	`inline void brzda()`
51	`{`	52	`{`
52	`int8 n,i;`	53	`int8 n,i;`
53		54
Line 93...	Line 94...
93	`}`	94	`}`
94		95
95	`#int_EXT`	96	`#int_EXT`
96	`EXT_isr() // Preruseni od prekazky`	97	`EXT_isr() // Preruseni od prekazky`
97	`{`	98	`{`
98	`int8 i;`	-
99		-
100	`set_pwm1_duty(0); // reverz (zabrzdi)`	99	`set_pwm1_duty(0); // reverz (zabrzdi)`
101	`set_pwm2_duty(0);`	100	`set_pwm2_duty(0);`
102	`output_high(MOT_L); // vzad`	101	`output_high(MOT_L); // vzad`
103	`output_high(MOT_R);`	102	`output_high(MOT_R);`
104	`// Ujistime se, ze prijaty signal je z naseho IR vysilace`	103	`// Ujistime se, ze prijaty signal je z naseho IR vysilace`
Line 123...	Line 122...
123		122
124	`set_pwm1_duty(140); // vpred`	123	`set_pwm1_duty(140); // vpred`
125	`set_pwm2_duty(140);`	124	`set_pwm2_duty(140);`
126	`output_low(MOT_L);`	125	`output_low(MOT_L);`
127	`output_low(MOT_R);`	126	`output_low(MOT_R);`
128	`i=0;`	127
-		128	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
129	`while(true)`	129	`while(true)`
130	`{`	130	`{`
131	`while(input(ODO)) if(!input(PROXIMITY)) goto brzdi; // Je cihla blizko?`	131	`if(!input(PROXIMITY)) break; // Je cihla blizko?`
132	`while(!input(ODO));`	-
133	`i++;`	-
134	`if(i==7) return; // nedojeli jsme k cihle, jed dal`	132	`if(get_timer1()>=odocounter+7) return; // nedojeli jsme k cihle, jed dal`
135	`};`	133	`};`
136		-
137	`brzdi:`	-
138	`set_pwm1_duty(0); // reverz (zabrzdi)`	134	`set_pwm1_duty(0); // reverz (zabrzdi)`
139	`set_pwm2_duty(0);`	135	`set_pwm2_duty(0);`
140	`output_high(MOT_L);`	136	`output_high(MOT_L);`
141	`output_high(MOT_R);`	137	`output_high(MOT_R);`
142	`delay_ms(150);`	138	`delay_ms(150);`
Line 191...	Line 187...
191	`i=0;`	187	`i=0;`
192	`disp(i);`	188	`disp(i);`
193	`}`	189	`}`
194		190
195		191
-		192	`//---------------------------------- MAIN --------------------------------------`
196	`void main()`	193	`void main()`
197	`{`	194	`{`
198	`int8 offset; // Promena pro ulozeni ovsetu`	195	`int8 offset; // Promena pro ulozeni ovsetu`
199	`int8 rr; // Promenna na ulozeni Rozumne rychlost`	196	`int8 rr; // Promenna na ulozeni Rozumne rychlost`
200	`int8 r1; // Rychlost motoru 1`	197	`int8 r1; // Rychlost motoru 1`
Line 202...	Line 199...
202	`int16 trasa; // Pocitadlo ujete vzdalenosti`	199	`int16 trasa; // Pocitadlo ujete vzdalenosti`
203		200
204	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`	201	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`
205	`setup_adc(ADC_CLOCK_INTERNAL);`	202	`setup_adc(ADC_CLOCK_INTERNAL);`
206	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`	203	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`
207	`// setup_timer_1(T1_DISABLED);`	-
208	`setup_timer_1(T1_EXTERNAL_SYNC); // Cita pulzy z odometrie z praveho kola`	204	`setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola`
209	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`	205	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`
210	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`	206	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`
211	`setup_ccp2(CCP_PWM); // RC2`	207	`setup_ccp2(CCP_PWM); // RC2`
212	`setup_comparator(NC_NC_NC_NC);`	208	`setup_comparator(NC_NC_NC_NC);`
213	`setup_vref(FALSE);`	209	`setup_vref(FALSE);`
214		210
215	`set_tris_c(0b11111000); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky`	211	`set_tris_c(0b11111001); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky`
216		212
217	`set_pwm1_duty(0); // Zastav motory`	213	`set_pwm1_duty(0); // Zastav motory`
218	`set_pwm2_duty(0);`	214	`set_pwm2_duty(0);`
219	`output_low(MOT_L);`	215	`output_low(MOT_L);`
220	`output_low(MOT_R);`	216	`output_low(MOT_R);`
Line 311...	Line 307...
311	`delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu`	307	`delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu`
312	`}`	308	`}`
313		309
314	`set_adc_channel(CERVENA); // --- Kroutitko pro jas ---`	310	`set_adc_channel(CERVENA); // --- Kroutitko pro jas ---`
315	`delay_ms(1);`	311	`delay_ms(1);`
316	`offset=read_adc();`	312	`offset=read_adc();`
317	`offset &= 0b11111100; // Dva nejnizsi bity ignoruj`	313	`offset &= 0b11111100; // Dva nejnizsi bity ignoruj`
318	`// offset += 0x70; // Jas nebude nikdy nizsi`	314	`// offset += 0x70; // Jas nebude nikdy nizsi`
319	`disp(offset);`	315	`disp(offset);`
320	`i2c_start(); // Brightness`	316	`i2c_start(); // Brightness`
321	`i2c_write(0xC0);`	317	`i2c_write(0xC0);`
Line 405...	Line 401...
405	`stred=read_adc();`	401	`stred=read_adc();`
406	`if(!input(PROXIMITY))`	402	`if(!input(PROXIMITY))`
407	`{`	403	`{`
408	`disp(0x80);`	404	`disp(0x80);`
409	`while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc`	405	`while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc`
-		406	`set_timer1(0); // Vynuluj citac odometrie`
410	`set_pwm1_duty(255); // Rychly rozjezd !!! Zkontrolovat na oscyloskopu`	407	`set_pwm1_duty(255); // Rychly rozjezd !!! Zkontrolovat na oscyloskopu`
411	`set_pwm2_duty(255);`	408	`set_pwm2_duty(255);`
412	`disp(0x1);`	409	`disp(0x1);`
413	`delay_ms(300);`	410	`delay_ms(300);`
414	`stav=jizda;`	411	`stav=jizda;`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 236 – Rev 197 → 198