RogiStandard-hal
STM32 向けサークル共通ライブラリ
読み取り中…
検索中…
一致する文字列を見つけられません
OmniWheelController.hpp
[詳解]
1
5#pragma once
6
7#include <units/velocity.h>
8
9#include <Eigen/Dense>
10#include <array>
11#include <cstddef>
12
13#include "Vector2d/Twist2d.hpp"
14#include "Wheel.hpp"
15#include "WheelMatrix.hpp"
16
55template <int N>
57 static constexpr std::size_t Nz = static_cast<std::size_t>(N);
58
59 public:
65 OmniWheelController(std::array<MotorWheel, Nz>& motor_wheels) {
66 std::array<WheelPositions, Nz> wheel_positions;
67 for (std::size_t i = 0; i < Nz; i++) {
68 wheel_positions[i] = motor_wheels[i].positions;
69 }
70 wheel_matrix_ = getWheelMatrix<N>(wheel_positions);
71 }
72
79 std::array<float, Nz> getTargetRps(Twist2D twist) {
80 Eigen::Vector<float, 3> twist_vector;
81 twist_vector << twist.x.value(), twist.y.value(), twist.theta.value();
82
83 Eigen::Vector<float, N> motor_speeds = wheel_matrix_ * twist_vector;
84
85 std::array<float, Nz> result;
86 for (int i = 0; i < N; i++) {
87 result[static_cast<std::size_t>(i)] = motor_speeds(i);
88 }
89
90 return result;
91 }
92
93 private:
94 Eigen::Matrix<float, N, 3> wheel_matrix_;
95};
機体の目標速度から、各車輪の目標回転速度を求める
std::array< float, Nz > getTargetRps(Twist2D twist)
機体の目標速度から、各車輪の目標回転速度を求める
OmniWheelController(std::array< MotorWheel, Nz > &motor_wheels)
車輪の配置から変換行列を計算する
Eigen::Matrix< float, N, 3 > getWheelMatrix(const std::array< WheelPositions, static_cast< std::size_t >(N)> &wheel_positions)