Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/OdometryWheel/OdometryWheel.cpp

Committer:
e2011220
Date:
2021-04-14
Revision:
0:ee7e9405e1c7

File content as of revision 0:ee7e9405e1c7:

#include "OdometryWheel.h"

OdometryWheel::OdometryWheel(QEI *qei, double wheel_angle, double wheel_radius, double position_angle, double position_radius)
	:_qei(qei), _wheel_angle(wheel_angle), _wheel_radius(wheel_radius), _position_angle(position_angle), _position_radius(position_radius)
{
	_calculate_coefficient_elements();
}

Eigen::Vector3d OdometryWheel::get_coefficient_elements()
{
	return _coefficient_elements;
}

double OdometryWheel::read_wheel_velocity()
{
	double velocity;
	velocity = _qei->get_ang() * _wheel_radius;
	_qei->reset();
	return velocity;
}

void OdometryWheel::_calculate_coefficient_elements()
{
	_coefficient_elements(X) = cos(_wheel_angle);
	_coefficient_elements(Y) = sin(_wheel_angle);
	_coefficient_elements(Yaw) = sin(_wheel_angle - _position_angle) * (_position_radius);
}