WheelOdometry.cpp

[詳解]

#include "WheelOdometry.h"

void Coordinate::update(float distance, float angle)
{
  // 移動角度
  float theta = angle / 2.0f;

  // 直交座標の更新
  x += distance * std::cos(arg + theta);
  y += distance * std::sin(arg + theta);

  radius += distance;
  arg += theta;
}

void Coordinate::reset()
{
  radius = 0.0f;
  arg = 0.0f;
  x = 0.0f;
  y = 0.0f;
}

float WheelOdometry::velocity(std::int32_t left_motor, std::int32_t right_motor)
{
  // 回転速度から走行体の速度へ変換するときに必要な係数
  float transform = 3.14f * property.radius_wheel / 180.0f;
  // 走行体の回転角の差を求める(LPF処理つき)
  float angle = motor_angle.relativeAngleMean(left_motor, right_motor);

  return transform * angle / property.dt;
}

float WheelOdometry::angularVelocity(std::int32_t left_motor, std::int32_t right_motor)
{
  // 回転速度から角速度へ変換するときに必要な係数
  float transform = property.radius_wheel / property.between_wheels;

  // 走行体の回転角の差を求める(右モータ - 左モータ), LPF処理つき
  float angle = motor_angle.angularDifference(left_motor, right_motor);

  return transform * angle / property.dt;
}

const Coordinate& WheelOdometry::reset()
{
  coordinate.reset();
  motor_angle.reset();
  return coordinate;
}

const Coordinate& WheelOdometry::update(std::int32_t left_motor, std::int32_t right_motor)
{
  // 走行体の移動距離を計算する
  float distance = velocity(left_motor, right_motor) * property.dt;
  // 走行体の旋回角度を計算する
  float angle = angularVelocity(left_motor, right_motor) * property.dt;
  // 走行体の位置情報を更新する
  coordinate.update(distance, angle);
  return coordinate;
}

float WheelOdometry::getRotationAngle(std::int32_t left_motor, std::int32_t right_motor)
{
  // モータの角度から回転角度へ変換するときに必要な係数
  float transform = 2.0f * property.radius_wheel / property.between_wheels;

  // モータの角度を絶対値で取得する
  left_motor = motor_angle.absoluteValueOfAngle(left_motor);
  right_motor = motor_angle.absoluteValueOfAngle(right_motor);

  // モータの角度の平均値を求める
  float mean = motor_angle.absoluteAngleMean(left_motor, right_motor);

  return transform * mean;
}

const Coordinate& WheelOdometry::getCoordinate()
{
  return coordinate;
}

float WheelOdometry::getPointX()
{
  return coordinate.x;
}

float WheelOdometry::getPointY()
{
  return coordinate.y;
}