Subversion Repositories svnkaklik

Rev

Rev 153 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 153 Rev 154
Line 1... Line 1...
1 
#include ".\camerus.h"
 1 
#include ".\camerus.h"
2 
 
 2 
 
3 
#USE FAST_IO (C)
 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
 12 
#define LED    PIN_C0      // LED signalizujici start programu
13 
#define HREF   PIN_C5
 13 
#define HREF   PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
14 
#define PIX    PIN_C6
 14 
#define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
15 
#define SERVO  PIN_B7
 15 
#define SERVO  PIN_B7      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
16 
#define MOT_L  PIN_B5
 16 
#define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
17 
#define MOT_R  PIN_B6
 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;
 21 
   int8 cas;      // Cas hrany bila/cerna v radce
22 
   int8 offset;
 22 
   int8 offset;   // Promena pro ulozeni ovsetu
23 
   int8 rr; // Rozumna rychlost
 23 
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
24 
   int8 r1;
 24 
   int8 r1;       // Rychlost motoru 1
25 
   int8 r2;
 25 
   int8 r2;       // Rychlost motoru 2
26 
 
 26 
 
27 
   int16 ble;
 27 
   int16 ble;     // Prodleva do rozjezdu
28
28 
 
29 
   setup_adc_ports(ALL_ANALOG);
 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);
 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);
 33 
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
34 
   setup_ccp1(CCP_PWM); // RC1
 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);
 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);
 46 
   output_high(LED);    // Budeme blikat LED, aby se poznalo, ze byl RESET
47 
 
 47 
 
48 
   delay_ms(200);
 48 
   delay_ms(200);       // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
49 
 
 49 
 
- 
 
 50 
   //... Nastaveni kamery ...
50 
   i2c_start();      // Reset
 51 
   i2c_start();      // Soft RESET kamery
51 
   i2c_write(0xC0);
 52 
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
52 
   i2c_write(0x12);
 53 
   i2c_write(0x12);        // Adresa registru COMH
53 
   i2c_write(0x80 | 0x24);
 54 
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
54 
   i2c_stop();
 55 
   i2c_stop();
55 
 
 56 
 
56 
   i2c_start();      // BW
 57 
   i2c_start();      // BW
57 
   i2c_write(0xC0);
 58 
   i2c_write(0xC0);
58 
   i2c_write(0x28);
 59 
   i2c_write(0x28);
Line 64... Line 65...
64 
   i2c_write(0xC0);
 65 
   i2c_write(0xC0);
65 
   i2c_write(0x05);
 66 
   i2c_write(0x05);
66 
   i2c_write(0xA0);
 67 
   i2c_write(0xA0);
67 
   i2c_stop();
 68 
   i2c_stop();
68 
/*
 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();
Line 76... Line 76...
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 
 
 - 
 
82 
   i2c_start();      // 4-bit, (CLK/4)
 - 
 
83 
   i2c_write(0xC0);
 - 
 
84 
   i2c_write(0x3E);
 - 
 
85 
   i2c_write(0x10 | 0x80);
 - 
 
86 
   i2c_stop();
 - 
 
87 
*/
 81 
*/
88 
   i2c_start();      // VSTRT
 82 
   i2c_start();      // VSTRT
89 
   i2c_write(0xC0);
 83 
   i2c_write(0xC0);
90 
   i2c_write(0x19);
 84 
   i2c_write(0x19);
91 
   i2c_write(45);
 85 
   i2c_write(45);
Line 95... Line 89...
95 
   i2c_write(0xC0);
 89 
   i2c_write(0xC0);
96 
   i2c_write(0x1A);
 90 
   i2c_write(0x1A);
97 
   i2c_write(45);
 91 
   i2c_write(45);
98 
   i2c_stop();
 92 
   i2c_stop();
99 
 
 93 
 
100 
   output_low(LED);
 94 
   output_low(LED); // Blikani LED
101 
   delay_ms(200);
 95 
   delay_ms(200);
102 
 
 96 
 
103 
/*
 - 
 
104 
   i2c_start();      // BW + freeze AGC/AEC
 - 
 
105 
   i2c_write(0xC0);
 - 
 
106 
   i2c_write(0x28);
 - 
 
107 
   i2c_write(0b01010001);
 - 
 
108 
   i2c_stop();
 - 
 
109 
*/
 - 
 
110 
   output_high(LED);
 97 
   output_high(LED); // Blikani LED
111 
   delay_ms(200);
 98 
   delay_ms(200);
112 
   output_low(LED);
 99 
   output_low(LED);
113 
 
 100 
 
114 
   cas=128;
 101 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
115 
 
 - 
 
116 
   ble=0;
 102 
   ble=0;
117
103 
 
- 
 
 104 
   // ... Hlavni smycka ...
118 
   while(true)
 105 
   while(true)
119 
   {
 106 
   {
120 
      while(!input(HREF));
 107 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
121 
//      delay_cycles(4);
 - 
 
122 
      set_timer0(0);
 108 
      set_timer0(0);          // Vynuluj pocitadlo casu
123 
      while(input(HREF))
 109 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
124 
      {
 110 
      {
125 
         if(!input(PIX))
 111 
         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA
126 
         if(!input(PIX))
 112 
         if(!input(PIX))
127 
         {
 113 
         {
128 
            cas=get_timer0();
 114 
            cas=get_timer0();    // Precti cas z citace casu hrany
129 
            cas=255-cas;
 115 
            cas=255-cas;         // Vlevo je vpravo
130 
            break;
 116 
            break;
131 
         };
 117 
         };
132 
      };
 118 
      };
133 
 
 119 
 
134 
      set_adc_channel(MODRA);
 120 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
135 
      Delay_ms(1);
 121 
      Delay_ms(1);
136 
      offset=read_adc();
 122 
      offset=read_adc();
- 
 
 123
137 
      output_high(SERVO);
 124 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
138 
      delay_us(1000);
 125 
      delay_us(1000);
139 
      delay_us(offset);
 126 
      delay_us(offset);
140 
      delay_us(offset);
 127 
      delay_us(offset);
141 
      delay_us(cas);
 128 
      delay_us(cas);
142 
      delay_us(cas);
 129 
      delay_us(cas);
143 
//      delay_us(cas);
 - 
 
144 
      output_low(SERVO);
 130 
      output_low(SERVO);
145
131 
 
146 
      ble++;
 132 
      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
147 
      set_adc_channel(ZELENA);
 133 
      set_adc_channel(ZELENA);   // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
148 
      Delay_ms(1);
 134 
      Delay_ms(1);
149 
      offset=read_adc();
 135 
      offset=read_adc();
150 
      set_adc_channel(CERNA);
 136 
      set_adc_channel(CERNA);
151 
      Delay_ms(1);
 137 
      Delay_ms(1);
152 
      rr=read_adc();
 138 
      rr=read_adc();
153 
      r1=cas>>1;
 139
- 
 
 140 
      r1=cas>>1;                 // Elektronicky diferencial
154 
      r2=255-offset-(cas>>1);
 141 
      r2=255-offset-(cas>>1);
- 
 
 142 
 
155 
      if (ble==99)
 143 
      if (ble==99)               // Rozjezd na Rozumnou rychlost
156 
      {
 144 
      {
157 
         set_pwm1_duty(rr);
 145 
         set_pwm1_duty(rr);
158 
         set_pwm2_duty(rr);
146 
         set_pwm2_duty(rr);
159 
      };
 147 
      };
160 
      if ((ble>100)&&(ble<60000))
 148 
      if ((ble>100)&&(ble<60000))      // Jizda
161 
      {
149 
      {
162 
         if (r1<=rr) set_pwm1_duty(r1);
 150 
         if (r1<=rr) set_pwm1_duty(r1);
163 
         if (r2<=rr) set_pwm2_duty(r2);
 151 
         if (r2<=rr) set_pwm2_duty(r2);
164 
      }
 152 
      }
165 
      else
 153 
      else
166 
      {
 154 
      {
167 
         set_pwm1_duty(0);
 155 
         set_pwm1_duty(0);       // Zastaveni
168 
         set_pwm2_duty(0);
 156 
         set_pwm2_duty(0);
169 
      }
 157 
      }
170 
   };
 158 
   };
171 
}
 159 
}