File content as of revision 1:b2063ffa4927:

#include "mbed.h"
#include "RoboClaw.h"
#include "QEI.h"

//位置PID
#define Kp 100
#define Ki 7
#define Kd 0

//回転角度PID
#define a_Kp 100
#define a_Ki 7
#define a_Kd 0

//オドメトリ用輪(mm
#define omni1 40.9401
#define omni2 41.0987
#define omni3 40.6526

#define radius 101.6 //駆動輪直径(mm
#define gaisetuen 121.24355652982 //オドメトリ用車輪の外接円
#define Pi 3.141592 //円周率
#define dt 0.001 //動作周期時間(s)

#define resolution 200 //オドメトリエンコーダーの分解能
#define en_bai 2 //エンコーダーの倍する変数

//縦：X 横:Y

Ticker tick_odo; //オドメトリ関数を時間割り込みさせる宣言

//オドメトリ用エンコーダープルアップ抵抗有効化
DigitalIn Enc1_A(p26);
DigitalIn Enc1_B(p25);
DigitalIn Enc2_A(p24);
DigitalIn Enc2_B(p23);
DigitalIn Enc3_A(p22);
DigitalIn Enc3_B(p21);
QEI odo1(p26, p25,NC,resolution);
QEI odo2(p24, p23,NC,resolution);
QEI odo3(p22, p21,NC,resolution);

RoboClaw roboclaw1(0x80,230400, p13, p14);
RoboClaw roboclaw2(0x81,115200, p9, p10);

//グローバル変数宣言
double x_global=0.0,y_global=0.0,angle=0.0,t=0.0,vertical_degree=0.0;

//サブ関数宣言
double d_r(double degree); //度数法→ラジアン
double r_d(double radian); //ラジアン→度数法
double max_min(double val,double max,double min); //最大最小値設定関数.math.hでいうmax関数とmin関数を統合した関数
double odo_way(int mode); //オドメトリ用エンコーダーからとったpulseをmmに変換
double mms_qpps(double millimeter_sec); //mmsからqpps(秒あたりのパルス数)に変換する関数
void omni_move(double x_speed,double y_speed,double angle); 
void odometry(); //オドメトリ関数

//動作コード
void setup_omni(){
    //オドメトリ用エンコーダープルアップ抵抗有効化
    Enc1_A.mode(PullUp);
    Enc1_B.mode(PullUp);
    Enc2_A.mode(PullUp);
    Enc2_B.mode(PullUp);
    Enc3_A.mode(PullUp);
    Enc3_B.mode(PullUp);
    
    //オドメトリ関数の時間割り込み有効化
    tick_odo.attach(&odometry,dt);
    
    //モーター初期化
    omni_move(0.0,0.0,0.0);
}


/*
int main() {
    setup();
    wait(4.0);
     while(1){
        omni_move(100.0,100.0,0.0);
        pc.printf("\nx_global:%lf\ty_global:%lf\tangle:%lf\t\n",x_global,y_global,angle);
        wait(dt);
     }
}
*/

//－－－－－－－以下サブ関数－－－－－－－//
double d_r(double degree){//度数→ラジアン
    return degree*Pi/180.0;
    }

double r_d(double radian){//ラジアン→度数
    return radian*180.0/Pi;
    }

double max_min(double val,double max,double min){//限度設定関数
    if(val>max)return max;
    else if(val<min)return min;
    else return val;
    }

double mms_qpps(double millimeter_sec){//mm/sにqppsを変換
    return millimeter_sec*((2024.0*4.0)/(radius*Pi));
    }

double odo_way(int mode){//pulseからmmに変換
    if(mode==1) return ((double)odo1.getPulses()*omni1*Pi)/(resolution*en_bai);
    else if(mode==2) return ((double)odo2.getPulses()*omni2*Pi)/(resolution*en_bai);
    else if(mode==3) return ((double)odo3.getPulses()*omni3*Pi)/(resolution*en_bai);
    else return 0;
    }
    
void odometry(){ //単位時間あたりの移動量を積分して自己位置を推定する,神が書いた領域である関数
    static double radian,pre_odo_way1=0.0,pre_odo_way2=0.0,pre_odo_way3=0.0;
    static double xg_dot, yg_dot, prev_xg_dot = 0, prev_yg_dot = 0, prev_theta_dot = 0;
    double x_dot,y_dot,v1,v2,v3,theta_dot;
    double dw[4];
    
    dw[1] = odo_way(1);
    dw[2] = odo_way(2);
    dw[3] = odo_way(3);
    
    //三角関数式
    radian=(dw[1]+dw[2]+dw[3])/(3*gaisetuen);
    //way_y=(dw[1]*cos(radian) + dw[2]*cos(radian+d_r(120.0)) + dw[3]*cos(radian+d_r(240.0)))/2.0;
    //way_x=(dw[1]*sin(radian) + dw[2]*sin(radian+d_r(120.0)) + dw[3]*sin(radian+d_r(240.0)))/2.0;   
    
    //積分式
    v1 = (dw[1] - pre_odo_way1)/dt;
    v2 = (dw[2] - pre_odo_way2)/dt;
    v3 = (dw[3] - pre_odo_way3)/dt;
    
    x_dot=(v2-v3)/1.7320508;
    y_dot=(-v1 + 2.0*(v2 + v3))/3.0;
    theta_dot=(v1+v2+v3)/(3.0*gaisetuen/2.0);
    
    xg_dot=x_dot*cos(radian) - y_dot*sin(radian);
    yg_dot=x_dot*sin(radian) + y_dot*cos(radian) - theta_dot/(gaisetuen/2.0);
    
    y_global += ((xg_dot + prev_xg_dot)/2.0)*dt;
    x_global -= ((yg_dot + prev_yg_dot)/2.0)*dt;
    radian += ((theta_dot + prev_theta_dot)/2.0)*dt;
    
    //y_global=(-1.0) * y_global;//値が常に0になるエラー発生
    
    prev_xg_dot = xg_dot;
    prev_yg_dot = yg_dot;
    prev_theta_dot = theta_dot;
    pre_odo_way1=dw[1];
    pre_odo_way2=dw[2];
    pre_odo_way3=dw[3];
    angle=(-1.0)*r_d(radian);
}

void omni_move(double x_speed,double y_speed,double angle){
    x_speed=mms_qpps(x_speed);
    y_speed=mms_qpps(y_speed);
    angle=mms_qpps(angle);
    roboclaw1.SpeedM2((int32_t)(x_speed*sin(d_r(90.0)) + y_speed*cos(d_r(90.0)) + angle));
    roboclaw2.SpeedM1((int32_t)(x_speed*sin(d_r(330.0)) + y_speed*cos(d_r(330.0)) + angle));
    roboclaw1.SpeedM1((int32_t)(-1.0*(x_speed*sin(d_r(210.0)) + y_speed*cos(d_r(210.0)) + angle)));
    }