One Framework 0.1.0
RoboMaster嵌入式框架“一键”解决方案，为你的“创意”服务。
载入中...
搜索中...
未找到
Mecanum.hpp
浏览该文件的文档.
1#ifndef OF_MECANUM_HPP
2#define OF_MECANUM_HPP
3#include <OF/utils/Units.hpp>
4
5namespace OF::Algo::Mecanum
6{
7 using namespace Units;
8 using namespace Units::literals;
9
10 struct Config
11 {
12 Length wheel_radius;
13 Length track_width;
14 Length wheel_base;
15
16 [[nodiscard]] constexpr Length geometry_factor() const
17 {
18 return (track_width + wheel_base) / 2.0f;
19 }
20 };
21
28
29 struct WheelStates
30 {
31 AngularVelocity fl; // Front-Left
32 AngularVelocity fr; // Front-Right
33 AngularVelocity bl; // Back-Left
34 AngularVelocity br; // Back-Right
35 };
36
37 class Solver
38 {
39 public:
40 constexpr explicit Solver(const Config& config) : m_config(config)
41 {
42 }
43
44 [[nodiscard]] constexpr WheelStates inverse(const ChassisSpeeds& cmd) const
45 {
46 const auto K = m_config.geometry_factor();
47 // K * vw = [m] * [rad/s] = [m·rad/s]
48 // 需要除以 rad
49 auto v_fl = cmd.vx - cmd.vy - K * cmd.vw / rad;
50 auto v_fr = cmd.vx + cmd.vy + K * cmd.vw / rad;
51 auto v_bl = cmd.vx + cmd.vy - K * cmd.vw / rad;
52 auto v_br = cmd.vx - cmd.vy + K * cmd.vw / rad;
53 return {
54 v_fl / m_config.wheel_radius * rad,
55 v_fr / m_config.wheel_radius * rad,
56 v_bl / m_config.wheel_radius * rad,
57 v_br / m_config.wheel_radius * rad
58 };
59 }
60
61 [[nodiscard]] constexpr ChassisSpeeds forward(const WheelStates& wheels) const
62 {
63 const auto K = m_config.geometry_factor();
64 auto v_fl = wheels.fl * m_config.wheel_radius / rad;
65 auto v_fr = wheels.fr * m_config.wheel_radius / rad;
66 auto v_bl = wheels.bl * m_config.wheel_radius / rad;
67 auto v_br = wheels.br * m_config.wheel_radius / rad;
68 return {
69 .vx = (v_fl + v_fr + v_bl + v_br) / 4.0,
70 .vy = (-v_fl + v_fr + v_bl - v_br) / 4.0,
71 // 分子是 [m/s]，分母 K 是 [m]
72 // [m/s] / [m] = [1/s] (Frequency)。我们需要 [rad/s]，所以必须 * rad
73 .vw = ((-v_fl + v_fr - v_bl + v_br) / (4.0 * K)) * rad
74 };
75 }
76
77 private:
78 Config m_config;
79 };
80}
81
82#endif //OF_MECANUM_HPP
OF::Algo::Mecanum::Solver::Solver
constexpr Solver(const Config &config)
定义 Mecanum.hpp:40
OF::Algo::Mecanum::Solver::forward
constexpr ChassisSpeeds forward(const WheelStates &wheels) const
定义 Mecanum.hpp:61
OF::Algo::Mecanum::Solver::inverse
constexpr WheelStates inverse(const ChassisSpeeds &cmd) const
定义 Mecanum.hpp:44
OF::Algo::Mecanum
定义 Mecanum.hpp:6
OF::Units::literals
定义 Units.hpp:51
OF::Units
定义 Units.hpp:13
OF::Units::AngularVelocity
quantity< radian/second, float > AngularVelocity
定义 Units.hpp:35
OF::Units::Velocity
quantity< metre/second > Velocity
定义 Units.hpp:30
OF::Units::Length
quantity< metre, float > Length
定义 Units.hpp:28
OF::Algo::Mecanum::ChassisSpeeds::vy
Velocity vy
定义 Mecanum.hpp:25
OF::Algo::Mecanum::ChassisSpeeds::vx
Velocity vx
定义 Mecanum.hpp:24
OF::Algo::Mecanum::ChassisSpeeds::vw
AngularVelocity vw
定义 Mecanum.hpp:26
OF::Algo::Mecanum::Config
定义 Mecanum.hpp:11
OF::Algo::Mecanum::Config::wheel_base
Length wheel_base
轴距 (前后轮中心距离)
定义 Mecanum.hpp:14
OF::Algo::Mecanum::Config::track_width
Length track_width
轮距 (左右轮中心距离)
定义 Mecanum.hpp:13
OF::Algo::Mecanum::Config::wheel_radius
Length wheel_radius
轮子半径
定义 Mecanum.hpp:12
OF::Algo::Mecanum::Config::geometry_factor
constexpr Length geometry_factor() const
定义 Mecanum.hpp:16
OF::Algo::Mecanum::WheelStates
定义 Mecanum.hpp:30
OF::Algo::Mecanum::WheelStates::br
AngularVelocity br
定义 Mecanum.hpp:34
OF::Algo::Mecanum::WheelStates::fl
AngularVelocity fl
定义 Mecanum.hpp:31
OF::Algo::Mecanum::WheelStates::fr
AngularVelocity fr
定义 Mecanum.hpp:32
OF::Algo::Mecanum::WheelStates::bl
AngularVelocity bl
定义 Mecanum.hpp:33