WheelOdometry.h

[詳解]

#ifndef WHEEL_ODOMETRY_H
#define WHEEL_ODOMETRY_H
#include "MotorAngle.h"
#include "RobotProperty.h"
#include <cmath>
#include <cstdint>

struct Coordinate {
  float radius;  // [極座標] 走行体の移動半径(走行体の移動距離で近似)
  float arg;     // [極座標] 走行体の移動角度
  float x;       // [直交座標] 走行体の位置(x座標)
  float y;       // [直交座標] 走行体の位置(y座標)
  Coordinate() : radius(0.0f), arg(0.0f), x(0.0f), y(0.0f) {}
  void update(float distance, float angle);
  void reset();
};

class WheelOdometry {
 private:
  MotorAngle motor_angle;  // モータ角度の情報
  RobotProperty property;  // 走行体の情報
  Coordinate coordinate;   // 走行体の現在位置の情報

 public:
  WheelOdometry(float diameter = 99.0, float tread = 147.5, float cycle = 0.04)
    : motor_angle(), property(diameter, tread, cycle), coordinate()
  {
  }
  float velocity(std::int32_t left_motor, std::int32_t right_motor);
  float angularVelocity(std::int32_t left_motor, std::int32_t right_motor);
  const Coordinate& reset();
  const Coordinate& update(std::int32_t left_motor, std::int32_t right_motor);
  float getRotationAngle(std::int32_t left_motor, std::int32_t right_motor);
  const Coordinate& getCoordinate();
  float getPointX();
  float getPointY();
};

#endif