encoder.c

gehe zur Dokumentation dieser Datei
00001 /****************************************************************************/
00038 /*****************************************************************************
00039 *                                                                            *
00040 *   This program is free software; you can redistribute it and/or modify     *
00041 *   it under the terms of the GNU General Public License as published by     *
00042 *   the Free Software Foundation; either version 2 of the License, or        *
00043 *   any later version.                                                       *
00044 *                                                                            *
00045 *****************************************************************************/
00046 #include "asuro.h"
00047 #include "myasuro.h"
00048 
00049 
00050 /***************************************************************************
00051 * void GoTurn(int distance, int degree, int speed)
00052 *
00053 *   Go's a distance in mm  OR
00054 *   Turn's given angle.
00055 * Attention: EncoderInit() has to be called first.
00056 *
00057 * the driven distance depends a little bit from the floor friction
00058 * limitations: maximum distance +-32m
00059 * possible problems: in full sunlight the encoder sensors may be disturbed
00060 *
00061 *   input
00062 * distance: postiv->go forward; negativ->go backward; ZERO->use degree for TURN
00063 * degree:   postiv->turn right; negativ->turn left
00064 * speed:    sets motorspeed
00065 *
00066 * last modification:
00067 *   Ver.     Date         Author           Comments
00068 *   -------  ----------   --------------   ---------------------------------
00069 *   sto1     29.07.2005   stochri          motorfunction
00070 *   And1     31.07.2005   Andun            added speed and Odometrie
00071 *   And2     07.08.2005   Andun            Added Odometrie function
00072 *   sto2     31.10.2006   stochri          distance in mm
00073 *   sto2     31.10.2006   stochri          added comments, corrected enc_count initialisation
00074 *   stth     07.06.2007   Sternthaler      combine Go() and Turn() into this
00075 *   -------  ----------   --------------   ---------------------------------
00076 *
00077 ***************************************************************************/
00078 /****************************************************************************/
00128 void GoTurn (
00129   int distance,
00130   int degree,
00131   int speed)
00132 {
00133   unsigned  long  enc_count;
00134             int   tot_count = 0;
00135             int   diff = 0;
00136             int   l_speed = speed, r_speed = speed;
00137 
00138   /* stop the motors until the direction is set */
00139   MotorSpeed (0, 0);
00140 
00141   /* if distance is NOT zero, then take this value to go ... */
00142   if (distance != 0)
00143   {
00144     /* calculate tics from mm */
00145     enc_count  = abs (distance) * 10000L;
00146     enc_count /= MY_GO_ENC_COUNT_VALUE;
00147 
00148     if (distance < 0)
00149       MotorDir (RWD, RWD);
00150     else
00151       MotorDir (FWD, FWD);
00152   }
00153   /* ... else take the value degree for a turn */
00154   else
00155   {
00156     /*  calculate tics from degree */
00157     enc_count  = abs (degree) * MY_TURN_ENC_COUNT_VALUE;
00158     enc_count /= 360L;
00159 
00160     if (degree < 0)
00161       MotorDir (RWD, FWD);
00162     else
00163       MotorDir (FWD, RWD);
00164   }
00165 
00166   /* reset encoder */
00167   EncoderSet (0, 0);
00168 
00169   /* now start the machine */
00170   MotorSpeed (l_speed, r_speed);
00171 
00172   while (tot_count < enc_count)
00173   {
00174     tot_count += encoder [LEFT];
00175     diff = encoder [LEFT] - encoder [RIGHT];
00176 
00177     if (diff > 0)
00178     { /* Left faster than right */
00179       if ((l_speed > speed) || (r_speed > 244))
00180         l_speed -= 10;
00181       else
00182         r_speed += 10;
00183     }
00184 
00185     if (diff < 0)
00186     { /* Right faster than left */
00187       if ((r_speed > speed) || (l_speed > 244))
00188         r_speed -= 10;
00189       else
00190         l_speed += 10;
00191     }
00192     /* reset encoder */
00193     EncoderSet (0, 0);
00194 
00195     MotorSpeed (l_speed, r_speed);
00196     Msleep (1);
00197   }
00198   MotorDir (BREAK, BREAK);
00199   Msleep (200);
00200 }

Erzeugt am Sun Nov 18 18:24:52 2007 für ASURO Library von  doxygen 1.5.1-p1