Subversion Repositories svnkaklik

Rev

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

Rev 189 Rev 190
Line 1... Line 1...
1
//******** Mrakomer ******************************************
1
//******** Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 189 2007-03-16 16:01:25Z kakl $"
2
//"$Id: camerus.c 190 2007-03-16 21:34:07Z kakl $"
3
//************************************************************
3
//*****************************************************
4
 
4
 
5
#include ".\camerus.h"
5
#include ".\camerus.h"
6
 
6
 
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
8
 
8
 
Line 17... Line 17...
17
#define HREF   PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
17
#define HREF   PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
18
#define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
18
#define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
19
#define SERVO  PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
19
#define SERVO  PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
20
#define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
20
#define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
21
#define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
21
#define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
-
 
22
#define MOT_1  PIN_C1      // PWM vystpy motoru
-
 
23
#define MOT_2  PIN_C2      // 
22
#define DATA   PIN_B2      // K modulu LEDbar data
24
#define DATA   PIN_B2      // K modulu LEDbar data
23
#define CP     PIN_B1      // K modulu LEDbar hodiny
25
#define CP     PIN_B1      // K modulu LEDbar hodiny
24
#define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
26
#define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
25
#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
27
#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
26
#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
28
#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
Line 41... Line 43...
41
 
43
 
42
enum stavy {start,rozjezd,jizda,cihla,cil};
44
enum stavy {start,rozjezd,jizda,cihla,cil};
43
stavy stav;    // Kde jsme na trati
45
stavy stav;    // Kde jsme na trati
44
int8 cas;      // Cas hrany bila/cerna v radce
46
int8 cas;      // Cas hrany bila/cerna v radce
45
 
47
 
-
 
48
 
-
 
49
inline void brzda()
-
 
50
{
-
 
51
   int8 n,i;
-
 
52
 
-
 
53
   set_pwm1_duty(0);    // vypni PWM
-
 
54
   set_pwm2_duty(0);
-
 
55
   setup_ccp1(CCP_OFF); 
-
 
56
   setup_ccp2(CCP_OFF); 
-
 
57
   for (n=0;n<255;n++)
-
 
58
   {
-
 
59
      output_low(MOT_L); 
-
 
60
      output_low(MOT_R);   
-
 
61
      output_high(MOT_1); 
-
 
62
      output_high(MOT_2);
-
 
63
      delay_us(800); 
-
 
64
      output_high(MOT_L); 
-
 
65
      output_high(MOT_R);   
-
 
66
      output_low(MOT_1); 
-
 
67
      output_low(MOT_2);   
-
 
68
      delay_us(800); 
-
 
69
   }
-
 
70
   output_low(MOT_L); // smer vpred
-
 
71
   output_low(MOT_R);   
-
 
72
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
-
 
73
   setup_ccp2(CCP_PWM); // RC2
-
 
74
}
-
 
75
 
46
void SetServo(int8 angle)
76
void SetServo(int8 angle)
47
{
77
{
48
   int8 n, offset;
78
   int8 n, offset;
49
 
79
 
50
   for(n=0; n<20; n++)
80
   for(n=0; n<14; n++)
51
   {
81
   {
52
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
82
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
53
      Delay_ms(1);
83
      Delay_ms(1);
54
      offset=read_adc();
84
      offset=read_adc();
55
 
85
 
Line 66... Line 96...
66
}
96
}
67
 
97
 
68
#int_EXT
98
#int_EXT
69
EXT_isr()   // Preruseni od prekazky
99
EXT_isr()   // Preruseni od prekazky
70
{
100
{
-
 
101
   set_pwm1_duty(0);    // reverz (zabrzdi)
-
 
102
   set_pwm2_duty(0);
-
 
103
   output_high(MOT_L);  // vzad
-
 
104
   output_high(MOT_R);
71
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
105
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
72
   output_high(IRTX);   // Vypni LED na detekci prekazky
106
   output_high(IRTX);   // Vypni LED na detekci prekazky
73
   set_pwm1_duty(0);    // Vypni motory
-
 
74
   set_pwm2_duty(0);
-
 
75
   output_low(MOT_L);
-
 
76
   output_low(MOT_R);
-
 
77
   delay_ms(2);
107
   delay_ms(2);
78
   if (!input(IRRX)) return;
108
   if (!input(IRRX))    // stale nas signal?
-
 
109
   {
-
 
110
      output_low(MOT_L); // je odraz -> vpred
-
 
111
      output_low(MOT_R);
-
 
112
      return;
-
 
113
   };
79
   output_low(IRTX);    // Zapni LED na detekci prekazky
114
   output_low(IRTX);    // Zapni LED na detekci prekazky
80
   delay_ms(10);
115
   delay_ms(10);
81
   if (input(IRRX)) return;
116
   if (input(IRRX))     // stale nas signal?
82
 
117
   {
83
   set_pwm1_duty(0);    // reverz (zabrzdi)
-
 
84
   set_pwm2_duty(0);
-
 
85
   output_high(MOT_L);
-
 
86
   output_high(MOT_R);
-
 
87
   delay_ms(400);
-
 
88
   output_low(MOT_L); // zastav
118
      output_low(MOT_L); // neni odraz -> vpred
89
   output_low(MOT_R);
119
      output_low(MOT_R);
-
 
120
      return;
-
 
121
   };
-
 
122
 
-
 
123
   SetServo((CASAVR-CASMIN));   // rovne
-
 
124
   delay_ms(100);    // Doba reverzace
-
 
125
 
-
 
126
   brzda();
-
 
127
 
90
   if (stav==cihla) while(true); // Zastav na furt
128
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
-
 
129
 
91
   if(stav==jizda)
130
   if(stav==jizda)   // Objed cihlu
92
   {
131
   {
93
      int n;
132
      int n;
94
/*
133
 
95
      SetServo(CASAVR-CASMIN);
134
      SetServo(CASMIN);   // prudce doleva
-
 
135
      delay_ms(200);
-
 
136
 
96
      set_pwm1_duty(40); // pomalu couvej
137
      set_pwm1_duty(150);  // vpred
97
      set_pwm2_duty(40);
138
      set_pwm2_duty(200);
98
      output_high(MOT_L);
139
      output_low(MOT_L);
99
      output_high(MOT_R);
140
      output_low(MOT_R);
100
      n=0;
141
      n=0;
101
      while(true)
142
      while(true)
102
      {
143
      {
103
         while(input(ODO));
144
         while(input(ODO));
104
         while(!input(ODO));
145
         while(!input(ODO));
105
         n++;
146
         n++;
106
         if(n==6) break;
147
         if(n==5) break;
107
      }
148
      }
108
      set_pwm1_duty(255);    // reverz (zabrzdi)
149
      set_pwm1_duty(0);    // reverz (zabrzdi)
109
      set_pwm2_duty(255);
-
 
110
      output_low(MOT_L);
-
 
111
      output_low(MOT_R);
-
 
112
      delay_ms(100);
-
 
113
      set_pwm1_duty(0);  // Zastav
-
 
114
      set_pwm2_duty(0);
150
      set_pwm2_duty(0);
115
      output_low(MOT_L);
151
      output_high(MOT_L);
116
      output_low(MOT_R);
152
      output_high(MOT_R);
117
*/
-
 
118
      delay_ms(1000);
-
 
119
 
153
 
120
      SetServo((CASAVR-CASMIN)-20);   // doleva
154
      SetServo((CASAVR-CASMIN));   // rovne
121
      set_pwm1_duty(150);  // vpred
155
      set_pwm1_duty(140);  // vpred
122
      set_pwm2_duty(200);
156
      set_pwm2_duty(140);
123
      output_low(MOT_L);
157
      output_low(MOT_L);
124
      output_low(MOT_R);
158
      output_low(MOT_R);
125
      stav=cihla;
-
 
126
      n=0;
159
      n=0;
127
      while(true)
160
      while(true)
128
      {
161
      {
129
         while(input(ODO));
162
         while(input(ODO));
130
         while(!input(ODO));
163
         while(!input(ODO));
131
         n++;
164
         n++;
132
         if(n==10) break;
165
         if(n==6) break;
133
      }
166
      }
134
      set_pwm1_duty(0);    // reverz (zabrzdi)
-
 
135
      set_pwm2_duty(0);
-
 
136
      output_high(MOT_L);
-
 
137
      output_high(MOT_R);
-
 
138
 
167
 
-
 
168
      brzda();    
-
 
169
 
139
      SetServo((CASAVR-CASMIN));   // rovne
170
      SetServo((CASAVR-CASMIN)+70);   // doprava
140
      set_pwm1_duty(140);  // vpred
171
      set_pwm1_duty(140);  // vpred
141
      set_pwm2_duty(140);
172
      set_pwm2_duty(140);
142
      output_low(MOT_L);
173
      output_low(MOT_L);
143
      output_low(MOT_R);
174
      output_low(MOT_R);
144
      stav=cihla;
-
 
145
      n=0;
175
      n=0;
146
      while(true)
176
      while(true)
147
      {
177
      {
148
         while(input(ODO));
178
         while(input(ODO));
149
         while(!input(ODO));
179
         while(!input(ODO));
150
         n++;
180
         n++;
151
         if(n==10) break;
181
         if(n==6) break;
152
      }
182
      }
-
 
183
 
153
      set_pwm1_duty(0);    // reverz (zabrzdi)
184
      set_pwm1_duty(0);    // reverz (zabrzdi)
154
      set_pwm2_duty(0);
185
      set_pwm2_duty(0);
155
      output_high(MOT_L);
186
      output_high(MOT_L);
156
      output_high(MOT_R);
187
      output_high(MOT_R);
157
      delay_ms(200);
188
      delay_ms(100);
158
      set_pwm1_duty(150);  // Zastav
-
 
159
      set_pwm2_duty(150);
-
 
160
      output_high(MOT_L);
-
 
161
      output_high(MOT_R);
-
 
-
 
189
 
162
      delay_ms(1000);
190
      brzda();    
163
 
191
 
164
      SetServo(CASMIN);   // max. doleva
192
      SetServo(CASMIN);   // max. doleva
165
      set_pwm1_duty(0);  // vzad
193
      set_pwm1_duty(0);   // vzad
166
      set_pwm2_duty(20);
194
      set_pwm2_duty(20);
167
      output_low(MOT_L);
195
      output_low(MOT_L);
168
      output_high(MOT_R);
196
      output_high(MOT_R);
169
      stav=cihla;
-
 
170
      n=0;
197
      n=0;
171
      while(true)
198
      while(true)
172
      {
199
      {
173
         while(input(ODO));
200
         while(input(ODO));
174
         while(!input(ODO));
201
         while(!input(ODO));
175
         n++;
202
         n++;
176
         if(n==10) break;
203
         if(n==5) break;
177
      }
204
      }
178
      set_pwm1_duty(0);    // reverz (zabrzdi)
205
      set_pwm1_duty(0);    // reverz (zabrzdi)
179
      set_pwm2_duty(255);
206
      set_pwm2_duty(255);
180
      output_low(MOT_L);
207
      output_low(MOT_L);
181
      output_low(MOT_R);
208
      output_low(MOT_R);
182
      delay_ms(100);
209
      delay_ms(100);
183
      set_pwm1_duty(0);  // Zastav
-
 
184
      set_pwm2_duty(0);
-
 
185
      output_low(MOT_L);
-
 
186
      output_low(MOT_R);
-
 
187
      delay_ms(500);
-
 
188
 
210
 
-
 
211
      brzda();
189
 
212
 
190
      SetServo((CASAVR-CASMIN)+5);   // mirne doprava
213
      SetServo((CASAVR-CASMIN)+20);   // mirne doprava
191
      set_pwm1_duty(180);  // vpred
214
      set_pwm1_duty(190);  // vpred
192
      set_pwm2_duty(180);
215
      set_pwm2_duty(190);
193
      output_low(MOT_L);
216
      output_low(MOT_L);
194
      output_low(MOT_R);
217
      output_low(MOT_R);
195
      stav=cihla;
-
 
196
      n=0;
218
      n=0;
197
      while(true)
219
      while(true)
198
      {
220
      {
199
         while(input(ODO));
221
         while(input(ODO));
200
         while(!input(ODO));
222
         while(!input(ODO));
201
         n++;
223
         n++;
202
         if(n==12) break;
224
         if(n==14) break;
203
      }
225
      }
-
 
226
/*
204
      set_pwm1_duty(0);  // Zastav
227
      set_pwm1_duty(0);    // reverz (zabrzdi)
205
      set_pwm2_duty(0);
228
      set_pwm2_duty(0);
206
      output_low(MOT_L);
229
      output_high(MOT_L);
207
      output_low(MOT_R);
230
      output_high(MOT_R);
208
      delay_ms(500);
231
      delay_ms(100);
209
 
232
*/
-
 
233
      brzda();
210
 
234
      
211
      cas=CASMIN;  // Cara je vlevo
235
      cas=CASMIN;  // Cara je vlevo
212
 
236
 
213
      stav=cihla;
237
      stav=cihla;
214
   }
238
   }
Line 342... Line 366...
342
      i2c_write(0x06);
366
      i2c_write(0x06);
343
      i2c_write(0);  // 80h default
367
      i2c_write(0);  // 80h default
344
      i2c_stop();
368
      i2c_stop();
345
      delay_ms(50);
369
      delay_ms(50);
346
 
370
 
347
      for(offset=0x60;offset<=254;offset+=0x04)   // Zacni od jasu 60h
371
      for(offset=0x60;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 60h
348
      {
372
      {
349
         i2c_start();      // Brightness
373
         i2c_start();      // Brightness
350
         i2c_write(0xC0);
374
         i2c_write(0xC0);
351
         i2c_write(0x06);
375
         i2c_write(0x06);
352
         i2c_write(offset);  // 80h default
376
         i2c_write(offset);  // 80h default
Line 376... Line 400...
376
 
400
 
377
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
401
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
378
   delay_ms(1);
402
   delay_ms(1);
379
   offset=read_adc()>>1;   // 0-127
403
   offset=read_adc()>>1;   // 0-127
380
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
404
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
-
 
405
   offset += 0x70;         // Jas nebude nikdy nizsi
381
   disp(offset);
406
   disp(offset);
382
   i2c_start();            // Brightness
407
   i2c_start();            // Brightness
383
   i2c_write(0xC0);
408
   i2c_write(0xC0);
384
   i2c_write(0x06);
409
   i2c_write(0x06);
385
   i2c_write(offset+0x70);  // 80h default
410
   i2c_write(offset);  // 80h default
386
   i2c_stop();
411
   i2c_stop();
387
   delay_ms(1000);   // Nech hodnotu chvili na displayi
412
   delay_ms(1000);   // Nech hodnotu chvili na displayi
388
 
413
 
389
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
414
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
390
   delay_ms(1);
415
   delay_ms(1);
Line 473... Line 498...
473
         disp(0x80);
498
         disp(0x80);
474
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
499
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
475
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
500
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
476
         set_pwm2_duty(255);
501
         set_pwm2_duty(255);
477
         disp(0x1);
502
         disp(0x1);
478
         delay_ms(200);
503
         delay_ms(400);
479
         stav=jizda;
504
         stav=jizda;
480
      }
505
      }
481
 
506
 
482
      pom=0x80;    // Zobrazeni pozice cary na displayi
507
      pom=0x80;    // Zobrazeni pozice cary na displayi
483
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
508
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;