High level mecanum robot controller library
Dependents: WRS2021_mecanum_driver
Diff: mecanum_controller.hpp
- Revision:
- 0:25bde5ebdd82
- Child:
- 1:26c16fb42252
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mecanum_controller.hpp Mon Aug 23 17:01:26 2021 +0000 @@ -0,0 +1,274 @@ +#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, double y, double theta) : 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; + pose_.y += dy; + } + + private: + MecanumInterface *mecanum_; + IMUInterface *imu_; + MecanumParameter param_; + + Pose2D pose_; + double wheel_rad_[4]; + int64_t wheel_frequency_[4]; + ElapsedTimer t_; + ElapsedTimer t_controller_; +}; + +#endif \ No newline at end of file