Subversion Repositories svnkaklik

Rev

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

Rev 179 Rev 180
Line 1... Line 1...
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 
// A/D vstupy
- 
 
 6 
#define  DALKOMER 4  // AN4/RA5 - dalkomer na cihlu
6 
#define  CERVENA  3  // AN3/RA3
 7 
#define  CERNA    3  // AN3/RA3
7 
#define  CERNA    2  // AN2/RA2
 8 
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
8 
#define  ZELENA   1  // AN1/RA0
 9 
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
9 
#define  MODRA    0  // AN0/RA1
 10 
#define  MODRA    0  // AN0/RA1 - modre kroutitko
10 
 
 11 
 
11 
// I/O
 12 
// 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)
 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_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 
 
 - 
 
19 
#define DATA   PIN_B2      // K modulu LEDbar data
 18 
#define DATA   PIN_B2      // K modulu LEDbar data
20 
#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
21 
 
 21 
 
22 
#define CASMIN 6           // Rozsah radku snimace
 22 
#define CASMIN 6           // Rozsah radku snimace
23 
#define CASMAX 192
 23 
#define CASMAX 192
24 
#define CASAVR ((CASMAX+CASMIN) / 2)
 24 
#define CASAVR ((CASMAX+CASMIN) / 2)
25 
 
 25 
 
Line 70... Line 70...
70 
   int8 cas;      // Cas hrany bila/cerna v radce
 70 
   int8 cas;      // Cas hrany bila/cerna v radce
71 
   int8 offset;   // Promena pro ulozeni ovsetu
 71 
   int8 offset;   // Promena pro ulozeni ovsetu
72 
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
 72 
   int8 rr;       // Promenna na ulozeni Rozumne rychlost
73 
   int8 r1;       // Rychlost motoru 1
 73 
   int8 r1;       // Rychlost motoru 1
74 
   int8 r2;       // Rychlost motoru 2
 74 
   int8 r2;       // Rychlost motoru 2
75 
 
 - 
 
- 
 
 75 
   enum stavy {start,rozjezd,jizda,cihla,cil};
- 
 
 76 
   stavy stav;    // Kde jsme na trati
76 
   int16 ble;     // Prodleva do rozjezdu
 77 
   int16 trasa;   // Pocitadlo ujete vzdalenosti
77 
 
 78 
 
78 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
 79 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
79 
   setup_adc(ADC_CLOCK_INTERNAL);
 80 
   setup_adc(ADC_CLOCK_INTERNAL);
80 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
 81 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
81 
   setup_timer_1(T1_DISABLED);
 82 
   setup_timer_1(T1_DISABLED);
Line 141... Line 142...
141 
   i2c_stop();
 142 
   i2c_stop();
142 
 
 143 
 
143 
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
 144 
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
144 
 
 145 
 
145 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
 146 
   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
- 
 
 147 
   stav=start;
146 
   ble=0;
 148 
   trasa=0;
147 
 
 149
148 
   // ... Hlavni smycka ...
 150 
   // ... Hlavni smycka ...
149 
   while(true)
 151 
   while(true)
150 
   {
 152 
   {
151 
      int8 pom;
 153 
      int8 pom;
- 
 
 154 
      int8 n;
152 
 
 155
153 
      pom=0;
 156 
      pom=0;
154 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
 157 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
155 
      set_timer0(0);          // Vynuluj pocitadlo casu
 158 
      set_timer0(0);          // Vynuluj pocitadlo casu
156 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
 159 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
157 
      {
 160 
      {
Line 180... Line 183...
180 
      delay_us(offset);
 183 
      delay_us(offset);
181 
      delay_us(cas);
 184 
      delay_us(cas);
182 
      delay_us(cas);
 185 
      delay_us(cas);
183 
      output_low(SERVO);
 186 
      output_low(SERVO);
184 
 
 187 
 
185 
      ble++;                     // Casovac pro odlozeni rozjezdu po zapnuti napajeni
 - 
 
186 
      set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
 188 
      set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
187 
      Delay_ms(1);
 189 
      Delay_ms(1);
188 
      rr=read_adc()>>2; //!!! // 0-31
 190 
      rr=read_adc()>>2; //!!! // 0-31
189 
 
 191 
 
190 
disp(cas & 0b11110000);
 192 
disp(cas & 0b11110000);
191
193 
 
192 
      // Elektronicky diferencial
 194 
      // Elektronicky diferencial
193 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
195 
      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
 196 
      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
 197 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
196
198 
 
197
199 
 
198 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
 200 
      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);
201 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
200 
 
 202 
 
201 
//      r1<<=1;     // Rychlost je dvojnasobna
 203 
//      r1<<=1;     // Rychlost je dvojnasobna
202 
//      r2<<=1;     // Rozsah 2 az 184
 204 
//      r2<<=1;     // Rozsah 2 az 184
203 
 
 205 
 
204 
      if ((ble>100)&&(ble<60000))      // Jizda
 206 
      if (stav==jizda)      // Jizda
205 
      {
 207 
      {
206 
         set_pwm1_duty(r1);
 208 
         set_pwm1_duty(r1);
207 
         set_pwm2_duty(r2);
 209 
         set_pwm2_duty(r2);
- 
 
 210 
         trasa++;
208 
      }
 211 
      }
209 
      else
 212 
      else
210 
      {
 213 
      {
211 
//!!!!!!!!!!!!!!!!!! Tenhle kod pred zavodem zrusit !!!!!!!!!!!!!!!!!!!!!!!!!!!!
- 
 
212 
         set_pwm1_duty(0);       // Zastaveni
 214 
         set_pwm1_duty(0);       // Zastaveni
213 
         set_pwm2_duty(0);
 215 
         set_pwm2_duty(0);
214 
      }
 216 
      }
- 
 
 217 
 
- 
 
 218 
      set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
- 
 
 219 
      Delay_ms(1);
- 
 
 220 
      for(n=0;n<5;n++)      // Detekce prekazky
- 
 
 221 
      {
- 
 
 222 
         if(read_adc()<128)
- 
 
 223 
         {
- 
 
 224 
            if(stav==start)
- 
 
 225 
            {
- 
 
 226 
              disp(0x55);
- 
 
 227 
              while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
- 
 
 228 
              set_pwm1_duty(255);
- 
 
 229 
              set_pwm2_duty(255);
- 
 
 230 
              disp(0xAA);
- 
 
 231 
              delay_ms(500);
- 
 
 232 
              stav=jizda;
- 
 
 233 
              break;
- 
 
 234 
            };
- 
 
 235 
            if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
- 
 
 236 
            {
- 
 
 237 
              set_pwm1_duty(0);
- 
 
 238 
              set_pwm2_duty(0);
- 
 
 239 
              output_high(MOT_L); // zabrzdi
- 
 
 240 
              output_high(MOT_R);
- 
 
 241 
              delay_ms(300);
- 
 
 242 
              output_low(MOT_L); // zastav
- 
 
 243 
              output_low(MOT_R);
- 
 
 244 
              stav=cil;
- 
 
 245 
              while(true);
- 
 
 246 
            };
- 
 
 247 
         }
- 
 
 248 
      }
- 
 
 249 
 
215 
   };
 250 
   };
216 
}
 251 
}