Subversion Repositories svnkaklik

Rev

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

Rev 176 Rev 179
Line 14... Line 14...
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_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
 15 
#define SERVO  PIN_B4      // 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 
#define DATA   PIN_B2
 19 
#define DATA   PIN_B2      // K modulu LEDbar data
20 
#define CP     PIN_B1
 20 
#define CP     PIN_B1      // K modulu LEDbar hodiny
21 
 
 21 
 
- 
 
 22 
#define CASMIN 6           // Rozsah radku snimace
- 
 
 23 
#define CASMAX 192
- 
 
 24 
#define CASAVR ((CASMAX+CASMIN) / 2)
- 
 
 25 
 
- 
 
 26 
// Zobrazeni jednoho byte na modulu LEDbar
22 
void disp(int8 x)
 27 
void disp(int8 x)
23 
{
 28 
{
24 
   int n;
 29 
   int n;
25 
 
 30 
 
26 
   x=~x;
 31 
   x=~x;
Line 31... Line 36...
31 
      x>>=1;
 36 
      x>>=1;
32 
      output_low(CP);
 37 
      output_low(CP);
33 
   }
 38 
   }
34 
}
 39 
}
35 
 
 40 
 
- 
 
 41 
// Blikani LEDbarem ve stilu Night Rider
36 
void NightRider(int x)
 42 
void NightRider(int x)
37 
{
 43 
{
38 
   int n,i,j;
 44 
   int n,i,j;
39 
 
 45 
 
40 
   for(j=0;j<x;j++)
 46 
   for(j=0;j<x;j++)
Line 110... Line 116...
110 
   i2c_stop();
 116 
   i2c_stop();
111 
*/
 117 
*/
112 
   i2c_start();      // Brightness
 118 
   i2c_start();      // Brightness
113 
   i2c_write(0xC0);
 119 
   i2c_write(0xC0);
114 
   i2c_write(0x06);
 120 
   i2c_write(0x06);
115 
   i2c_write(0x90);  // 80h
 121 
   i2c_write(0x9A);  // 80h
116 
   i2c_stop();
 122 
   i2c_stop();
117 
 
 123 
 
118 
/*
 124 
/*
119 
   i2c_start();      // Band Filter
 125 
   i2c_start();      // Band Filter
120 
   i2c_write(0xC0);
 126 
   i2c_write(0xC0);
Line 132... Line 138...
132 
   i2c_write(0xC0);
 138 
   i2c_write(0xC0);
133 
   i2c_write(0x1A);
 139 
   i2c_write(0x1A);
134 
   i2c_write(45);
 140 
   i2c_write(45);
135 
   i2c_stop();
 141 
   i2c_stop();
136 
 
 142 
 
137 
   NightRider(1);    // Aby se poznalo, ze byl RESET
 143 
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
138 
 
 144 
 
139 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
 145 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
140 
   ble=0;
 146 
   ble=0;
141 
 
 147 
 
142 
/*
 - 
 
143 
while(true)
 - 
 
144 
{
 - 
 
145 
disp(ble);
 - 
 
146 
   i2c_start();
- 
 
147 
   i2c_write(0xC0);
 - 
 
148 
   i2c_write(0x06);
 - 
 
149 
   i2c_write(ble);
- 
 
150 
   i2c_stop();
 - 
 
151 
delay_ms(1000);
 - 
 
152 
ble+=0x10;
 - 
 
153 
};
 - 
 
154 
*/
 - 
 
155 
 
 - 
 
156 
   // ... Hlavni smycka ...
 148 
   // ... Hlavni smycka ...
157 
   while(true)
 149 
   while(true)
158 
   {
 150 
   {
- 
 
 151 
      int8 pom;
- 
 
 152 
 
- 
 
 153 
      pom=0;
159 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
 154 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
160 
      set_timer0(0);          // Vynuluj pocitadlo casu
 155 
      set_timer0(0);          // Vynuluj pocitadlo casu
161 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
 156 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
162 
      {
 157 
      {
163 
// Dodelat rozpoznani cerne cary napric pro zastaveni
 158 
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni
164 
// Pockat na shozeni signalu HREF
 - 
 
165 
         if(!input(PIX))   // Pokud se dvakrat za sebou precetla CERNA
 159 
         if(!input(PIX))   // Pokud se 3x za sebou precetla CERNA
166 
         if(!input(PIX))
 160 
         if(!input(PIX))
167 
         if(!input(PIX))
 161 
         if(!input(PIX))
168 
         {
 162 
         {
169 
            int8 pom;
 - 
 
170 
 
 - 
 
171 
            pom=get_timer0();    // Precti cas z citace casu hrany
 163 
            pom=get_timer0();    // Precti cas z citace casu hrany
172 
//            cas=255-cas;         // Vlevo je vpravo
 - 
 
173 
            if(pom<0b11000000) cas=pom;
 - 
 
174 
            break;
 164 
            break;
175 
         };
 165 
         };
176 
      };
 166 
      };
- 
 
 167 
      while(input(HREF));      // Pockej na shozeni signalu HREF
- 
 
 168 
 
- 
 
 169 
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
- 
 
 170 
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
- 
 
 171 
      // a vubec to nejak na kraji nefunguje.
177 
 
 172 
 
178 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
 173 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
179 
      Delay_ms(1);
 174 
      Delay_ms(1);
180 
      offset=read_adc();
 175 
      offset=read_adc();
181 
 
 176 
 
Line 186... Line 181...
186 
      delay_us(cas);
 181 
      delay_us(cas);
187 
      delay_us(cas);
 182 
      delay_us(cas);
188 
      output_low(SERVO);
 183 
      output_low(SERVO);
189 
 
 184 
 
190 
      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
 185 
      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
191 
      set_adc_channel(ZELENA);   // Kroutitka pro vykon motoru (!!! to bude chtit jeste predelat !!!)
 186 
      set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
192 
      Delay_ms(1);
 187 
      Delay_ms(1);
193 
      offset=read_adc();
 - 
 
194 
      set_adc_channel(CERNA);
 - 
 
195 
      Delay_ms(1);
 - 
 
196 
      rr=read_adc();
 188 
      rr=read_adc()>>2; //!!! // 0-31
197 
 
 189 
 
198 
disp(cas);
 190 
disp(cas & 0b11110000);
- 
 
 191
- 
 
 192 
      // Elektronicky diferencial
- 
 
 193 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
- 
 
 194 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
- 
 
 195 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
- 
 
 196
- 
 
 197
- 
 
 198 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
- 
 
 199 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
199 
 
 200 
 
200 
      r1=cas>>1;                 // Elektronicky diferencial
 201 
//      r1<<=1;     // Rychlost je dvojnasobna
201 
      r2=255-offset-(cas>>1);
 202 
//      r2<<=1;     // Rozsah 2 az 184
202 
 
 203 
 
203 
      if (ble==99)               // Rozjezd na Rozumnou rychlost
 - 
 
204 
      {
 - 
 
205 
         set_pwm1_duty(rr);
 - 
 
206 
         set_pwm2_duty(rr);
 - 
 
207 
      };
 - 
 
208 
      if ((ble>100)&&(ble<60000))      // Jizda
 204 
      if ((ble>100)&&(ble<60000))      // Jizda
209 
      {
 205 
      {
210 
         if (r1<=rr) set_pwm1_duty(r1);
 206 
         set_pwm1_duty(r1);
211 
         if (r2<=rr) set_pwm2_duty(r2);
 207 
         set_pwm2_duty(r2);
212 
      }
 208 
      }
213 
      else
 209 
      else
214 
      {
 210 
      {
- 
 
 211 
//!!!!!!!!!!!!!!!!!! Tenhle kod pred zavodem zrusit !!!!!!!!!!!!!!!!!!!!!!!!!!!!
215 
         set_pwm1_duty(0);       // Zastaveni
 212 
         set_pwm1_duty(0);       // Zastaveni
216 
         set_pwm2_duty(0);
 213 
         set_pwm2_duty(0);
217 
      }
 214 
      }
218 
   };
 215 
   };
219 
}
 216 
}