hidaka sato / MecanumController

High level mecanum robot controller library

Dependents:   WRS2021_mecanum_driver

mecanum_controller.hpp@1:26c16fb42252, 2021-08-25 (annotated)

Committer:
sgrsn
Date:
Wed Aug 25 08:52:40 2021 +0000
Revision:
1:26c16fb42252
Parent:
0:25bde5ebdd82
Child:
2:bc2e5034dda7
Update

Who changed what in which revision?

UserRevisionLine numberNew 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 0:25bde5ebdd82 258 pose_.x += dx;
sgrsn 0:25bde5ebdd82 259 pose_.y += dy;
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