encoder.c

gehe zur Dokumentation dieser Datei

/****************************************************************************/
/*****************************************************************************
*                                                                            *
*   This program is free software; you can redistribute it and/or modify     *
*   it under the terms of the GNU General Public License as published by     *
*   the Free Software Foundation; either version 2 of the License, or        *
*   any later version.                                                       *
*                                                                            *
*****************************************************************************/
#include "asuro.h"
#include "myasuro.h"


/***************************************************************************
* void GoTurn(int distance, int degree, int speed)
*
*   Go's a distance in mm  OR
*   Turn's given angle.
* Attention: EncoderInit() has to be called first.
*
* the driven distance depends a little bit from the floor friction
* limitations: maximum distance +-32m
* possible problems: in full sunlight the encoder sensors may be disturbed
*
*   input
* distance: postiv->go forward; negativ->go backward; ZERO->use degree for TURN
* degree:   postiv->turn right; negativ->turn left
* speed:    sets motorspeed
*
* last modification:
*   Ver.     Date         Author           Comments
*   -------  ----------   --------------   ---------------------------------
*   sto1     29.07.2005   stochri          motorfunction
*   And1     31.07.2005   Andun            added speed and Odometrie
*   And2     07.08.2005   Andun            Added Odometrie function
*   sto2     31.10.2006   stochri          distance in mm
*   sto2     31.10.2006   stochri          added comments, corrected enc_count initialisation
*   stth     07.06.2007   Sternthaler      combine Go() and Turn() into this
*   -------  ----------   --------------   ---------------------------------
*
***************************************************************************/
/****************************************************************************/
void GoTurn (
  int distance,
  int degree,
  int speed)
{
  unsigned  long  enc_count;
            int   tot_count = 0;
            int   diff = 0;
            int   l_speed = speed, r_speed = speed;

  /* stop the motors until the direction is set */
  MotorSpeed (0, 0);

  /* if distance is NOT zero, then take this value to go ... */
  if (distance != 0)
  {
    /* calculate tics from mm */
    enc_count  = abs (distance) * 10000L;
    enc_count /= MY_GO_ENC_COUNT_VALUE;

    if (distance < 0)
      MotorDir (RWD, RWD);
    else
      MotorDir (FWD, FWD);
  }
  /* ... else take the value degree for a turn */
  else
  {
    /*  calculate tics from degree */
    enc_count  = abs (degree) * MY_TURN_ENC_COUNT_VALUE;
    enc_count /= 360L;

    if (degree < 0)
      MotorDir (RWD, FWD);
    else
      MotorDir (FWD, RWD);
  }

  /* reset encoder */
  EncoderSet (0, 0);

  /* now start the machine */
  MotorSpeed (l_speed, r_speed);

  while (tot_count < enc_count)
  {
    tot_count += encoder [LEFT];
    diff = encoder [LEFT] - encoder [RIGHT];

    if (diff > 0)
    { /* Left faster than right */
      if ((l_speed > speed) || (r_speed > 244))
        l_speed -= 10;
      else
        r_speed += 10;
    }

    if (diff < 0)
    { /* Right faster than left */
      if ((r_speed > speed) || (l_speed > 244))
        r_speed -= 10;
      else
        l_speed += 10;
    }
    /* reset encoder */
    EncoderSet (0, 0);

    MotorSpeed (l_speed, r_speed);
    Msleep (1);
  }
  MotorDir (BREAK, BREAK);
  Msleep (200);
}

---