Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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