One Framework 0.1.0
RoboMaster嵌入式框架“一键”解决方案，为你的“创意”服务。
载入中...
搜索中...
未找到
JointSetpointGenerator.hpp
浏览该文件的文档.
1#ifndef OF_JOINTINTEGRATOR_HPP
2#define OF_JOINTINTEGRATOR_HPP
3
4#include <algorithm>
5#include <cmath>
6#include <OF/utils/Units.hpp>
7
8namespace OF::JointSetpointGenerator
9{
10 using namespace Units::literals;
11
12 struct Config
13 {
14 Units::Angle min_angle; // 最小机械限位
15 Units::Angle max_angle; // 最大机械限位
16 Units::AngularVelocity max_speed; // 最大运行速度
17 Units::Time dt = 1 * ms;
18 float deadzone = 0.05f; // 遥控器死区
19 };
20
21 class Solver
22 {
23 public:
24 Solver(const Config& config)
25 :
26 m_cfg(config), m_current_angle(0 * rad)
27 {
28 }
29
30 void set_current(const Units::Angle current_angle)
31 {
32 m_current_angle = std::clamp(current_angle, m_cfg.min_angle, m_cfg.max_angle);
33 }
34
35 Units::Angle step(float input_ratio)
36 {
37 // 死区处理
38 if (std::abs(input_ratio) < m_cfg.deadzone)
39 {
40 input_ratio = 0.0;
41 }
42
43 // 位置变化量 = 速度比例 * 最大速度 * 时间
44 // mp-units 保证 [rad] = [1] * [rad/s] * [s]
45 Units::Angle delta = input_ratio * m_cfg.max_speed * m_cfg.dt;
46
47 // 累积
48 m_current_angle += delta;
49
50 // 限制目标值在安全范围内
51 m_current_angle = std::clamp(m_current_angle, m_cfg.min_angle, m_cfg.max_angle);
52
53 return m_current_angle;
54 }
55
56 [[nodiscard]] Units::Angle get_target() const { return m_current_angle; }
57
58 private:
59 Config m_cfg;
60 Units::Angle m_current_angle; // 这是积分器维护的唯一状态
61 };
62}
63#endif //OF_JOINTINTEGRATOR_HPP
OF::JointSetpointGenerator::Solver::step
Units::Angle step(float input_ratio)
定义 JointSetpointGenerator.hpp:35
OF::JointSetpointGenerator::Solver::set_current
void set_current(const Units::Angle current_angle)
定义 JointSetpointGenerator.hpp:30
OF::JointSetpointGenerator::Solver::get_target
Units::Angle get_target() const
定义 JointSetpointGenerator.hpp:56
OF::JointSetpointGenerator::Solver::Solver
Solver(const Config &config)
定义 JointSetpointGenerator.hpp:24
OF::JointSetpointGenerator
定义 JointSetpointGenerator.hpp:9
OF::Units::literals
定义 Units.hpp:53
OF::Units::Angle
quantity< radian, float > Angle
定义 Units.hpp:32
OF::Units::Time
quantity< milli< second >, float > Time
定义 Units.hpp:50
OF::Units::AngularVelocity
quantity< radian/second, float > AngularVelocity
定义 Units.hpp:35
OF::JointSetpointGenerator::Config
定义 JointSetpointGenerator.hpp:13
OF::JointSetpointGenerator::Config::deadzone
float deadzone
定义 JointSetpointGenerator.hpp:18
OF::JointSetpointGenerator::Config::min_angle
Units::Angle min_angle
定义 JointSetpointGenerator.hpp:14
OF::JointSetpointGenerator::Config::dt
Units::Time dt
定义 JointSetpointGenerator.hpp:17
OF::JointSetpointGenerator::Config::max_angle
Units::Angle max_angle
定义 JointSetpointGenerator.hpp:15
OF::JointSetpointGenerator::Config::max_speed
Units::AngularVelocity max_speed
定义 JointSetpointGenerator.hpp:16