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



#define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
#define DATA   PIN_B2      // K modulu LEDbar data
#define CP     PIN_B1      // K modulu LEDbar hodiny
#define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
//#define CIHLA  PIN_B0      // Vstup INT, generuje preruseni pri prekazce
#define IRLED  PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
 
#define CASMIN 6           // Rozsah radku snimace
#define CASMAX 192
#define CASAVR ((CASMAX+CASMIN) / 2)
 

int8 cas;      // Cas hrany bila/cerna v radce
 
void SetServo(int8 angle)
{
   int8 n, offset;
 
   for(n=0; n<20; n++)
   {
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
      Delay_ms(1);
      offset=read_adc();

      delay_us(offset);
      delay_us(angle);
      delay_us(angle);
      output_low(SERVO);
      delay_ms(18);
   }
}
 
#int_EXT
EXT_isr()   // Preruseni od prekazky
{

   if (stav==cihla) while(true); // Zastav na furt
   if(stav==jizda)
   {
      int n;
 
      SetServo(CASAVR-CASMIN);
      set_pwm1_duty(40); // pomalu couvej
      set_pwm2_duty(40);
      output_high(MOT_L);
      output_high(MOT_R);
      n=0;

      set_pwm2_duty(0);
      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(1000);
 
      SetServo((CASAVR-CASMIN)-20);   // doleva
      set_pwm1_duty(150);  // vpred
      set_pwm2_duty(200);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;

      set_pwm1_duty(0);    // reverz (zabrzdi)
      set_pwm2_duty(0);
      output_high(MOT_L);
      output_high(MOT_R);
 
      SetServo((CASAVR-CASMIN));   // rovne
      set_pwm1_duty(140);  // vpred
      set_pwm2_duty(140);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;

      set_pwm2_duty(150);
      output_high(MOT_L);
      output_high(MOT_R);
      delay_ms(1000);
 
      SetServo(CASMIN);   // max. doleva
      set_pwm1_duty(0);  // vzad
      set_pwm2_duty(20);
      output_low(MOT_L);
      output_high(MOT_R);
      stav=cihla;

      output_low(MOT_L);
      output_low(MOT_R);
      delay_ms(500);
 
 
      SetServo((CASAVR-CASMIN)+5);   // mirne doprava
      set_pwm1_duty(180);  // vpred
      set_pwm2_duty(180);
      output_low(MOT_L);
      output_low(MOT_R);
      stav=cihla;

      output_low(MOT_R);
      delay_ms(500);
 
 
      cas=CASMIN;  // Cara je vlevo
 
      stav=cihla;
   }
}
 
// Zobrazeni jednoho byte na modulu LEDbar

   set_pwm1_duty(0);   // Zastav motory
   set_pwm2_duty(0);
   output_low(MOT_L);
   output_low(MOT_R);
 
   output_low(IRLED);   // Zapni LED na detekci prekazky
   
   NightRider(1);    // Aby se poznalo, ze byl RESET
                     // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
 
   //... Nastaveni kamery ...
   i2c_start();      // Soft RESET kamery

      {
         pom>>=1;
      }
      disp(pom);
 
 
 
 
 
   };
}

Rev 184	Rev 186
Line 17...	Line 17...
17	`#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1`	17	`#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1`
18	`#define DATA PIN_B2 // K modulu LEDbar data`	18	`#define DATA PIN_B2 // K modulu LEDbar data`
19	`#define CP PIN_B1 // K modulu LEDbar hodiny`	19	`#define CP PIN_B1 // K modulu LEDbar hodiny`
20	`#define ODO PIN_A4 // Ze snimace z odometrie z praveho kola`	20	`#define ODO PIN_A4 // Ze snimace z odometrie z praveho kola`
21	`//#define CIHLA PIN_B0 // Vstup INT, generuje preruseni pri prekazce`	21	`//#define CIHLA PIN_B0 // Vstup INT, generuje preruseni pri prekazce`
-		22	`#define IRLED PIN_C0 // Modulovani vysilaci IR LED na detekci prekazky`
22		23
23	`#define CASMIN 6 // Rozsah radku snimace`	24	`#define CASMIN 6 // Rozsah radku snimace`
24	`#define CASMAX 192`	25	`#define CASMAX 192`
25	`#define CASAVR ((CASMAX+CASMIN) / 2)`	26	`#define CASAVR ((CASMAX+CASMIN) / 2)`
26		27
Line 39...	Line 40...
39	`int8 cas; // Cas hrany bila/cerna v radce`	40	`int8 cas; // Cas hrany bila/cerna v radce`
40		41
41	`void SetServo(int8 angle)`	42	`void SetServo(int8 angle)`
42	`{`	43	`{`
43	`int8 n, offset;`	44	`int8 n, offset;`
44		45
45	`for(n=0; n<20; n++)`	46	`for(n=0; n<20; n++)`
46	`{`	47	`{`
47	`set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka`	48	`set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka`
48	`Delay_ms(1);`	49	`Delay_ms(1);`
49	`offset=read_adc();`	50	`offset=read_adc();`
Line 55...	Line 56...
55	`delay_us(offset);`	56	`delay_us(offset);`
56	`delay_us(angle);`	57	`delay_us(angle);`
57	`delay_us(angle);`	58	`delay_us(angle);`
58	`output_low(SERVO);`	59	`output_low(SERVO);`
59	`delay_ms(18);`	60	`delay_ms(18);`
60	`}`	61	`}`
61	`}`	62	`}`
62		63
63	`#int_EXT`	64	`#int_EXT`
64	`EXT_isr() // Preruseni od prekazky`	65	`EXT_isr() // Preruseni od prekazky`
65	`{`	66	`{`
Line 73...	Line 74...
73	`if (stav==cihla) while(true); // Zastav na furt`	74	`if (stav==cihla) while(true); // Zastav na furt`
74	`if(stav==jizda)`	75	`if(stav==jizda)`
75	`{`	76	`{`
76	`int n;`	77	`int n;`
77		78
78	`SetServo(CASAVR-CASMIN);`	79	`SetServo(CASAVR-CASMIN);`
79	`set_pwm1_duty(40); // pomalu couvej`	80	`set_pwm1_duty(40); // pomalu couvej`
80	`set_pwm2_duty(40);`	81	`set_pwm2_duty(40);`
81	`output_high(MOT_L);`	82	`output_high(MOT_L);`
82	`output_high(MOT_R);`	83	`output_high(MOT_R);`
83	`n=0;`	84	`n=0;`
Line 97...	Line 98...
97	`set_pwm2_duty(0);`	98	`set_pwm2_duty(0);`
98	`output_low(MOT_L);`	99	`output_low(MOT_L);`
99	`output_low(MOT_R);`	100	`output_low(MOT_R);`
100	`delay_ms(1000);`	101	`delay_ms(1000);`
101		102
102	`SetServo((CASAVR-CASMIN)-20); // doleva`	103	`SetServo((CASAVR-CASMIN)-20); // doleva`
103	`set_pwm1_duty(150); // vpred`	104	`set_pwm1_duty(150); // vpred`
104	`set_pwm2_duty(200);`	105	`set_pwm2_duty(200);`
105	`output_low(MOT_L);`	106	`output_low(MOT_L);`
106	`output_low(MOT_R);`	107	`output_low(MOT_R);`
107	`stav=cihla;`	108	`stav=cihla;`
Line 116...	Line 117...
116	`set_pwm1_duty(0); // reverz (zabrzdi)`	117	`set_pwm1_duty(0); // reverz (zabrzdi)`
117	`set_pwm2_duty(0);`	118	`set_pwm2_duty(0);`
118	`output_high(MOT_L);`	119	`output_high(MOT_L);`
119	`output_high(MOT_R);`	120	`output_high(MOT_R);`
120		121
121	`SetServo((CASAVR-CASMIN)); // rovne`	122	`SetServo((CASAVR-CASMIN)); // rovne`
122	`set_pwm1_duty(140); // vpred`	123	`set_pwm1_duty(140); // vpred`
123	`set_pwm2_duty(140);`	124	`set_pwm2_duty(140);`
124	`output_low(MOT_L);`	125	`output_low(MOT_L);`
125	`output_low(MOT_R);`	126	`output_low(MOT_R);`
126	`stav=cihla;`	127	`stav=cihla;`
Line 141...	Line 142...
141	`set_pwm2_duty(150);`	142	`set_pwm2_duty(150);`
142	`output_high(MOT_L);`	143	`output_high(MOT_L);`
143	`output_high(MOT_R);`	144	`output_high(MOT_R);`
144	`delay_ms(1000);`	145	`delay_ms(1000);`
145		146
146	`SetServo(CASMIN); // max. doleva`	147	`SetServo(CASMIN); // max. doleva`
147	`set_pwm1_duty(0); // vzad`	148	`set_pwm1_duty(0); // vzad`
148	`set_pwm2_duty(20);`	149	`set_pwm2_duty(20);`
149	`output_low(MOT_L);`	150	`output_low(MOT_L);`
150	`output_high(MOT_R);`	151	`output_high(MOT_R);`
151	`stav=cihla;`	152	`stav=cihla;`
Line 167...	Line 168...
167	`output_low(MOT_L);`	168	`output_low(MOT_L);`
168	`output_low(MOT_R);`	169	`output_low(MOT_R);`
169	`delay_ms(500);`	170	`delay_ms(500);`
170		171
171		172
172	`SetServo((CASAVR-CASMIN)+5); // mirne doprava`	173	`SetServo((CASAVR-CASMIN)+5); // mirne doprava`
173	`set_pwm1_duty(180); // vpred`	174	`set_pwm1_duty(180); // vpred`
174	`set_pwm2_duty(180);`	175	`set_pwm2_duty(180);`
175	`output_low(MOT_L);`	176	`output_low(MOT_L);`
176	`output_low(MOT_R);`	177	`output_low(MOT_R);`
177	`stav=cihla;`	178	`stav=cihla;`
Line 189...	Line 190...
189	`output_low(MOT_R);`	190	`output_low(MOT_R);`
190	`delay_ms(500);`	191	`delay_ms(500);`
191		192
192		193
193	`cas=CASMIN; // Cara je vlevo`	194	`cas=CASMIN; // Cara je vlevo`
194		195
195	`stav=cihla;`	196	`stav=cihla;`
196	`}`	197	`}`
197	`}`	198	`}`
198		199
199	`// Zobrazeni jednoho byte na modulu LEDbar`	200	`// Zobrazeni jednoho byte na modulu LEDbar`
Line 262...	Line 263...
262	`set_pwm1_duty(0); // Zastav motory`	263	`set_pwm1_duty(0); // Zastav motory`
263	`set_pwm2_duty(0);`	264	`set_pwm2_duty(0);`
264	`output_low(MOT_L);`	265	`output_low(MOT_L);`
265	`output_low(MOT_R);`	266	`output_low(MOT_R);`
266		267
-		268	`output_low(IRLED); // Zapni LED na detekci prekazky`
-		269
267	`NightRider(1); // Aby se poznalo, ze byl RESET`	270	`NightRider(1); // Aby se poznalo, ze byl RESET`
268	`// Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`	271	`// Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`
269		272
270	`//... Nastaveni kamery ...`	273	`//... Nastaveni kamery ...`
271	`i2c_start(); // Soft RESET kamery`	274	`i2c_start(); // Soft RESET kamery`
Line 452...	Line 455...
452	`{`	455	`{`
453	`pom>>=1;`	456	`pom>>=1;`
454	`}`	457	`}`
455	`disp(pom);`	458	`disp(pom);`
456		459
457	`output_high(PIN_C0);`	-
458	`delay_ms(1);`	-
459	`output_low(PIN_C0);`	-
460		-
461	`};`	460	`};`
462	`}`	461	`}`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c – Rev 184 → 186