C++ Encoder::getPosition方法代码示例

int main(int argc, char** argv)
{
    Motor* motor = new Motor(0);
    Encoder* encoder = new Encoder(motor);
    PIDController* pidController = new PIDController(motor, encoder, POSITION_REV, 1, 0, 0);
    pidController->setSetpoint(10);
    pidController->enable();
    
    Motor* motor2 = new Motor(1);
    Encoder* encoder2 = new Encoder(motor2);
    PIDController* pidController2 = new PIDController(motor2, encoder2, SPEED, 0, 0, 0, .4/20);
    pidController2->setSetpoint(20);
    pidController2->enable();
    
    int ticks = 0;
    
    while(!pidController->onTarget())
    {
        pidController->update();
        pidController2->update();
        std::cout << "tick:\t" << ++ticks << "\tcurrent position:\t" << encoder->getPosition() << "\tcurrent speed:\t" << encoder->getSpeed() << "\n";
        std::cout << "tick:\t" << ticks << "\tcurrent speed:\t" << encoder2->getSpeed() << "\n";
    }
    
    std::cout << "done with position " << encoder->getPosition() << "\n";
    
    return 0;
}

/*
 *  motionType parameter:
 *  1 - move forward
 *  2 - move backward
 *  3 - turn left
 *  4 - turn right
 */
void MotorController::updatePositionAndHeading(int motionType)
{
  double leftMotorPositionDelta = leftEncoder.getPosition() - _leftMotorLastPosition;
  double rightMotorPositionDelta = rightEncoder.getPosition() - _rightMotorLastPosition;

  _leftMotorLastPosition = leftEncoder.getPosition();
  _rightMotorLastPosition = rightEncoder.getPosition();

  double deltaPosition = (leftMotorPositionDelta + rightMotorPositionDelta) / 2;

  if (motionType == 1 || motionType == 2)
  {
    _position.setX(_position.getX() + deltaPosition * COS[round(_heading)]);
    _position.setY(_position.getY() + deltaPosition * SIN[round(_heading)]);

    // If robot turns slightly to left when moving forward or backward
    // if (leftMotorPositionDelta < rightMotorPositionDelta)
    // {
    //   double average = rightMotorPositionDelta - leftMotorPositionDelta;
    //   double radians = average / ROTATION_CIRCUMFERENCE;
    //   double degrees = radians * RAD_TO_DEG;   

    //   // _heading = motionType == 1 ? _heading + degrees : _heading - degrees;
    // }
    // else
    // {
    //   double average = leftMotorPositionDelta - rightMotorPositionDelta;
    //   double radians = average / ROTATION_CIRCUMFERENCE;
    //   double degrees = radians * RAD_TO_DEG;   

    //   // _heading = motionType == 1 ? _heading - degrees : _heading + degrees;
    // }
  }
  else
  {
    // Robot turns left
    if (motionType == 3)
    {
      double average = (rightMotorPositionDelta - leftMotorPositionDelta) / 2;
      double radians = average / ROTATION_RADIUS;
      double degrees = radians * RAD_TO_DEG;

      _heading += degrees;
    }
    else
    {
      double average = (leftMotorPositionDelta - rightMotorPositionDelta) / 2;
      double radians = average / ROTATION_RADIUS;
      double degrees = radians * RAD_TO_DEG;

      _heading -= degrees;
    }
  }
  _heading = _heading < 0 ? 360 + _heading : _heading;
  _heading = _heading > 360 ? _heading - 360 : _heading;
}

/*
 * Move the robot backward and update the value of the 
 * move backward command which happens to be the first
 * in the commands list, reason for being called
 */
void MotorController::doMoveBackward()
{
  fuzzyMove(-1, -1);

  _leftMotorPosition += _leftMotorLastPosition - leftEncoder.getPosition();
  _rightMotorPosition += _rightMotorLastPosition - rightEncoder.getPosition();

  updatePositionAndHeading(2);

  double average = (_leftMotorPosition + _rightMotorPosition) / 2;

  if (average >= _commands.getFirstValue())
  {
    _commands.updateFirstValue(0);
  }
}

/*
 * Move the robot forward and update the value of the 
 * move forward command which happens to be the first
 * in the commands list, reason for being called
 */
void MotorController::doMoveForward()
{
  fuzzyMove(1, 1);

  _leftMotorPosition += leftEncoder.getPosition() - _leftMotorLastPosition;
  _rightMotorPosition += rightEncoder.getPosition() - _rightMotorLastPosition;

  updatePositionAndHeading(1);

  double average = (_leftMotorPosition + _rightMotorPosition) / 2;

  if (average >= _commands.getFirstValue())
  {
    _commands.updateFirstValue(0);
  }
}

/*
 * Turn the robot to the right and update the value of the 
 * turn right command which happens to be the first
 * in the commands list, reason for being called
 */
void MotorController::doTurnRight()
{
  // When moving in place half down the speed of the motors
  // as they compose because they're moving in different Headings
  fuzzyMove(1, -1);

  _leftMotorPosition += leftEncoder.getPosition() - _leftMotorLastPosition;
  _rightMotorPosition += rightEncoder.getPosition() - _rightMotorLastPosition;

  updatePositionAndHeading(4);

  double average = (_leftMotorPosition - _rightMotorPosition) / 2;
  double radians = average / ROTATION_RADIUS;
  double degrees = radians * RAD_TO_DEG;
  
  if (degrees >= _commands.getFirstValue())
  {
    _commands.updateFirstValue(0);
  }
}