Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/OdometryWheel/OdometryWheel.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "OdometryWheel.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 OdometryWheel::OdometryWheel(QEI *qei, double wheel_angle, double wheel_radius, double position_angle, double position_radius)
e2011220 0:ee7e9405e1c7 4 :_qei(qei), _wheel_angle(wheel_angle), _wheel_radius(wheel_radius), _position_angle(position_angle), _position_radius(position_radius)
e2011220 0:ee7e9405e1c7 5 {
e2011220 0:ee7e9405e1c7 6 _calculate_coefficient_elements();
e2011220 0:ee7e9405e1c7 7 }
e2011220 0:ee7e9405e1c7 8
e2011220 0:ee7e9405e1c7 9 Eigen::Vector3d OdometryWheel::get_coefficient_elements()
e2011220 0:ee7e9405e1c7 10 {
e2011220 0:ee7e9405e1c7 11 return _coefficient_elements;
e2011220 0:ee7e9405e1c7 12 }
e2011220 0:ee7e9405e1c7 13
e2011220 0:ee7e9405e1c7 14 double OdometryWheel::read_wheel_velocity()
e2011220 0:ee7e9405e1c7 15 {
e2011220 0:ee7e9405e1c7 16 double velocity;
e2011220 0:ee7e9405e1c7 17 velocity = _qei->get_ang() * _wheel_radius;
e2011220 0:ee7e9405e1c7 18 _qei->reset();
e2011220 0:ee7e9405e1c7 19 return velocity;
e2011220 0:ee7e9405e1c7 20 }
e2011220 0:ee7e9405e1c7 21
e2011220 0:ee7e9405e1c7 22 void OdometryWheel::_calculate_coefficient_elements()
e2011220 0:ee7e9405e1c7 23 {
e2011220 0:ee7e9405e1c7 24 _coefficient_elements(X) = cos(_wheel_angle);
e2011220 0:ee7e9405e1c7 25 _coefficient_elements(Y) = sin(_wheel_angle);
e2011220 0:ee7e9405e1c7 26 _coefficient_elements(Yaw) = sin(_wheel_angle - _position_angle) * (_position_radius);
e2011220 0:ee7e9405e1c7 27 }