File content as of revision 5:17ede03f1fa5:

#include "motor_controller.h"
#include "MyOdometer.h"
#include "define.h"
#include "PID.h"

// 目標位置制御用の制御周期
const float pose_tick = 0.010; // sec
const float L = 0.233; //233 [mm]

class MakePath
{
    public:
    int target_x;
    int target_y;
    int target_z;
    
    MakePath()
    {
    }
    int makePath(int targetX, int targetY, int targetZ, int x, int y, int z)
    {
        target_x = targetX;
        target_y = targetY;
        target_z = targetZ;
        int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0;   //5mm間隔
        //printf("num = %d\r\n", num);
        for(int i = 1; i <= num; i++)
        {
            float a = float(i) / num;
            PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD);
            PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD);
            PATH[i-1][2] = z + float(targetZ - z) * a;
        }
        if(num > 0)
        {
            PATH[num-1][0] = targetX;
            PATH[num-1][1] = targetY;
            PATH[num-1][2] = targetZ;
        }
        else
        {
            PATH[0][0] = targetX;
            PATH[0][1] = targetY;
            PATH[0][2] = targetZ;
            num = 1;
        }
        return num;
    }
    
    int getPathX(int i)
    {
        return PATH[i][0];
    }
    int getPathY(int i)
    {
        return PATH[i][1];
    }
    int getPathZ(int i)
    {
        return PATH[i][2];
    }
    
    private:
    int PATH[500][3];
};

Ticker ticker_pose_controll;
MakePath myPath;
int path = 0;
int pathSize;

bool bool_tmp = true;
bool *is_stop_ptr_ = &bool_tmp;

Timer timer2;
PID pid_x(&timer2);
PID pid_y(&timer2);
PID pid_theta(&timer2);

void robot_vel_controll(float vel_x, float vel_y, float vel_theta);
void moveStart(float pose_x, float pose_y, float pose_theta);
void moveStop();
void pose_controll();


void robot_vel_controll(float vel_x, float vel_y, float vel_theta)
{   
    float v[MOTOR_NUM] = {};
    // 各車輪の速度
    v[0] = -vel_x + vel_y - L * vel_theta;  // m/sec
    v[1] = -vel_x - vel_y - L * vel_theta;
    v[2] =  vel_x - vel_y - L * vel_theta;
    v[3] =  vel_x + vel_y - L * vel_theta;
    
    // 本当はこうするべき
    // f = v * ppr / ( 2*PI * r);
    // controlMotor(i, (int)f[i]);
    
    for(int i = 0; i < MOTOR_NUM; i++)
    {
        float f = v[i] / wheel_r * RAD_TO_DEG / deg_per_pulse;
        controlMotor(i, (int)f);
    }
}

void moveStart(float pose_x, float pose_y, float pose_theta, bool* is_stop_pointer)
{
    is_stop_ptr_ = is_stop_pointer;
    *is_stop_ptr_ = false;
    pid_x.setParameter(0.40, 0.0, 0.0);
    pid_y.setParameter(0.40, 0.0, 0.0);
    pid_theta.setParameter(0.10, 0.0, 0.0);
    
    pathSize = myPath.makePath(pose_x*1000,              pose_y*1000,   pose_theta*RAD_TO_DEG, 
                                    my_odometry.x*1000, my_odometry.y*1000, my_odometry.theta*RAD_TO_DEG);
    ticker_pose_controll.attach(&pose_controll, 0.010);
    path = 0;
}

void moveStop()
{
    ticker_pose_controll.detach();
    *is_stop_ptr_ = true;
}

void pose_controll()
{
    float x_robot = my_odometry.x*1000;
    float y_robot = my_odometry.y*1000;
    float theta_robot = my_odometry.theta;
    
    // 目標位置判定
    float err_x = myPath.target_x - x_robot;
    float err_y = myPath.target_y - y_robot;
    float err_theta = myPath.target_z - theta_robot;
    
    // 目標速度計算
    // 慣性座標での速度
    float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
    float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
    float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
    
    if(path < pathSize-1)
        path++;
    else 
        path = pathSize-1;
    
    // ロボット座標での速度
    float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
    float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
    
    robot_vel_controll(x_dot/1000, y_dot/1000, theta_dot);
    
    // 目標位置到達した場合
    if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
    {
        // 車輪を停止
        coastAllMotor();
        //pid_x.reset();
        //pid_y.reset();
        //pid_theta.reset();
        wait_ms(500);
        jy901.reset();
        
        // 制御を停止
        moveStop();
    }
}