encoder.c

gehe zur Dokumentation dieser Datei
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 }

Erzeugt am Wed Feb 14 16:10:02 2007 für ASURO Library von  doxygen 1.5.1-p1