Diff: omni.cpp

Revision:

1:b2063ffa4927

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omni.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -0,0 +1,166 @@
+
+#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)));
+    }
+    
+   
\ No newline at end of file