WebSVN – svnkaklik – Diff – /programy/C/PIC/PICcam/876/camerus.c


#include ".\camerus.h"
 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery
 
// kroutitka
#define  CERVENA  3  // AN3/RA3
#define  CERNA    2  // AN2/RA2
#define  ZELENA   1  // AN1/RA0
#define  MODRA    0  // AN0/RA1
 
// I/O
#define LED    PIN_C0      // LED signalizujici start programu
#define HREF   PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
#define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
#define SERVO  PIN_B7      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
#define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
#define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
 
void main()
{
   int8 cas;      // Cas hrany bila/cerna v radce
   int8 offset;   // Promena pro ulozeni ovsetu
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
   int8 r1;       // Rychlost motoru 1
   int8 r2;       // Rychlost motoru 2
 
   int16 ble;     // Prodleva do rozjezdu
 
   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_DISABLED);
   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(0b11111000);     // 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);
 
   output_high(LED);    // Budeme blikat LED, aby se poznalo, ze byl RESET 
 
   delay_ms(200);       // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
 
   //... Nastaveni kamery ...   
   i2c_start();      // Soft RESET kamery
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
   i2c_write(0x12);        // Adresa registru COMH
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
   i2c_stop();
 
   i2c_start();      // BW
   i2c_write(0xC0);
   i2c_write(0x28);
   i2c_write(0b01000001);
   i2c_stop();
 
 
   i2c_start();      // Contrast
   i2c_write(0xC0);
   i2c_write(0x05);
   i2c_write(0xA0);
   i2c_stop();
/*
   i2c_start();      // Brightness
   i2c_write(0xC0);
   i2c_write(0x06);
   i2c_write(0x80);
   i2c_stop();
 
   i2c_start();      // Band Filter
   i2c_write(0xC0);
   i2c_write(0x2D);
   i2c_write(0x04 | 0x03);
   i2c_stop();
*/
   i2c_start();      // VSTRT
   i2c_write(0xC0);
   i2c_write(0x19);
   i2c_write(45);
   i2c_stop();
 
   i2c_start();      // VEND
   i2c_write(0xC0);
   i2c_write(0x1A);
   i2c_write(45);
   i2c_stop();
 
   output_low(LED); // Blikani LED
   delay_ms(200);
 
   output_high(LED); // Blikani LED
   delay_ms(200);
   output_low(LED);
 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
   ble=0;
 
   // ... Hlavni smycka ...
   while(true)
   {
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
      set_timer0(0);          // Vynuluj pocitadlo casu
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
      {
         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA
         if(!input(PIX))
         {
            cas=get_timer0();    // Precti cas z citace casu hrany
            cas=255-cas;         // Vlevo je vpravo
            break;
         };
      };
 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
      Delay_ms(1);
      offset=read_adc();
      
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
      delay_us(1000);
      delay_us(offset);
      delay_us(offset);
      delay_us(cas);
      delay_us(cas);
      output_low(SERVO);
 
      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
      set_adc_channel(ZELENA);   // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
      Delay_ms(1);
      offset=read_adc();
      set_adc_channel(CERNA);
      Delay_ms(1);
      rr=read_adc();
      
      r1=cas>>1;                 // Elektronicky diferencial
      r2=255-offset-(cas>>1);
 
      if (ble==99)               // Rozjezd na Rozumnou rychlost
      {
         set_pwm1_duty(rr);
         set_pwm2_duty(rr);
      };
      if ((ble>100)&&(ble<60000))      // Jizda
      {
         if (r1<=rr) set_pwm1_duty(r1);
         if (r2<=rr) set_pwm2_duty(r2);
      }
      else
      {
         set_pwm1_duty(0);       // Zastaveni
         set_pwm2_duty(0);
      }
   };
}

Rev 410	Rev 659
1	`#include ".\camerus.h"`	1	`#include ".\camerus.h"`
2		2
3	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery`	3	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cteni z kamery`
4		4
5	`// kroutitka`	5	`// kroutitka`
6	`#define CERVENA 3 // AN3/RA3`	6	`#define CERVENA 3 // AN3/RA3`
7	`#define CERNA 2 // AN2/RA2`	7	`#define CERNA 2 // AN2/RA2`
8	`#define ZELENA 1 // AN1/RA0`	8	`#define ZELENA 1 // AN1/RA0`
9	`#define MODRA 0 // AN0/RA1`	9	`#define MODRA 0 // AN0/RA1`
10		10
11	`// I/O`	11	`// I/O`
12	`#define LED PIN_C0 // LED signalizujici start programu`	12	`#define LED PIN_C0 // LED signalizujici start programu`
13	`#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)`	13	`#define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku)`
14	`#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))`	14	`#define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))`
15	`#define SERVO PIN_B7 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))`	15	`#define SERVO PIN_B7 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))`
16	`#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2`	16	`#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2`
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		18
19	`void main()`	19	`void main()`
20	`{`	20	`{`
21	`int8 cas; // Cas hrany bila/cerna v radce`	21	`int8 cas; // Cas hrany bila/cerna v radce`
22	`int8 offset; // Promena pro ulozeni ovsetu`	22	`int8 offset; // Promena pro ulozeni ovsetu`
23	`int8 rr; // Promenna na ulozeni Rozumne rychlost`	23	`int8 rr; // Promenna na ulozeni Rozumne rychlost`
24	`int8 r1; // Rychlost motoru 1`	24	`int8 r1; // Rychlost motoru 1`
25	`int8 r2; // Rychlost motoru 2`	25	`int8 r2; // Rychlost motoru 2`
26		26
27	`int16 ble; // Prodleva do rozjezdu`	27	`int16 ble; // Prodleva do rozjezdu`
28		28
29	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`	29	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`
30	`setup_adc(ADC_CLOCK_INTERNAL);`	30	`setup_adc(ADC_CLOCK_INTERNAL);`
31	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`	31	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`
32	`setup_timer_1(T1_DISABLED);`	32	`setup_timer_1(T1_DISABLED);`
33	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`	33	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`
34	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`	34	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`
35	`setup_ccp2(CCP_PWM); // RC2`	35	`setup_ccp2(CCP_PWM); // RC2`
36	`setup_comparator(NC_NC_NC_NC);`	36	`setup_comparator(NC_NC_NC_NC);`
37	`setup_vref(FALSE);`	37	`setup_vref(FALSE);`
38		38
39	`set_tris_c(0b11111000); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky`	39	`set_tris_c(0b11111000); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky`
40		40
41	`set_pwm1_duty(0); // Zastav motory`	41	`set_pwm1_duty(0); // Zastav motory`
42	`set_pwm2_duty(0);`	42	`set_pwm2_duty(0);`
43	`output_low(MOT_L);`	43	`output_low(MOT_L);`
44	`output_low(MOT_R);`	44	`output_low(MOT_R);`
45		45
46	`output_high(LED); // Budeme blikat LED, aby se poznalo, ze byl RESET`	46	`output_high(LED); // Budeme blikat LED, aby se poznalo, ze byl RESET`
47		47
48	`delay_ms(200); // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`	48	`delay_ms(200); // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy`
49		49
50	`//... Nastaveni kamery ...`	50	`//... Nastaveni kamery ...`
51	`i2c_start(); // Soft RESET kamery`	51	`i2c_start(); // Soft RESET kamery`
52	`i2c_write(0xC0); // Pro single slave musi mit vsechny zapisy adresu C0h`	52	`i2c_write(0xC0); // Pro single slave musi mit vsechny zapisy adresu C0h`
53	`i2c_write(0x12); // Adresa registru COMH`	53	`i2c_write(0x12); // Adresa registru COMH`
54	`i2c_write(0x80 \| 0x24); // Zapis ridiciho slova`	54	`i2c_write(0x80 \| 0x24); // Zapis ridiciho slova`
55	`i2c_stop();`	55	`i2c_stop();`
56		56
57	`i2c_start(); // BW`	57	`i2c_start(); // BW`
58	`i2c_write(0xC0);`	58	`i2c_write(0xC0);`
59	`i2c_write(0x28);`	59	`i2c_write(0x28);`
60	`i2c_write(0b01000001);`	60	`i2c_write(0b01000001);`
61	`i2c_stop();`	61	`i2c_stop();`
62		62
63		63
64	`i2c_start(); // Contrast`	64	`i2c_start(); // Contrast`
65	`i2c_write(0xC0);`	65	`i2c_write(0xC0);`
66	`i2c_write(0x05);`	66	`i2c_write(0x05);`
67	`i2c_write(0xA0);`	67	`i2c_write(0xA0);`
68	`i2c_stop();`	68	`i2c_stop();`
69	`/*`	69	`/*`
70	`i2c_start(); // Brightness`	70	`i2c_start(); // Brightness`
71	`i2c_write(0xC0);`	71	`i2c_write(0xC0);`
72	`i2c_write(0x06);`	72	`i2c_write(0x06);`
73	`i2c_write(0x80);`	73	`i2c_write(0x80);`
74	`i2c_stop();`	74	`i2c_stop();`
75		75
76	`i2c_start(); // Band Filter`	76	`i2c_start(); // Band Filter`
77	`i2c_write(0xC0);`	77	`i2c_write(0xC0);`
78	`i2c_write(0x2D);`	78	`i2c_write(0x2D);`
79	`i2c_write(0x04 \| 0x03);`	79	`i2c_write(0x04 \| 0x03);`
80	`i2c_stop();`	80	`i2c_stop();`
81	`*/`	81	`*/`
82	`i2c_start(); // VSTRT`	82	`i2c_start(); // VSTRT`
83	`i2c_write(0xC0);`	83	`i2c_write(0xC0);`
84	`i2c_write(0x19);`	84	`i2c_write(0x19);`
85	`i2c_write(45);`	85	`i2c_write(45);`
86	`i2c_stop();`	86	`i2c_stop();`
87		87
88	`i2c_start(); // VEND`	88	`i2c_start(); // VEND`
89	`i2c_write(0xC0);`	89	`i2c_write(0xC0);`
90	`i2c_write(0x1A);`	90	`i2c_write(0x1A);`
91	`i2c_write(45);`	91	`i2c_write(45);`
92	`i2c_stop();`	92	`i2c_stop();`
93		93
94	`output_low(LED); // Blikani LED`	94	`output_low(LED); // Blikani LED`
95	`delay_ms(200);`	95	`delay_ms(200);`
96		96
97	`output_high(LED); // Blikani LED`	97	`output_high(LED); // Blikani LED`
98	`delay_ms(200);`	98	`delay_ms(200);`
99	`output_low(LED);`	99	`output_low(LED);`
100		100
101	`cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska`	101	`cas=128; // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska`
102	`ble=0;`	102	`ble=0;`
103		103
104	`// ... Hlavni smycka ...`	104	`// ... Hlavni smycka ...`
105	`while(true)`	105	`while(true)`
106	`{`	106	`{`
107	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`	107	`while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky`
108	`set_timer0(0); // Vynuluj pocitadlo casu`	108	`set_timer0(0); // Vynuluj pocitadlo casu`
109	`while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B`	109	`while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B`
110	`{`	110	`{`
111	`if(!input(PIX)) // Pokud se dvakrat za sebou precetla CERNA`	111	`if(!input(PIX)) // Pokud se dvakrat za sebou precetla CERNA`
112	`if(!input(PIX))`	112	`if(!input(PIX))`
113	`{`	113	`{`
114	`cas=get_timer0(); // Precti cas z citace casu hrany`	114	`cas=get_timer0(); // Precti cas z citace casu hrany`
115	`cas=255-cas; // Vlevo je vpravo`	115	`cas=255-cas; // Vlevo je vpravo`
116	`break;`	116	`break;`
117	`};`	117	`};`
118	`};`	118	`};`
119		119
120	`set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka`	120	`set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka`
121	`Delay_ms(1);`	121	`Delay_ms(1);`
122	`offset=read_adc();`	122	`offset=read_adc();`
123		123
124	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`	124	`output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo`
125	`delay_us(1000);`	125	`delay_us(1000);`
126	`delay_us(offset);`	126	`delay_us(offset);`
127	`delay_us(offset);`	127	`delay_us(offset);`
128	`delay_us(cas);`	128	`delay_us(cas);`
129	`delay_us(cas);`	129	`delay_us(cas);`
130	`output_low(SERVO);`	130	`output_low(SERVO);`
131		131
132	`ble++; // Casovac pro odlozeni rozjezdu po zapnuti napajeni`	132	`ble++; // Casovac pro odlozeni rozjezdu po zapnuti napajeni`
133	`set_adc_channel(ZELENA); // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)`	133	`set_adc_channel(ZELENA); // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)`
134	`Delay_ms(1);`	134	`Delay_ms(1);`
135	`offset=read_adc();`	135	`offset=read_adc();`
136	`set_adc_channel(CERNA);`	136	`set_adc_channel(CERNA);`
137	`Delay_ms(1);`	137	`Delay_ms(1);`
138	`rr=read_adc();`	138	`rr=read_adc();`
139		139
140	`r1=cas>>1; // Elektronicky diferencial`	140	`r1=cas>>1; // Elektronicky diferencial`
141	`r2=255-offset-(cas>>1);`	141	`r2=255-offset-(cas>>1);`
142		142
143	`if (ble==99) // Rozjezd na Rozumnou rychlost`	143	`if (ble==99) // Rozjezd na Rozumnou rychlost`
144	`{`	144	`{`
145	`set_pwm1_duty(rr);`	145	`set_pwm1_duty(rr);`
146	`set_pwm2_duty(rr);`	146	`set_pwm2_duty(rr);`
147	`};`	147	`};`
148	`if ((ble>100)&&(ble<60000)) // Jizda`	148	`if ((ble>100)&&(ble<60000)) // Jizda`
149	`{`	149	`{`
150	`if (r1<=rr) set_pwm1_duty(r1);`	150	`if (r1<=rr) set_pwm1_duty(r1);`
151	`if (r2<=rr) set_pwm2_duty(r2);`	151	`if (r2<=rr) set_pwm2_duty(r2);`
152	`}`	152	`}`
153	`else`	153	`else`
154	`{`	154	`{`
155	`set_pwm1_duty(0); // Zastaveni`	155	`set_pwm1_duty(0); // Zastaveni`
156	`set_pwm2_duty(0);`	156	`set_pwm2_duty(0);`
157	`}`	157	`}`
158	`};`	158	`};`
159	`}`	159	`}`

Subversion Repositories svnkaklik

(root)/programy/C/PIC/PICcam/876/camerus.c @ 660 – Rev 410 → 659