Subversion Repositories svnkaklik

Rev

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

Rev 191 Rev 192
Line 1... Line 1...
1 
//******** Robot Camerus pro IstRobot 2007 ************
 1 
//******** Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 191 2007-03-16 21:34:52Z kakl $"
 2 
//"$Id: camerus.c 192 2007-03-16 22:56:25Z 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
Line 52... Line 52...
52 
 
 52 
 
53 
   set_pwm1_duty(0);    // vypni PWM
 53 
   set_pwm1_duty(0);    // vypni PWM
54 
   set_pwm2_duty(0);
 54 
   set_pwm2_duty(0);
55 
   setup_ccp1(CCP_OFF);
55 
   setup_ccp1(CCP_OFF);
56 
   setup_ccp2(CCP_OFF);
56 
   setup_ccp2(CCP_OFF);
57 
   for (n=0;n<255;n++)
 57 
   for (n=0;n<150;n++)
58 
   {
 58 
   {
59 
      output_low(MOT_L);
59 
      output_low(MOT_L);
60 
      output_low(MOT_R);
60 
      output_low(MOT_R);
61 
      output_high(MOT_1);
61 
      output_high(MOT_1);
62 
      output_high(MOT_2);
 62 
      output_high(MOT_2);
Line 119... Line 119...
119 
      output_low(MOT_R);
 119 
      output_low(MOT_R);
120 
      return;
 120 
      return;
121 
   };
 121 
   };
122 
 
 122 
 
123 
   SetServo((CASAVR-CASMIN));   // rovne
 123 
   SetServo((CASAVR-CASMIN));   // rovne
124 
   delay_ms(100);    // Doba reverzace
 124 
   delay_ms(200);    // Doba reverzace
125 
 
 125 
 
126 
   brzda();
 126 
   brzda();
127 
 
 127 
 
128 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
 128 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
129 
 
 129 
 
130 
   if(stav==jizda)   // Objed cihlu
 130 
   if(stav==jizda)   // Objed cihlu
131 
   {
 131 
   {
132 
      int n;
 132 
      int n;
133 
 
 133 
 
134 
      SetServo(CASMIN);   // prudce doleva
 134 
      SetServo(CASMIN);   // prudce doleva
135 
      delay_ms(200);
 - 
 
136 
 
 - 
 
137 
      set_pwm1_duty(150);  // vpred
 135 
      set_pwm1_duty(0);  // vpred
138 
      set_pwm2_duty(200);
 136 
      set_pwm2_duty(200);
139 
      output_low(MOT_L);
 137 
      output_low(MOT_L);
140 
      output_low(MOT_R);
 138 
      output_low(MOT_R);
141 
      n=0;
 139 
      n=0;
142 
      while(true)
 140 
      while(true)
143 
      {
 141 
      {
144 
         while(input(ODO));
 142 
         while(input(ODO));
145 
         while(!input(ODO));
 143 
         while(!input(ODO));
146 
         n++;
 144 
         n++;
147 
         if(n==5) break;
 145 
         if(n==7) break;
148 
      }
 146 
      }
149 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 147 
      set_pwm1_duty(0);    // reverz (zabrzdi)
150 
      set_pwm2_duty(0);
 148 
      set_pwm2_duty(0);
151 
      output_high(MOT_L);
 149 
      output_low(MOT_L);
152 
      output_high(MOT_R);
 150 
      output_high(MOT_R);
- 
 
 151 
      delay_ms(200);
- 
 
 152 
      brzda();
153 
 
 153 
 
154 
      SetServo((CASAVR-CASMIN));   // rovne
 154 
      SetServo((CASAVR-CASMIN));   // rovne
155 
      set_pwm1_duty(140);  // vpred
 155 
      set_pwm1_duty(140);  // vpred
156 
      set_pwm2_duty(140);
 156 
      set_pwm2_duty(140);
157 
      output_low(MOT_L);
 157 
      output_low(MOT_L);
Line 160... Line 160...
160 
      while(true)
 160 
      while(true)
161 
      {
 161 
      {
162 
         while(input(ODO));
 162 
         while(input(ODO));
163 
         while(!input(ODO));
 163 
         while(!input(ODO));
164 
         n++;
 164 
         n++;
165 
         if(n==6) break;
 165 
         if(n==10) break;
166 
      }
 166 
      }
167 
 
 167 
 
- 
 
 168 
      set_pwm1_duty(0);    // reverz (zabrzdi)
- 
 
 169 
      set_pwm2_duty(0);
- 
 
 170 
      output_high(MOT_L);
- 
 
 171 
      output_high(MOT_R);
- 
 
 172 
      delay_ms(200);
168 
      brzda();
173 
      brzda();
169 
 
 174 
 
- 
 
 175 
      SetServo(CASMIN);   // max. doleva
- 
 
 176 
      set_pwm1_duty(0);   // vzad
- 
 
 177 
      set_pwm2_duty(20);
- 
 
 178 
      output_low(MOT_L);
- 
 
 179 
      output_high(MOT_R);
- 
 
 180 
      n=0;
- 
 
 181 
      while(true)
- 
 
 182 
      {
- 
 
 183 
         while(input(ODO));
- 
 
 184 
         while(!input(ODO));
- 
 
 185 
         n++;
- 
 
 186 
         if(n==9) break;
- 
 
 187 
      }
- 
 
 188 
      set_pwm1_duty(0);    // reverz (zabrzdi)
- 
 
 189 
      set_pwm2_duty(255);
- 
 
 190 
      output_low(MOT_L);
- 
 
 191 
      output_low(MOT_R);
- 
 
 192 
      delay_ms(200);
- 
 
 193 
 
- 
 
 194 
      brzda();
- 
 
 195 
 
170 
      SetServo((CASAVR-CASMIN)+70);   // doprava
 196 
      SetServo((CASAVR-CASMIN));   // rovne
171 
      set_pwm1_duty(140);  // vpred
 197 
      set_pwm1_duty(140);  // vpred
172 
      set_pwm2_duty(140);
 198 
      set_pwm2_duty(140);
173 
      output_low(MOT_L);
 199 
      output_low(MOT_L);
174 
      output_low(MOT_R);
 200 
      output_low(MOT_R);
175 
      n=0;
 201 
      n=0;
176 
      while(true)
 202 
      while(true)
177 
      {
 203 
      {
178 
         while(input(ODO));
 204 
         while(input(ODO));
179 
         while(!input(ODO));
 205 
         while(!input(ODO));
180 
         n++;
 206 
         n++;
181 
         if(n==6) break;
 207 
         if(n==13) break;
182 
      }
 208 
      }
183 
 
 209 
 
184 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 210 
      set_pwm1_duty(0);    // reverz (zabrzdi)
185 
      set_pwm2_duty(0);
 211 
      set_pwm2_duty(0);
186 
      output_high(MOT_L);
 212 
      output_high(MOT_L);
187 
      output_high(MOT_R);
 213 
      output_high(MOT_R);
188 
      delay_ms(100);
 214 
      delay_ms(200);
189 
 
 - 
 
190 
      brzda();
215 
      brzda();
191 
 
 216 
 
192 
      SetServo(CASMIN);   // max. doleva
 217 
      SetServo(CASMIN);   // max. doleva
193 
      set_pwm1_duty(0);   // vzad
 218 
      set_pwm1_duty(0);   // vzad
194 
      set_pwm2_duty(20);
 219 
      set_pwm2_duty(20);
Line 198... Line 223...
198 
      while(true)
 223 
      while(true)
199 
      {
 224 
      {
200 
         while(input(ODO));
 225 
         while(input(ODO));
201 
         while(!input(ODO));
 226 
         while(!input(ODO));
202 
         n++;
 227 
         n++;
203 
         if(n==5) break;
 228 
         if(n==6) break;
204 
      }
 229 
      }
205 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 230 
      set_pwm1_duty(0);    // reverz (zabrzdi)
206 
      set_pwm2_duty(255);
 231 
      set_pwm2_duty(255);
207 
      output_low(MOT_L);
 232 
      output_low(MOT_L);
208 
      output_low(MOT_R);
 233 
      output_low(MOT_R);
209 
      delay_ms(100);
 234 
      delay_ms(200);
210 
 
 235 
 
211 
      brzda();
 236 
      brzda();
212 
 
 237 
 
213 
      SetServo((CASAVR-CASMIN)+20);   // mirne doprava
 238 
      SetServo((CASAVR-CASMIN));   // rovne
214 
      set_pwm1_duty(190);  // vpred
 239 
      set_pwm1_duty(190);  // vpred
215 
      set_pwm2_duty(190);
 240 
      set_pwm2_duty(190);
216 
      output_low(MOT_L);
 241 
      output_low(MOT_L);
217 
      output_low(MOT_R);
 242 
      output_low(MOT_R);
218 
      n=0;
 243 
      n=0;
219 
      while(true)
 244 
      while(true)
220 
      {
 245 
      {
221 
         while(input(ODO));
 246 
         while(input(ODO));
222 
         while(!input(ODO));
 247 
         while(!input(ODO));
223 
         n++;
 248 
         n++;
224 
         if(n==14) break;
 249 
         if(n==9) break;
225 
      }
 250 
      }
226 
/*
 251 
/*
227 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 252 
      set_pwm1_duty(0);    // reverz (zabrzdi)
228 
      set_pwm2_duty(0);
 253 
      set_pwm2_duty(0);
229 
      output_high(MOT_L);
 254 
      output_high(MOT_L);