High level mecanum robot controller library
Dependents: WRS2021_mecanum_driver
mecanum_controller.hpp
- Committer:
- sgrsn
- Date:
- 2021-08-25
- Revision:
- 2:bc2e5034dda7
- Parent:
- 1:26c16fb42252
File content as of revision 2:bc2e5034dda7:
#ifndef MECANUM_CONTROLLER_HPP #define MECANUM_CONTROLLER_HPP #include "mecanum_interface.hpp" #include "imu_interface.hpp" #include "pid_controller.hpp" #include "math.h" #include <vector> //const double PI = 3.14159265359; const float DEG_TO_RAD = PI/180.0; const float RAD_TO_DEG = 180.0/PI; using namespace std::chrono; class Pose2D { public: Pose2D(double x=0, double y=0, double theta=0) : x(x), y(y), theta(theta) { } Pose2D(double x, double y, double theta, double time) : x(x), y(y), theta(theta), time_stamp(time) { } Pose2D operator +(Pose2D ref) { return Pose2D(x + ref.x, y + ref.y, theta + ref.theta); } Pose2D operator -(Pose2D ref) { return Pose2D(x - ref.x, y - ref.y, theta - ref.theta); } Pose2D operator *(Pose2D ref) { return Pose2D(x * ref.x, y * ref.y, theta * ref.theta); } Pose2D operator *(double val) { return Pose2D(x * val, y * val, theta * val); } /*Pose2D operator /(Pose2D ref) { return Pose2D(x / ref.x, y / ref.y, theta / ref.theta); }*/ double operator /(Pose2D ref) { return std::max( std::max( abs(x / ref.x), abs(y / ref.y) ), abs(theta / ref.theta) ); } Pose2D operator /(double val) { return Pose2D(x / val, y / val, theta / val); } void ConstraintUpper(Pose2D ref, Pose2D dir) { if(dir.x > 0 && x > ref.x) x = ref.x; if(dir.y > 0 && y > ref.y) y = ref.y; if(dir.theta > 0 && theta > ref.theta) theta = ref.theta; if(dir.x < 0 && x < ref.x) x = ref.x; if(dir.y < 0 && y < ref.y) y = ref.y; if(dir.theta < 0 && theta < ref.theta) theta = ref.theta; } void ConstraintLower(Pose2D ref, Pose2D dir) { if(dir.x > 0 && x < ref.x) x = ref.x; if(dir.y > 0 && y < ref.y) y = ref.y; if(dir.theta > 0 && theta < ref.theta) theta = ref.theta; if(dir.x < 0 && x > ref.x) x = ref.x; if(dir.y < 0 && y > ref.y) y = ref.y; if(dir.theta < 0 && theta > ref.theta) theta = ref.theta; } double x = 0; double y = 0; double theta = 0; double time_stamp = 0; }; class Vel2D { public: Vel2D(double x, double y, double theta) : x(x), y(y), theta(theta) { } double x = 0; double y = 0; double theta = 0; }; struct MecanumParameter { const double L = 0.233; //233 [mm] const double wheel_radius = 0.0635; // メカナム半径[m] const double deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) }; class ElapsedTimer { public: ElapsedTimer() { timer_.start(); last_us_ = chrono::duration_cast<chrono::microseconds>(timer_.elapsed_time()).count(); } double GetElapsedSeconds() { uint64_t current_us = chrono::duration_cast<chrono::microseconds>(timer_.elapsed_time()).count(); double dt = (double)(current_us - last_us_) / 1000000; last_us_ = current_us; return dt; } private: Timer timer_; uint64_t last_us_ = 0; }; class PoseController { public: PoseController() { /*id_name_.emplace_back("pose_x"); id_name_.emplace_back("pose_y"); id_name_.emplace_back("pose_theta"); for(int i=0; i<id_name_.size(); i++) { id_map_[id_name_[i]] = i; }*/ //pid_param_.resize(id_name_.size()); //controller_.resize(id_name_.size(), 0); pid_param_.resize(3); controller_.resize(3); // for pose x pid_param_[0].bound.upper = 1.0; pid_param_[0].bound.lower = -1.0; pid_param_[0].threshold.upper = 0.001; pid_param_[0].threshold.lower = -0.001; pid_param_[0].kp = 0.5; pid_param_[0].ki = 0.03; pid_param_[0].kd = 0.0; // for pose y pid_param_[1].bound.upper = 1.0; pid_param_[1].bound.lower = -1.0; pid_param_[1].threshold.upper = 0.001; pid_param_[1].threshold.lower = -0.001; pid_param_[1].kp = 0.5; pid_param_[1].ki = 0.03; pid_param_[1].kd = 0.0; // for pose theta pid_param_[2].bound.upper = 1.0; pid_param_[2].bound.lower = -1.0; pid_param_[2].threshold.upper = 0.005; pid_param_[2].threshold.lower = -0.005; pid_param_[2].kp = 1.50; pid_param_[2].ki = 0.2; pid_param_[2].kd = 0.05; controller_[0] = std::make_unique<PIDControl>(pid_param_[0]); controller_[1] = std::make_unique<PIDControl>(pid_param_[1]); controller_[2] = std::make_unique<PIDControl>(pid_param_[2]); //controller_[0] -> parameter(pid_param_[0]); //controller_[1] -> parameter(pid_param_[1]); //controller_[2] -> parameter(pid_param_[2]); } protected: std::vector<PIDControl::Parameter> pid_param_; //[x, y, theta] //std::map<std::string, uint16_t> id_map_; //std::vector<std::string> id_name_; std::vector<std::unique_ptr<PIDControl>> controller_; }; class MecanumController : PoseController { public: MecanumController(MecanumInterface *mecanum, IMUInterface *imu) : pose_(0, 0, 0), PoseController() { mecanum_ = mecanum; imu_ = imu; } void ControllVelocity(Vel2D vel) { double v[MOTOR_NUM] = {}; // 各車輪の速度 v[0] = -vel.x + vel.y - param_.L * vel.theta; // m/sec v[1] = -vel.x - vel.y - param_.L * vel.theta; v[2] = vel.x - vel.y - param_.L * vel.theta; v[3] = vel.x + vel.y - param_.L * vel.theta; // 各モータのパルスに変換 for(int i = 0; i < MOTOR_NUM; i++) { float f = v[i] / param_.wheel_radius * RAD_TO_DEG / param_.deg_per_pulse; wheel_frequency_[i] = mecanum_ -> ControlMotor(i, (int)f); } } // 周波数を保存する必要があるのでここに実装 void CoastAllMotor() { for(int i = 0; i < MOTOR_NUM; i++) { // オドメトリ用に周波数を保存 wheel_frequency_[i] = mecanum_ -> ControlMotor(i, 0); } } void ControlPosition(const Pose2D& ref_pose) { //Pose2D pose_err = ref_pose - pose_; double dt = t_controller_.GetElapsedSeconds(); double input_x = controller_[0] -> calculate(ref_pose.x, pose_.x, dt); double input_y = controller_[1] -> calculate(ref_pose.y, pose_.y, dt); double input_theta = controller_[2] -> calculate(ref_pose.theta, pose_.theta, dt); Vel2D vel = Vel2D(input_x, input_y, input_theta); ControllVelocity(vel); } bool IsReferencePose(Pose2D ref_pose) { Pose2D pose_err = ref_pose - pose_; bool is_refpose = (pose_err.x < pid_param_[0].threshold.upper) && (pose_err.x > pid_param_[0].threshold.lower); is_refpose &= (pose_err.y < pid_param_[1].threshold.upper) && (pose_err.y > pid_param_[1].threshold.lower); is_refpose &= (pose_err.theta < pid_param_[2].threshold.upper) && (pose_err.theta > pid_param_[2].threshold.lower); return is_refpose; } Pose2D GetPose() { return pose_; } void UpdateWheel() { double dt = t_.GetElapsedSeconds(); for(int i = 0; i < MOTOR_NUM; i++) { wheel_rad_[i] = param_.deg_per_pulse * (double)wheel_frequency_[i] * dt * DEG_TO_RAD; } } void UpdateTheta() { pose_.theta = imu_ -> GetYawRadians(); } void ComputePose() { UpdateWheel(); UpdateTheta(); double dx = (-wheel_rad_[0] - wheel_rad_[1] + wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius; double dy = ( wheel_rad_[0] - wheel_rad_[1] - wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius; pose_.x += dx * cos(pose_.theta) + dy * sin(-pose_.theta); pose_.y += dy * cos(pose_.theta) + dx * sin(pose_.theta); } private: MecanumInterface *mecanum_; IMUInterface *imu_; MecanumParameter param_; Pose2D pose_; double wheel_rad_[4]; int64_t wheel_frequency_[4]; ElapsedTimer t_; ElapsedTimer t_controller_; }; #endif