High level mecanum robot controller library
Dependents: WRS2021_mecanum_driver
mecanum_controller.hpp@2:bc2e5034dda7, 2021-08-25 (annotated)
- Committer:
- sgrsn
- Date:
- Wed Aug 25 12:11:24 2021 +0000
- Revision:
- 2:bc2e5034dda7
- Parent:
- 1:26c16fb42252
Fix calculation of odometry
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:25bde5ebdd82 | 1 | #ifndef MECANUM_CONTROLLER_HPP |
sgrsn | 0:25bde5ebdd82 | 2 | #define MECANUM_CONTROLLER_HPP |
sgrsn | 0:25bde5ebdd82 | 3 | |
sgrsn | 0:25bde5ebdd82 | 4 | #include "mecanum_interface.hpp" |
sgrsn | 0:25bde5ebdd82 | 5 | #include "imu_interface.hpp" |
sgrsn | 0:25bde5ebdd82 | 6 | #include "pid_controller.hpp" |
sgrsn | 0:25bde5ebdd82 | 7 | #include "math.h" |
sgrsn | 0:25bde5ebdd82 | 8 | |
sgrsn | 0:25bde5ebdd82 | 9 | #include <vector> |
sgrsn | 0:25bde5ebdd82 | 10 | |
sgrsn | 0:25bde5ebdd82 | 11 | //const double PI = 3.14159265359; |
sgrsn | 0:25bde5ebdd82 | 12 | const float DEG_TO_RAD = PI/180.0; |
sgrsn | 0:25bde5ebdd82 | 13 | const float RAD_TO_DEG = 180.0/PI; |
sgrsn | 0:25bde5ebdd82 | 14 | |
sgrsn | 0:25bde5ebdd82 | 15 | using namespace std::chrono; |
sgrsn | 0:25bde5ebdd82 | 16 | |
sgrsn | 0:25bde5ebdd82 | 17 | class Pose2D |
sgrsn | 0:25bde5ebdd82 | 18 | { |
sgrsn | 0:25bde5ebdd82 | 19 | public: |
sgrsn | 1:26c16fb42252 | 20 | Pose2D(double x=0, double y=0, double theta=0) : x(x), y(y), theta(theta) |
sgrsn | 0:25bde5ebdd82 | 21 | { |
sgrsn | 0:25bde5ebdd82 | 22 | } |
sgrsn | 0:25bde5ebdd82 | 23 | Pose2D(double x, double y, double theta, double time) : x(x), y(y), theta(theta), time_stamp(time) |
sgrsn | 0:25bde5ebdd82 | 24 | { |
sgrsn | 0:25bde5ebdd82 | 25 | } |
sgrsn | 0:25bde5ebdd82 | 26 | Pose2D operator +(Pose2D ref) |
sgrsn | 0:25bde5ebdd82 | 27 | { |
sgrsn | 0:25bde5ebdd82 | 28 | return Pose2D(x + ref.x, y + ref.y, theta + ref.theta); |
sgrsn | 0:25bde5ebdd82 | 29 | } |
sgrsn | 0:25bde5ebdd82 | 30 | Pose2D operator -(Pose2D ref) |
sgrsn | 0:25bde5ebdd82 | 31 | { |
sgrsn | 0:25bde5ebdd82 | 32 | return Pose2D(x - ref.x, y - ref.y, theta - ref.theta); |
sgrsn | 0:25bde5ebdd82 | 33 | } |
sgrsn | 0:25bde5ebdd82 | 34 | Pose2D operator *(Pose2D ref) |
sgrsn | 0:25bde5ebdd82 | 35 | { |
sgrsn | 0:25bde5ebdd82 | 36 | return Pose2D(x * ref.x, y * ref.y, theta * ref.theta); |
sgrsn | 0:25bde5ebdd82 | 37 | } |
sgrsn | 0:25bde5ebdd82 | 38 | Pose2D operator *(double val) |
sgrsn | 0:25bde5ebdd82 | 39 | { |
sgrsn | 0:25bde5ebdd82 | 40 | return Pose2D(x * val, y * val, theta * val); |
sgrsn | 0:25bde5ebdd82 | 41 | } |
sgrsn | 0:25bde5ebdd82 | 42 | /*Pose2D operator /(Pose2D ref) |
sgrsn | 0:25bde5ebdd82 | 43 | { |
sgrsn | 0:25bde5ebdd82 | 44 | return Pose2D(x / ref.x, y / ref.y, theta / ref.theta); |
sgrsn | 0:25bde5ebdd82 | 45 | }*/ |
sgrsn | 0:25bde5ebdd82 | 46 | double operator /(Pose2D ref) |
sgrsn | 0:25bde5ebdd82 | 47 | { |
sgrsn | 0:25bde5ebdd82 | 48 | return std::max( std::max( abs(x / ref.x), abs(y / ref.y) ), abs(theta / ref.theta) ); |
sgrsn | 0:25bde5ebdd82 | 49 | } |
sgrsn | 0:25bde5ebdd82 | 50 | Pose2D operator /(double val) |
sgrsn | 0:25bde5ebdd82 | 51 | { |
sgrsn | 0:25bde5ebdd82 | 52 | return Pose2D(x / val, y / val, theta / val); |
sgrsn | 0:25bde5ebdd82 | 53 | } |
sgrsn | 0:25bde5ebdd82 | 54 | void ConstraintUpper(Pose2D ref, Pose2D dir) |
sgrsn | 0:25bde5ebdd82 | 55 | { |
sgrsn | 0:25bde5ebdd82 | 56 | if(dir.x > 0 && x > ref.x) x = ref.x; |
sgrsn | 0:25bde5ebdd82 | 57 | if(dir.y > 0 && y > ref.y) y = ref.y; |
sgrsn | 0:25bde5ebdd82 | 58 | if(dir.theta > 0 && theta > ref.theta) theta = ref.theta; |
sgrsn | 0:25bde5ebdd82 | 59 | if(dir.x < 0 && x < ref.x) x = ref.x; |
sgrsn | 0:25bde5ebdd82 | 60 | if(dir.y < 0 && y < ref.y) y = ref.y; |
sgrsn | 0:25bde5ebdd82 | 61 | if(dir.theta < 0 && theta < ref.theta) theta = ref.theta; |
sgrsn | 0:25bde5ebdd82 | 62 | } |
sgrsn | 0:25bde5ebdd82 | 63 | void ConstraintLower(Pose2D ref, Pose2D dir) |
sgrsn | 0:25bde5ebdd82 | 64 | { |
sgrsn | 0:25bde5ebdd82 | 65 | if(dir.x > 0 && x < ref.x) x = ref.x; |
sgrsn | 0:25bde5ebdd82 | 66 | if(dir.y > 0 && y < ref.y) y = ref.y; |
sgrsn | 0:25bde5ebdd82 | 67 | if(dir.theta > 0 && theta < ref.theta) theta = ref.theta; |
sgrsn | 0:25bde5ebdd82 | 68 | if(dir.x < 0 && x > ref.x) x = ref.x; |
sgrsn | 0:25bde5ebdd82 | 69 | if(dir.y < 0 && y > ref.y) y = ref.y; |
sgrsn | 0:25bde5ebdd82 | 70 | if(dir.theta < 0 && theta > ref.theta) theta = ref.theta; |
sgrsn | 0:25bde5ebdd82 | 71 | } |
sgrsn | 0:25bde5ebdd82 | 72 | double x = 0; |
sgrsn | 0:25bde5ebdd82 | 73 | double y = 0; |
sgrsn | 0:25bde5ebdd82 | 74 | double theta = 0; |
sgrsn | 0:25bde5ebdd82 | 75 | double time_stamp = 0; |
sgrsn | 0:25bde5ebdd82 | 76 | }; |
sgrsn | 0:25bde5ebdd82 | 77 | |
sgrsn | 0:25bde5ebdd82 | 78 | class Vel2D |
sgrsn | 0:25bde5ebdd82 | 79 | { |
sgrsn | 0:25bde5ebdd82 | 80 | public: |
sgrsn | 0:25bde5ebdd82 | 81 | Vel2D(double x, double y, double theta) : x(x), y(y), theta(theta) |
sgrsn | 0:25bde5ebdd82 | 82 | { |
sgrsn | 0:25bde5ebdd82 | 83 | } |
sgrsn | 0:25bde5ebdd82 | 84 | double x = 0; |
sgrsn | 0:25bde5ebdd82 | 85 | double y = 0; |
sgrsn | 0:25bde5ebdd82 | 86 | double theta = 0; |
sgrsn | 0:25bde5ebdd82 | 87 | }; |
sgrsn | 0:25bde5ebdd82 | 88 | |
sgrsn | 0:25bde5ebdd82 | 89 | struct MecanumParameter |
sgrsn | 0:25bde5ebdd82 | 90 | { |
sgrsn | 0:25bde5ebdd82 | 91 | const double L = 0.233; //233 [mm] |
sgrsn | 0:25bde5ebdd82 | 92 | const double wheel_radius = 0.0635; // メカナム半径[m] |
sgrsn | 0:25bde5ebdd82 | 93 | const double deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) |
sgrsn | 0:25bde5ebdd82 | 94 | }; |
sgrsn | 0:25bde5ebdd82 | 95 | |
sgrsn | 0:25bde5ebdd82 | 96 | class ElapsedTimer |
sgrsn | 0:25bde5ebdd82 | 97 | { |
sgrsn | 0:25bde5ebdd82 | 98 | public: |
sgrsn | 0:25bde5ebdd82 | 99 | ElapsedTimer() |
sgrsn | 0:25bde5ebdd82 | 100 | { |
sgrsn | 0:25bde5ebdd82 | 101 | timer_.start(); |
sgrsn | 0:25bde5ebdd82 | 102 | last_us_ = chrono::duration_cast<chrono::microseconds>(timer_.elapsed_time()).count(); |
sgrsn | 0:25bde5ebdd82 | 103 | } |
sgrsn | 0:25bde5ebdd82 | 104 | |
sgrsn | 0:25bde5ebdd82 | 105 | double GetElapsedSeconds() |
sgrsn | 0:25bde5ebdd82 | 106 | { |
sgrsn | 0:25bde5ebdd82 | 107 | uint64_t current_us = chrono::duration_cast<chrono::microseconds>(timer_.elapsed_time()).count(); |
sgrsn | 0:25bde5ebdd82 | 108 | double dt = (double)(current_us - last_us_) / 1000000; |
sgrsn | 0:25bde5ebdd82 | 109 | last_us_ = current_us; |
sgrsn | 0:25bde5ebdd82 | 110 | return dt; |
sgrsn | 0:25bde5ebdd82 | 111 | } |
sgrsn | 0:25bde5ebdd82 | 112 | |
sgrsn | 0:25bde5ebdd82 | 113 | private: |
sgrsn | 0:25bde5ebdd82 | 114 | Timer timer_; |
sgrsn | 0:25bde5ebdd82 | 115 | uint64_t last_us_ = 0; |
sgrsn | 0:25bde5ebdd82 | 116 | }; |
sgrsn | 0:25bde5ebdd82 | 117 | |
sgrsn | 0:25bde5ebdd82 | 118 | class PoseController |
sgrsn | 0:25bde5ebdd82 | 119 | { |
sgrsn | 0:25bde5ebdd82 | 120 | public: |
sgrsn | 0:25bde5ebdd82 | 121 | PoseController() |
sgrsn | 0:25bde5ebdd82 | 122 | { |
sgrsn | 0:25bde5ebdd82 | 123 | /*id_name_.emplace_back("pose_x"); |
sgrsn | 0:25bde5ebdd82 | 124 | id_name_.emplace_back("pose_y"); |
sgrsn | 0:25bde5ebdd82 | 125 | id_name_.emplace_back("pose_theta"); |
sgrsn | 0:25bde5ebdd82 | 126 | |
sgrsn | 0:25bde5ebdd82 | 127 | for(int i=0; i<id_name_.size(); i++) { |
sgrsn | 0:25bde5ebdd82 | 128 | id_map_[id_name_[i]] = i; |
sgrsn | 0:25bde5ebdd82 | 129 | }*/ |
sgrsn | 0:25bde5ebdd82 | 130 | //pid_param_.resize(id_name_.size()); |
sgrsn | 0:25bde5ebdd82 | 131 | //controller_.resize(id_name_.size(), 0); |
sgrsn | 0:25bde5ebdd82 | 132 | pid_param_.resize(3); |
sgrsn | 0:25bde5ebdd82 | 133 | controller_.resize(3); |
sgrsn | 0:25bde5ebdd82 | 134 | |
sgrsn | 0:25bde5ebdd82 | 135 | // for pose x |
sgrsn | 0:25bde5ebdd82 | 136 | pid_param_[0].bound.upper = 1.0; |
sgrsn | 0:25bde5ebdd82 | 137 | pid_param_[0].bound.lower = -1.0; |
sgrsn | 0:25bde5ebdd82 | 138 | pid_param_[0].threshold.upper = 0.001; |
sgrsn | 0:25bde5ebdd82 | 139 | pid_param_[0].threshold.lower = -0.001; |
sgrsn | 0:25bde5ebdd82 | 140 | pid_param_[0].kp = 0.5; |
sgrsn | 0:25bde5ebdd82 | 141 | pid_param_[0].ki = 0.03; |
sgrsn | 0:25bde5ebdd82 | 142 | pid_param_[0].kd = 0.0; |
sgrsn | 0:25bde5ebdd82 | 143 | |
sgrsn | 0:25bde5ebdd82 | 144 | // for pose y |
sgrsn | 0:25bde5ebdd82 | 145 | pid_param_[1].bound.upper = 1.0; |
sgrsn | 0:25bde5ebdd82 | 146 | pid_param_[1].bound.lower = -1.0; |
sgrsn | 0:25bde5ebdd82 | 147 | pid_param_[1].threshold.upper = 0.001; |
sgrsn | 0:25bde5ebdd82 | 148 | pid_param_[1].threshold.lower = -0.001; |
sgrsn | 0:25bde5ebdd82 | 149 | pid_param_[1].kp = 0.5; |
sgrsn | 0:25bde5ebdd82 | 150 | pid_param_[1].ki = 0.03; |
sgrsn | 0:25bde5ebdd82 | 151 | pid_param_[1].kd = 0.0; |
sgrsn | 0:25bde5ebdd82 | 152 | |
sgrsn | 0:25bde5ebdd82 | 153 | // for pose theta |
sgrsn | 0:25bde5ebdd82 | 154 | pid_param_[2].bound.upper = 1.0; |
sgrsn | 0:25bde5ebdd82 | 155 | pid_param_[2].bound.lower = -1.0; |
sgrsn | 0:25bde5ebdd82 | 156 | pid_param_[2].threshold.upper = 0.005; |
sgrsn | 0:25bde5ebdd82 | 157 | pid_param_[2].threshold.lower = -0.005; |
sgrsn | 0:25bde5ebdd82 | 158 | pid_param_[2].kp = 1.50; |
sgrsn | 0:25bde5ebdd82 | 159 | pid_param_[2].ki = 0.2; |
sgrsn | 0:25bde5ebdd82 | 160 | pid_param_[2].kd = 0.05; |
sgrsn | 0:25bde5ebdd82 | 161 | |
sgrsn | 0:25bde5ebdd82 | 162 | controller_[0] = std::make_unique<PIDControl>(pid_param_[0]); |
sgrsn | 0:25bde5ebdd82 | 163 | controller_[1] = std::make_unique<PIDControl>(pid_param_[1]); |
sgrsn | 0:25bde5ebdd82 | 164 | controller_[2] = std::make_unique<PIDControl>(pid_param_[2]); |
sgrsn | 0:25bde5ebdd82 | 165 | //controller_[0] -> parameter(pid_param_[0]); |
sgrsn | 0:25bde5ebdd82 | 166 | //controller_[1] -> parameter(pid_param_[1]); |
sgrsn | 0:25bde5ebdd82 | 167 | //controller_[2] -> parameter(pid_param_[2]); |
sgrsn | 0:25bde5ebdd82 | 168 | } |
sgrsn | 0:25bde5ebdd82 | 169 | |
sgrsn | 0:25bde5ebdd82 | 170 | protected: |
sgrsn | 0:25bde5ebdd82 | 171 | std::vector<PIDControl::Parameter> pid_param_; //[x, y, theta] |
sgrsn | 0:25bde5ebdd82 | 172 | //std::map<std::string, uint16_t> id_map_; |
sgrsn | 0:25bde5ebdd82 | 173 | //std::vector<std::string> id_name_; |
sgrsn | 0:25bde5ebdd82 | 174 | std::vector<std::unique_ptr<PIDControl>> controller_; |
sgrsn | 0:25bde5ebdd82 | 175 | }; |
sgrsn | 0:25bde5ebdd82 | 176 | |
sgrsn | 0:25bde5ebdd82 | 177 | class MecanumController : PoseController |
sgrsn | 0:25bde5ebdd82 | 178 | { |
sgrsn | 0:25bde5ebdd82 | 179 | public: |
sgrsn | 0:25bde5ebdd82 | 180 | MecanumController(MecanumInterface *mecanum, IMUInterface *imu) : pose_(0, 0, 0), PoseController() |
sgrsn | 0:25bde5ebdd82 | 181 | { |
sgrsn | 0:25bde5ebdd82 | 182 | mecanum_ = mecanum; |
sgrsn | 0:25bde5ebdd82 | 183 | imu_ = imu; |
sgrsn | 0:25bde5ebdd82 | 184 | } |
sgrsn | 0:25bde5ebdd82 | 185 | |
sgrsn | 0:25bde5ebdd82 | 186 | void ControllVelocity(Vel2D vel) |
sgrsn | 0:25bde5ebdd82 | 187 | { |
sgrsn | 0:25bde5ebdd82 | 188 | double v[MOTOR_NUM] = {}; |
sgrsn | 0:25bde5ebdd82 | 189 | // 各車輪の速度 |
sgrsn | 0:25bde5ebdd82 | 190 | v[0] = -vel.x + vel.y - param_.L * vel.theta; // m/sec |
sgrsn | 0:25bde5ebdd82 | 191 | v[1] = -vel.x - vel.y - param_.L * vel.theta; |
sgrsn | 0:25bde5ebdd82 | 192 | v[2] = vel.x - vel.y - param_.L * vel.theta; |
sgrsn | 0:25bde5ebdd82 | 193 | v[3] = vel.x + vel.y - param_.L * vel.theta; |
sgrsn | 0:25bde5ebdd82 | 194 | |
sgrsn | 0:25bde5ebdd82 | 195 | // 各モータのパルスに変換 |
sgrsn | 0:25bde5ebdd82 | 196 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:25bde5ebdd82 | 197 | { |
sgrsn | 0:25bde5ebdd82 | 198 | float f = v[i] / param_.wheel_radius * RAD_TO_DEG / param_.deg_per_pulse; |
sgrsn | 0:25bde5ebdd82 | 199 | wheel_frequency_[i] = mecanum_ -> ControlMotor(i, (int)f); |
sgrsn | 0:25bde5ebdd82 | 200 | } |
sgrsn | 0:25bde5ebdd82 | 201 | } |
sgrsn | 0:25bde5ebdd82 | 202 | |
sgrsn | 0:25bde5ebdd82 | 203 | // 周波数を保存する必要があるのでここに実装 |
sgrsn | 0:25bde5ebdd82 | 204 | void CoastAllMotor() |
sgrsn | 0:25bde5ebdd82 | 205 | { |
sgrsn | 0:25bde5ebdd82 | 206 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:25bde5ebdd82 | 207 | { |
sgrsn | 0:25bde5ebdd82 | 208 | // オドメトリ用に周波数を保存 |
sgrsn | 0:25bde5ebdd82 | 209 | wheel_frequency_[i] = mecanum_ -> ControlMotor(i, 0); |
sgrsn | 0:25bde5ebdd82 | 210 | } |
sgrsn | 0:25bde5ebdd82 | 211 | } |
sgrsn | 0:25bde5ebdd82 | 212 | |
sgrsn | 0:25bde5ebdd82 | 213 | void ControlPosition(const Pose2D& ref_pose) |
sgrsn | 0:25bde5ebdd82 | 214 | { |
sgrsn | 0:25bde5ebdd82 | 215 | //Pose2D pose_err = ref_pose - pose_; |
sgrsn | 0:25bde5ebdd82 | 216 | double dt = t_controller_.GetElapsedSeconds(); |
sgrsn | 0:25bde5ebdd82 | 217 | double input_x = controller_[0] -> calculate(ref_pose.x, pose_.x, dt); |
sgrsn | 0:25bde5ebdd82 | 218 | double input_y = controller_[1] -> calculate(ref_pose.y, pose_.y, dt); |
sgrsn | 0:25bde5ebdd82 | 219 | double input_theta = controller_[2] -> calculate(ref_pose.theta, pose_.theta, dt); |
sgrsn | 0:25bde5ebdd82 | 220 | Vel2D vel = Vel2D(input_x, input_y, input_theta); |
sgrsn | 0:25bde5ebdd82 | 221 | ControllVelocity(vel); |
sgrsn | 0:25bde5ebdd82 | 222 | } |
sgrsn | 0:25bde5ebdd82 | 223 | |
sgrsn | 0:25bde5ebdd82 | 224 | bool IsReferencePose(Pose2D ref_pose) |
sgrsn | 0:25bde5ebdd82 | 225 | { |
sgrsn | 0:25bde5ebdd82 | 226 | Pose2D pose_err = ref_pose - pose_; |
sgrsn | 0:25bde5ebdd82 | 227 | bool is_refpose = (pose_err.x < pid_param_[0].threshold.upper) && (pose_err.x > pid_param_[0].threshold.lower); |
sgrsn | 0:25bde5ebdd82 | 228 | is_refpose &= (pose_err.y < pid_param_[1].threshold.upper) && (pose_err.y > pid_param_[1].threshold.lower); |
sgrsn | 0:25bde5ebdd82 | 229 | is_refpose &= (pose_err.theta < pid_param_[2].threshold.upper) && (pose_err.theta > pid_param_[2].threshold.lower); |
sgrsn | 0:25bde5ebdd82 | 230 | return is_refpose; |
sgrsn | 0:25bde5ebdd82 | 231 | } |
sgrsn | 0:25bde5ebdd82 | 232 | |
sgrsn | 0:25bde5ebdd82 | 233 | Pose2D GetPose() |
sgrsn | 0:25bde5ebdd82 | 234 | { |
sgrsn | 0:25bde5ebdd82 | 235 | return pose_; |
sgrsn | 0:25bde5ebdd82 | 236 | } |
sgrsn | 0:25bde5ebdd82 | 237 | |
sgrsn | 0:25bde5ebdd82 | 238 | void UpdateWheel() |
sgrsn | 0:25bde5ebdd82 | 239 | { |
sgrsn | 0:25bde5ebdd82 | 240 | double dt = t_.GetElapsedSeconds(); |
sgrsn | 0:25bde5ebdd82 | 241 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:25bde5ebdd82 | 242 | { |
sgrsn | 0:25bde5ebdd82 | 243 | wheel_rad_[i] = param_.deg_per_pulse * (double)wheel_frequency_[i] * dt * DEG_TO_RAD; |
sgrsn | 0:25bde5ebdd82 | 244 | } |
sgrsn | 0:25bde5ebdd82 | 245 | } |
sgrsn | 0:25bde5ebdd82 | 246 | |
sgrsn | 0:25bde5ebdd82 | 247 | void UpdateTheta() |
sgrsn | 0:25bde5ebdd82 | 248 | { |
sgrsn | 0:25bde5ebdd82 | 249 | pose_.theta = imu_ -> GetYawRadians(); |
sgrsn | 0:25bde5ebdd82 | 250 | } |
sgrsn | 0:25bde5ebdd82 | 251 | |
sgrsn | 0:25bde5ebdd82 | 252 | void ComputePose() |
sgrsn | 0:25bde5ebdd82 | 253 | { |
sgrsn | 0:25bde5ebdd82 | 254 | UpdateWheel(); |
sgrsn | 0:25bde5ebdd82 | 255 | UpdateTheta(); |
sgrsn | 0:25bde5ebdd82 | 256 | double dx = (-wheel_rad_[0] - wheel_rad_[1] + wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius; |
sgrsn | 0:25bde5ebdd82 | 257 | double dy = ( wheel_rad_[0] - wheel_rad_[1] - wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius; |
sgrsn | 2:bc2e5034dda7 | 258 | pose_.x += dx * cos(pose_.theta) + dy * sin(-pose_.theta); |
sgrsn | 2:bc2e5034dda7 | 259 | pose_.y += dy * cos(pose_.theta) + dx * sin(pose_.theta); |
sgrsn | 0:25bde5ebdd82 | 260 | } |
sgrsn | 0:25bde5ebdd82 | 261 | |
sgrsn | 0:25bde5ebdd82 | 262 | private: |
sgrsn | 0:25bde5ebdd82 | 263 | MecanumInterface *mecanum_; |
sgrsn | 0:25bde5ebdd82 | 264 | IMUInterface *imu_; |
sgrsn | 0:25bde5ebdd82 | 265 | MecanumParameter param_; |
sgrsn | 0:25bde5ebdd82 | 266 | |
sgrsn | 0:25bde5ebdd82 | 267 | Pose2D pose_; |
sgrsn | 0:25bde5ebdd82 | 268 | double wheel_rad_[4]; |
sgrsn | 0:25bde5ebdd82 | 269 | int64_t wheel_frequency_[4]; |
sgrsn | 0:25bde5ebdd82 | 270 | ElapsedTimer t_; |
sgrsn | 0:25bde5ebdd82 | 271 | ElapsedTimer t_controller_; |
sgrsn | 0:25bde5ebdd82 | 272 | }; |
sgrsn | 0:25bde5ebdd82 | 273 | |
sgrsn | 0:25bde5ebdd82 | 274 | #endif |