File content as of revision 0:25bde5ebdd82:

#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