Revision 0:870e2b195513, committed 2016-06-10

Comitter:

yuto_oba

Date:

Fri Jun 10 13:30:17 2016 +0000

Commit message:

oba

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Fri Jun 10 13:30:17 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ITG3200/#8967cbe04d96

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NEW_CMG_2016_3_7.cpp	Fri Jun 10 13:30:17 2016 +0000
@@ -0,0 +1,709 @@
+/*外部依存*/
+#include "QEI.h"        //エンコーダ
+#include "math.h"
+#include "ITG3200.h"    //9軸センサ
+
+/*固定値*/
+#define Samp_T 0.05         //サンプリングタイム
+#define Brake_ON 0.0        //電磁ブレーキオン
+#define Brake_OFF 1.0       //電磁ブレーキオフ
+#define PI 3.1415926
+#define Target_RPM 5000.0   //目標ホイール回転数
+#define I 0.0039            //ホイール慣性モーメント
+
+/********************自作関数********************/
+/*サーボ読み取り*/
+void All_Read(uint16_t a, double b[]);                  //(ID, 測定データ)
+/*角度、角速度変化*/
+void Position_Change(uint16_t a, double b);             //(ID, 角度(degree))
+void Position_Change2(uint16_t a, double b, double c);  //(ID, 角度(degree), 回転速度(rpm))
+/*トルクモード変換*/
+void Torque_Control_Mode_Enable_Write(uint16_t a);      //(ID)　使用しない
+void Torque_Control_Mode_Enable_Read(uint16_t a);
+/*目標トルク*/
+void Goal_Torque(uint16_t a, double b);                 //(ID, 目標トルク？)　使用しない
+/*電流読み取り*/
+void Current_Read(uint16_t a, double b[]);              //(ID, 測定データ)
+/********************設定********************/
+/*シリアル通信*/
+Serial pc(p28, p27);        //(Tx, Rx)
+Serial servo(p13,p14);      //(Tx, Rx)
+/*i2c通信*/
+ITG3200 gyr(p9, p10);       //(sda, scl)
+/*エンコーダ*/
+QEI wheel_1 (p29, p30, NC, 300);
+QEI wheel_2 (p25, p26, NC, 300);
+/*モータ*/
+PwmOut motor(p21);
+/*クラッチブレーキ*/
+PwmOut Brake(p22);
+/*タイマー*/
+Timer timer;
+
+/********************諸定義********************/
+int i,j,k;
+int Phase = 0;
+int Case = 0;
+double time2 = 0;
+
+double Radian = 0;
+double Degree = 0;
+
+int Step = 0;
+
+int num = 0;
+
+/*応答遅延時間設定*/
+int Wait_time = 2;
+
+/*ID番号*/
+int ID_1 = 1;
+
+/*エンコーダ*/
+int Enc_p = 600;
+int enc_1 = 0;
+int Enc_1 = 0;
+int enc_2 = 0;
+int Enc_2 = 0;
+double Unit_enc_angle_1 = 0.0;
+double Unit_enc_angle_2 = 0.0;
+double Enc_omega_1 = 0.0;
+double Enc_omega_2 = 0.0;
+double Enc_rpm_1 = 0.0;
+double Enc_rpm_2 = 0.0;
+
+/*モータ入力値*/
+double i_1 = 1000;
+double i_1k = 1000;
+
+/*サーボ測定データ*/
+double Servo_all_data[5];
+double Current[1];
+
+/*サーボ入力*/
+double Theta = 180.0;
+double Theta_rpm = 0.0;
+
+/*角運動量_トルク*/
+double H = 0.0;
+double T = 0.0;
+double T2 = 0.0;
+double T3 = 0.0;
+
+/*センサ*/
+double gyro_offset_z = 10.0;
+double gyro_raw_data = 0.0;
+double delta_gyrodata = 0.0;
+
+/********************外乱オブザーバ********************/
+/*機体特性ノミナル値*/
+double In = 0.5;
+double Csn = 0.5;
+double Hn = 2.0;
+/*フィルター極*/
+double a = 1.0;
+/*フィードバックゲイン*/
+double F[3] = {-0.01909730,0.799230891,0.439083659};
+/*状態変数*/
+double X[3];
+/*入力指令値*/
+double UR = 0.0;
+/*入力値*/
+double u = 0.0;
+double Theta_omega = 0.0;
+
+/*その他*/
+double Rv = 0.0;
+double e = 0.0;
+double Fai = 0.0;
+double Faik = 0.0;
+double Delta_fai = 0.0;
+double Fai_omega = 0.0;
+double Fai_omegak = 0.0;
+double Delta_fai_omega = 0.0;
+double W = 0.0;
+double Wk = 0.0;
+
+
+/********************モータ特性固定用変数********************/
+double iR1 = 0.0, iRa1 = 0.0;
+double Kn = 10, aa = 15, Jn = 1.0, Dn = 1.0;
+double w1 = 0.0, wk1 = 0.0;
+double Rv_RPM = Target_RPM;
+
+/*電磁ブレーキ入力*/
+float brake = 1.0;
+
+/*サーボトルク入力*/
+double Input_Torque = 1200;
+
+/*入力ローパスフィルタ*/
+double Lpf_Theta_omega = 0.0;
+double Lpf_a = 0.35;
+/*########################################メインプログラム########################################*/
+int main()
+{
+    /********************初期設定********************/
+    /*角度変換*/
+    Degree = 180/PI;
+    Radian = PI/180;
+
+    /*通信速度*/
+    pc.baud(115200);
+    servo.baud(200000);
+    servo.format(8,Serial::None,1);
+
+    /*PWM周期設定*/
+    motor.period(0.020);
+    Brake.period(0.0035);
+
+    /*センサ設定*/
+    gyr.setLpBandwidth(LPFBW_5HZ);//Set highest bandwidth.
+
+    /*初期サーボ角度*/
+    /*Position_Change2(1,180,20);
+    Theta_rpm = 20;
+    Theta = 180;*/
+    Position_Change2(1,270,20);
+    Theta_rpm = 20;
+    Theta = 270;
+    
+    /*電磁ブレーキ動作確認*/
+    wait_ms(100);
+    Brake.write(Brake_OFF);
+    wait_ms(1000);
+    Brake.write(Brake_ON);
+    wait_ms(1000);
+
+    /*タイマースタート*/
+    timer.start();
+
+
+
+    /********************ループ処理********************/
+    while(1) {
+        /*50ms毎の処理*/
+        if(timer.read_ms() == 50) {
+
+            timer.reset();              //タイマー初期化
+            time2+=0.05;                //1ループ = 0.05s
+
+            /********************エンコーダ処理********************/
+            /*50ms間のエンコーダパルス測定*/
+            enc_1=wheel_1.getPulses();
+            enc_2=wheel_2.getPulses();
+            Enc_1+=enc_1;
+            Enc_2+=enc_2;
+            wheel_1.reset();
+            wheel_2.reset();
+
+            /*エンコーダ速度・RPM計算*/
+            Unit_enc_angle_1 = 1 * ((double)enc_1 / (double)Enc_p) * (2*PI);
+            Unit_enc_angle_2 = -1 * ((double)enc_2 / (double)Enc_p) * (2*PI);
+            Enc_omega_1 = Unit_enc_angle_1 / Samp_T;
+            Enc_omega_2 = Unit_enc_angle_2 / Samp_T;
+            if(Enc_omega_2 < 0)Enc_omega_2 *= -1;
+            Enc_rpm_1 = Enc_omega_1 * 60.00 / (2*PI);
+            Enc_rpm_2 = Enc_omega_2 * 60.00 / (2*PI);
+
+
+            /********************姿勢角度測定********************/
+            gyro_raw_data = gyr.getGyroZ();
+            delta_gyrodata = gyro_raw_data - gyro_offset_z;
+            if(delta_gyrodata <= 3 && delta_gyrodata >= -3) delta_gyrodata = 0;     //値をカットしているので、他のセンサで補正してください
+            Fai_omega = delta_gyrodata/14.375;
+            if(Fai_omega <= 0.5 && Fai_omega >= -0.5) delta_gyrodata = 0;           //値をカットしているので、他のセンサで補正してください
+            Fai += Fai_omega * Samp_T;
+
+            /*特異点復帰時センサ初期化用*/
+            if(Case == 0 && Phase == 0) {
+                Fai = 0;
+                Fai_omega = 0;
+                delta_gyrodata = 0;
+            }
+
+            /**********************************Case判別1(通常)***********************************/
+            if(Case == 0) {
+
+                /*ホイール加速フェイズ*/
+                if(Phase == 0) {
+                    brake = Brake_ON;
+                    i_1 += 0.5;
+                    i_1k=i_1;
+                    if(Enc_rpm_1>=Rv_RPM-2000)Phase = 1;
+
+                    //if(i_1 >= 1010)Phase = 1;         //ホイール加速フェイズ飛ばし用
+                }
+
+
+                /*制御フェイズ*/
+                else if(Phase == 1) {
+                    /********************ホイール入力計算********************/
+                    /*ホイール1_外乱オブザーバ用計算*/
+                    iR1 = ((Rv_RPM/60)*(2*PI)) / Kn;     //目標rpmを角速度に変更して推力定数ノミナル値で割る？
+                    iRa1 = iR1 - (aa * Jn * Enc_omega_1 + w1) / Kn;
+                    wk1 = w1 - Samp_T * aa * (w1 + aa * Jn * Enc_omega_1 - Dn * Enc_omega_1 + Kn * iRa1);
+                    w1 = wk1;
+
+                    /*モータ入力値、リミット*/
+                    i_1 = iRa1 + i_1k;
+                    if (i_1 >= 2500)i_1 = 2500;
+                    if (i_1 <= i_1k-50)i_1 = i_1k-50;
+
+
+                    if(time2 >= 40.00 && time2 <= 45.00) {        //特異点復帰動作実験用
+                        Case = 1;
+                        Phase = 2;
+                    }
+
+                    /*目標姿勢角度*/
+                    /*if(time2 >= 40 && time2 < 50)Rv = 10;
+                    else if(time2 >= 50)Rv = 0;*/
+
+                    /********************最適サーボ系計算********************/
+                    /*Delta_fai_omega = Fai_omega - Fai_omegak;   //角速度差分
+                    Delta_fai = Fai - Faik;                     //角度差分
+                    Fai_omegak = Fai_omega;
+                    Faik = Fai;
+
+                    e = Rv - Fai;*/                               //角度目標誤差
+
+                    /*状態変数*/
+                    /*X[0] = e * Radian;
+                    X[1] = Delta_fai * Radian;
+                    X[2] = Delta_fai_omega * Radian;*/
+
+                    /*入力指令値*/
+                    /*for(i=0; i<=2; i++)UR += X[i] * F[i];*/
+
+                    /*入力値*/
+                    /*u = UR + (1/(1*Hn)) * (a * In * (Fai_omega * Radian) + W);
+                    Wk = W - Samp_T * a * (W +a * In * (Fai_omega * Radian) - Csn * (Fai_omega * Radian) - 1 * Hn * u);
+                    W = Wk;*/
+
+                    /*サーボ角速度、RPM算出*/
+                    /*Theta_omega = u/cos((Servo_all_data[0]*Radian) - PI);
+                    Lpf_Theta_omega = Lpf_a*Theta_omega + (1-Lpf_a)*Lpf_Theta_omega;
+                    Theta_rpm = (Lpf_Theta_omega*60)/(2*PI);*/
+
+                    /*正値化、リミット、回転方向*/
+                    /*if(Theta_rpm < 0)Theta_rpm *= -1;
+                    if(Theta_rpm >= 30)Theta_rpm = 30;
+                    if(Lpf_Theta_omega < 0)Theta = 90;
+                    else if(Lpf_Theta_omega > 0)Theta = 270;*/
+
+                    /*特異点対策挙動*/                                                 //完全ではありません,サーボ系から復帰動作に移行するまえに
+                    if(Servo_all_data[0] >= 265) {                                 //u,UR,W,Wk等初期化しないと復帰後暴走の可能性あり
+                        Phase = 2;
+                        Case = 1;
+                        u = 0.0;
+                        UR = 0.0;
+                        W = 0.0;
+                        Wk = 0.0;
+                        Input_Torque = 1200;
+                        //brake = 0.82;
+                    }
+
+                    if(Servo_all_data[0] <=95) {
+                        Phase = 3;
+                        Case = 1;
+                        u = 0.0;
+                        UR = 0.0;
+                        W = 0.0;
+                        Wk = 0.0;
+                        Input_Torque = 176;
+                        //brake = 0.82;
+                    }
+                }
+
+                else if(Phase == 2) {
+                }
+            }//Case = 0
+
+            /**********************************Case判別2(復帰)***********************************/
+            //Phase = 0 ホイール回転数減速
+            //Phase = 1 減速後の復帰動作
+            //Phase = 2 減速しない復帰動作
+
+            else if(Case ==1) {
+                /********************ホイール入力計算********************/
+                /*ホイール1_外乱オブザーバ用計算*/
+                iR1 = ((Rv_RPM/60) * (2 * PI))/Kn;//目標rpmを角速度に変更して推力定数ノミナル値で割る？
+                iRa1 = iR1 - (aa * Jn * Enc_omega_1 + w1) / Kn;
+                wk1 = w1 - Samp_T * aa * (w1 + aa * Jn * Enc_omega_1 - Dn * Enc_omega_1 + Kn * iRa1);
+                w1 = wk1;
+
+                /*モータ入力値、リミット*/
+                i_1 = iRa1 + i_1k;
+                if (i_1 >= 2000)i_1 = 2000;
+                if (i_1 <= i_1k-50)i_1 = i_1k-50;
+
+
+                /*減速フェイズ*/
+                if(Phase == 0) {
+                    i_1 = 1000;                     //1000usのニュートラルのときハードブレーキ、ESCでブレーキの強さ設定変更可
+
+                    /*ブレーキ入力*/
+                    brake = Brake_ON;                 //ジンバル2固定（比較実験用）
+                    if(Enc_rpm_2 > 10)brake = 0.55;     //現在適当な数値でON-OFF制御しているのでPIDとかで制御してください
+                    if(Enc_rpm_2 <= 10)brake = 0.68;    //ブレーキ入力はPWMデューティ比入力し、0~1間の値が使用可。Brake.period()の設定によって周期が変動し入力値も変動するので注意
+
+                    if(Enc_rpm_1 <= 100) {
+                        Phase = 1;
+                    }
+                }
+
+                /*ジンバル2半ブレーキ減速*/
+                else if (Phase == 1) {
+                    brake = 0.55;
+                    if (Enc_rpm_2 <= 0.0) {
+                        brake = Brake_ON;
+
+                        Theta = 180;
+                        Theta_rpm = 15;
+
+                        Case = 0;
+                        Phase = 2;
+
+                        /*Rv_RPM = Target_RPM;
+                         iR1 = 0;
+                         iRa1 = 0;
+                         wk1 = 0;
+                         w1 = 0;*/
+                    }
+                }
+
+                /*ジンバル復帰*/
+                else if(Phase == 2) {
+                    /*復帰動作*/
+                    brake = Brake_ON;                 //ジンバル2固定（比較実験用）
+                    if(Enc_rpm_2 > 5)brake = 1;
+                    if(Enc_rpm_2 <= 5)brake = 1;
+
+                    Theta = 180;
+                    Theta_rpm = 1;
+
+                    if (Servo_all_data[0] >= 175 && Servo_all_data[0] <= 185) {
+                        if(Enc_rpm_2 <= 0) {
+                            Case = 0;
+                            Phase = 2;
+                            brake = Brake_ON;
+                        }
+                    }
+
+                }
+            }//Case = 1
+
+
+
+            /********************サーボ入力********************/
+            Position_Change2(ID_1, Theta, Theta_rpm);
+            /********************モータ入力********************/
+            motor.pulsewidth_us(i_1);
+            /********************ブレーキ入力********************/
+            Brake.write(brake);
+            /********************サーボ読み取り********************/
+            All_Read(ID_1, Servo_all_data);
+            Current_Read(ID_1,Current);
+
+            /*ホイール角運動量、トルク計測*/ //トルク関連は曖昧なので注意
+            H = I * Enc_omega_1;
+            T = H * ((Servo_all_data[1]/60) * (2*PI)) * (cos((180 - Servo_all_data[0])*Radian));    //CMGが出力するトルク
+            T2 = H * (Fai_omega*Radian) * (cos((180 - Servo_all_data[0])*Radian));                  //機体回転からサーボへ受けるトルク
+            T3 = H * (Enc_omega_2) * (cos((180 - Servo_all_data[0])*Radian));                       //ジンバル2の回転からサーボへ受けるトルク
+            /********************表示処理********************/
+
+            pc.printf("%10.2f\t%i\t%i\t%i\t%10.0f\t%10.0f\t%10.1f\t%10.1f\t%10.1f\t%10.2f\t%10.2f\t%10.2f\t%10.0f\t%10.0f\t%10.2f\t%10.0f\t%10.2f\t%10.2f\t%10.2f\t%10.2f\t%10.2f\t%10.2f\t%10.2f\r\n",
+                      time2,
+                      timer.read_ms(),
+                      Case,
+                      Phase,
+                      Enc_rpm_1,
+                      Enc_rpm_2,
+                      Servo_all_data[0],//角度
+                      Servo_all_data[1],//RPM
+                      Servo_all_data[2],//負荷
+                      //Servo_all_data[3],//電圧
+                      //Servo_all_data[4],//温度
+                      Current[0],
+                      Fai_omega,
+                      Fai,
+                      Rv,
+                      i_1,
+                      brake,
+                      Input_Torque,
+
+                      u,
+                      Lpf_Theta_omega,
+                      Theta_rpm,
+                      H,
+                      T,
+                      T2,
+                      T3
+                     );
+
+        }//if_time
+    }//while
+}//main
+
+
+/*########################################自作関数設定########################################*/
+void Position_Change(uint16_t a, double b)   //Position_Change(ID,angle)
+{
+    int i;
+    uint16_t Tx[9] = {0xff,0xff,0,0x05,0x03,0x1e,0,0,0};
+    uint16_t Rx[6];
+    uint16_t input;
+
+    input = b / 0.088;               //入力値算出
+    Tx[2] = a;                          //ID
+    /*目標角度 → 下位バイト + 上位バイト*/
+    Tx[6] = input & 0xff;               //論理積より下位バイト出力
+    Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
+    /*Check Sum*/
+    Tx[8] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7])& 0xff ))&0xff;
+    /*データ出入力*/
+    for (i = 0; i <= 8; i++)servo.putc(Tx[i]);
+    wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7],Tx[8]);
+
+    while(servo.readable()) {
+        for(i=0; i<6; i++)Rx[i]=servo.getc();
+    }
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5]);
+}
+
+/***************************************************/
+void Position_Change2(uint16_t a, double b, double c)
+{
+    int i;
+    uint16_t Tx[11] = {0xff,0xff,0,0x07,0x03,0x1e,0,0,0,0,0};
+    uint16_t Rx[6];
+    uint16_t input;
+    uint16_t input_rpm;
+    Tx[2] = a;                          //ID
+
+    /*角度*/
+    input = b / 0.088;               //入力値算出
+
+    /*目標角度 → 下位バイト + 上位バイト*/
+    Tx[6] = input & 0xff;               //論理積より下位バイト出力
+    Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
+
+    /*速度*/
+    input_rpm = c / 0.114;
+    if(input_rpm == 0)input_rpm = 1;
+    //pc.printf("%i\r\n",input_rpm);
+    /*目標速度 → 下位バイト + 上位バイト*/
+    Tx[8] = input_rpm & 0xff;               //論理積より下位バイト出力
+    Tx[9] = input_rpm >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
+    /*Check Sum*/
+    Tx[10] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7]+ Tx[8]+ Tx[9])& 0xff ))&0xff;
+    /*データ出入力*/
+    for (i = 0; i <= 10; i++)servo.putc(Tx[i]);
+    //wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7],Tx[8],Tx[9],Tx[10]);
+
+
+    while(1) {
+        Step++;
+        if(Step == 50000) {
+            pc.printf("While Stop1\r\n");
+            Step = 0;
+            break;
+        }
+        if(servo.readable()==1) {
+            for(i=0; i<6; i++)Rx[i]=servo.getc();
+            Step = 0;
+            break;
+        }
+    }
+
+    /*while(servo.readable()) {
+        for(i=0; i<6; i++)Rx[i]=servo.getc();
+    }*/
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5]);
+}
+
+/******************************************/
+void All_Read(uint16_t a, double b[])
+{
+    int i;
+    uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x24,0x08,0};
+    uint16_t Rx[14];
+    Tx[2] = a;            //ID
+
+    Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
+
+    for(i=0; i<8; i++)servo.putc(Tx[i]);
+    //wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7]);
+    /*while(servo.readable()) {
+            for(i=0; i<14; i++)Rx[i]=servo.getc();
+        }*/
+
+
+
+    /*
+    timer.read_ms() == 50) {
+
+                timer.reset();*/
+    while(1) {
+        Step++;
+        if(Step == 50000) {
+            pc.printf("While Stop2\r\n");
+            Step = 0;
+            timer.reset();
+            break;
+        }
+        if(servo.readable()==1) {
+            for(i=0; i<14; i++)Rx[i]=servo.getc();
+            Step = 0;
+            break;
+        }
+    }
+
+    if(Rx[4] & 0x20 == 0x20)pc.printf("Error\r\n");
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5],Rx[6],Rx[7],Rx[8],Rx[9],Rx[10],Rx[11],Rx[12],Rx[13]);
+
+    b[0] = (double)((Rx[6]<<8)| Rx[5])*0.088;
+    b[1] = (double)(((Rx[8]<<8)| Rx[7]) & 0x7ff);
+    if(b[1] >= 1024)b[1] = b[1] - 1024;
+    b[1] = b[1] * 0.114;
+    b[2] = (double)(((Rx[10]<<8)| Rx[9]) & 0x7ff);
+    if(b[2] >= 1024)b[2] = b[2] - 1024;
+    b[2] = b[2] * 0.1;
+    b[3] = (double)Rx[11]*0.1;
+    b[4] = (double)Rx[12];
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%10.2f\t%10.2f\t%10.2f\t%10.2f\t%10.2f\r\n",time2,timer.read_ms(),b[0],b[1],b[2],b[3],b[4]);
+
+}
+/**********************************************/
+void Torque_Control_Mode_Enable_Read(uint16_t a)
+{
+    int i;
+    uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x46,0x01,0};
+    uint16_t Rx[7];
+    Tx[2] = a;            //ID
+
+    Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
+    for(i=0; i<8; i++)servo.putc(Tx[i]);
+    wait_ms(Wait_time);
+
+    //デバック出力
+    pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7]);
+
+    while(servo.readable()) {
+        for(i=0; i<7; i++)Rx[i]=servo.getc();
+    }
+
+    //デバック出力
+    pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5],Rx[6]);
+}
+/***********************************************/
+void Torque_Control_Mode_Enable_Write(uint16_t a)
+{
+    int i;
+    uint16_t Tx[8] = {0xff,0xff,0xFe,0x4,0x03,0x46,0,0};
+    //uint16_t Rx[7];
+    Tx[6] = a;            //ID
+
+    Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
+    for(i=0; i<8; i++)servo.putc(Tx[i]);
+    wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7]);
+
+    /*while(servo.readable()) {
+        for(i=0; i<7; i++)Rx[i]=servo.getc();
+    }*/
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5],Rx[6]);
+}
+/************************************/
+void Goal_Torque(uint16_t a, double b)
+{
+    int i;
+    uint16_t Tx[9] = {0xff,0xff,0,0x05,0x03,0x47,0,0,0};
+    uint16_t Rx[6];
+    uint16_t input;
+
+    input = b;               //入力値算出
+    Tx[2] = a;                          //ID
+    /*目標角度 → 下位バイト + 上位バイト*/
+    Tx[6] = input & 0xff;               //論理積より下位バイト出力
+    Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
+    /*Check Sum*/
+    Tx[8] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7])& 0xff ))&0xff;
+    /*データ出入力*/
+    for (i = 0; i <= 8; i++)servo.putc(Tx[i]);
+    wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7],Tx[8]);
+
+    while(servo.readable()) {
+        for(i=0; i<6; i++)Rx[i]=servo.getc();
+    }
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5]);
+}
+/***************************************/
+void Current_Read(uint16_t a, double b[])
+{
+    int i;
+    //double b;
+    uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x44,0x02,0};
+    uint16_t Rx[8];
+    Tx[2] = a;            //ID
+
+    Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
+    for(i=0; i<8; i++)servo.putc(Tx[i]);
+    //wait_ms(Wait_time);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Tx[0],Tx[1],Tx[2],Tx[3],Tx[4],Tx[5],Tx[6],Tx[7]);
+
+    /*while(servo.readable()) {
+        for(i=0; i<8; i++)Rx[i]=servo.getc();
+    }*/
+
+    while(1) {
+        Step++;
+        if(Step == 50000) {
+            pc.printf("While Stop3\r\n");
+            Step = 0;
+            break;
+        }
+        if(servo.readable()==1) {
+            for(i=0; i<8; i++)Rx[i]=servo.getc();
+            Step = 0;
+            break;
+        }
+    }
+
+
+    b[0] = 0.0045 * ((double)((Rx[6]<<8)| Rx[5]) - 2048);
+    //pc.printf("%10.4f\r\n",b);
+
+    //デバック出力
+    //pc.printf("%10.2f\t%i\t%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x\r\n",time2,timer.read_ms(),Rx[0],Rx[1],Rx[2],Rx[3],Rx[4],Rx[5],Rx[6],Rx[7]);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Jun 10 13:30:17 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 10 13:30:17 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0
\ No newline at end of file