00001 /****************************************************************************/ 00023 /***************************************************************************** 00024 * * 00025 * This program is free software; you can redistribute it and/or modify * 00026 * it under the terms of the GNU General Public License as published by * 00027 * the Free Software Foundation; either version 2 of the License, or * 00028 * any later version. * 00029 * * 00030 *****************************************************************************/ 00031 #include "asuro.h" 00032 00033 00034 00035 /****************************************************************************/ 00091 void EncoderInit (void) 00092 { 00093 /* 00094 Alle definierten Interrupts im Asuro sperren. 00095 */ 00096 cli(); 00097 00098 /* 00099 Port C als Input => dadurch gehen die Back-LED aus ... 00100 */ 00101 DDRC &= ~ ((1<<PC0) | (1<<PC1)); 00102 /* 00103 ... aber nun koennen die LED's am Rad eingeschaltet werden, und die 00104 Sensoren koennen gemessen werden. 00105 */ 00106 ODOMETRIE_LED_ON; 00107 00108 /* 00109 AD-Wandler einschalten, Parameter einstellen und Starten. (clk/128) 00110 */ 00111 ADCSRA = (1<<ADEN) | (1<<ADFR) | (1<<ADIE) | (1<<ADSC) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2); 00112 00113 /* 00114 Linken Odometrie-Sensor auswaehlen. (AVCC ref. with external capacitor) 00115 */ 00116 ADMUX = (1<<ADLAR) | (1<<REFS0) | WHEEL_LEFT; 00117 00118 /* 00119 Odometrie im Interruptbetrieb weiter bearbeiten. 00120 */ 00121 autoencode = TRUE; 00122 00123 /* 00124 Alle definierten Interrupts im Asuro wieder zulassen. 00125 */ 00126 sei(); 00127 00128 /* 00129 Die Odometrie Hell-/Dunkel-Zaehler zuruecksetzen/initialisieren. 00130 */ 00131 EncoderSet (0, 0); 00132 } 00133 00134 00135 00136 /****************************************************************************/ 00159 void EncoderStop (void) 00160 { 00161 autoencode = FALSE; 00162 } 00163 00164 00165 00166 /****************************************************************************/ 00189 void EncoderStart (void) 00190 { 00191 autoencode = TRUE; 00192 } 00193 00194 00195 00196 /****************************************************************************/ 00221 void EncoderSet ( 00222 int setl, 00223 int setr) 00224 { 00225 encoder [LEFT] = setl; 00226 encoder [RIGHT] = setr; 00227 } 00228 00229 00230 00231 /*************************************************************************** 00232 * void Go(int distance, int speed) 00233 * 00234 * Go distance in mm. Attention: EncoderInit() has to be called first. 00235 * 00236 * the driven distance depends a little bit from the floor friction 00237 * limitations: maximum distance +-32m 00238 * possible problems: in full sunlight the encoder sensors may be disturbed 00239 * 00240 * input 00241 * distance: postiv->go forward ; negativ-> go backward 00242 * speed: sets motorspeed 00243 * 00244 * last modification: 00245 * Ver. Date Author Comments 00246 * ------- ---------- -------------- --------------------------------- 00247 * sto1 29.07.2005 stochri motorfunction 00248 * And1 31.07.2005 Andun added speed and Odometrie 00249 * sto2 31.10.2006 stochri distance in mm 00250 * ------- ---------- -------------- --------------------------------- 00251 * 00252 ***************************************************************************/ 00253 /****************************************************************************/ 00307 void Go ( 00308 int distance, 00309 int speed) 00310 { 00311 uint32_t enc_count; 00312 int tot_count = 0; 00313 int diff = 0; 00314 int l_speed = speed, r_speed = speed; 00315 00316 // calculation tics/mm 00317 enc_count=abs(distance)*10000L; 00318 enc_count/=19363L; 00319 00320 EncoderSet(0,0); // reset encoder 00321 00322 MotorSpeed(l_speed,r_speed); 00323 if (distance<0) MotorDir(RWD,RWD); 00324 else MotorDir(FWD,FWD); 00325 00326 while (tot_count<enc_count) 00327 { 00328 tot_count += encoder[LEFT]; 00329 diff = encoder[LEFT] - encoder[RIGHT]; 00330 if (diff > 0) 00331 { //Left faster than right 00332 if ((l_speed > speed) || (r_speed > 244)) l_speed -= 10; 00333 else r_speed += 10; 00334 } 00335 if (diff < 0) 00336 { //Right faster than left 00337 if ((r_speed > speed) || (l_speed > 244)) r_speed -= 10; 00338 else l_speed += 10; 00339 } 00340 EncoderSet(0,0); // reset encoder 00341 MotorSpeed(l_speed,r_speed); 00342 Msleep(1); 00343 } 00344 MotorDir(BREAK,BREAK); 00345 Msleep(200); 00346 } 00347 00348 00349 00350 /*************************************************************************** 00351 * void Turn(int degree, int speed) 00352 * 00353 * Turn given angle. Attention: EncoderInit() has to be called first. 00354 * 00355 * input 00356 * degree: postiv->turn right ; negativ-> turn left 00357 * 00358 * last modification: 00359 * Ver. Date Author Comments 00360 * ------- ---------- -------------- --------------------------------- 00361 * sto1 29.07.2005 stochri motorfunction 00362 * And1 07.08.2005 Andun Added Odometrie function 00363 * sto2 31.10.2006 stochri added comments, corrected enc_count initialisation 00364 * ------- ---------- -------------- --------------------------------- 00365 * 00366 ***************************************************************************/ 00367 /****************************************************************************/ 00411 void Turn ( 00412 int degree, 00413 int speed) 00414 { 00415 long enc_count; 00416 enc_count=abs(degree)*177L; 00417 enc_count /= 360L; 00418 00419 int tot_count = 0; 00420 int diff = 0; 00421 int l_speed = speed, r_speed = speed; 00422 00423 00424 EncoderSet(0,0); // reset encoder 00425 00426 MotorSpeed(l_speed,r_speed); 00427 if (degree<0) MotorDir(RWD,FWD); 00428 else MotorDir(FWD,RWD); 00429 00430 while (tot_count<enc_count) 00431 { 00432 tot_count += encoder[LEFT]; 00433 diff = encoder[LEFT] - encoder[RIGHT]; 00434 if (diff > 0) 00435 { //Left faster than right 00436 if ((l_speed > speed) || (r_speed > 244)) l_speed -= 10; 00437 else r_speed += 10; 00438 } 00439 if (diff < 0) 00440 { //Right faster than left 00441 if ((r_speed > speed) || (l_speed > 244)) r_speed -= 10; 00442 else l_speed += 10; 00443 } 00444 EncoderSet(0,0); // reset encoder 00445 MotorSpeed(l_speed,r_speed); 00446 Msleep(1); 00447 } 00448 MotorDir(BREAK,BREAK); 00449 Msleep(200); 00450 }