yuto

Dependencies:   ITG3200 QEI mbed

Dependents:   NEW_CMG_2016_3_7

Fork of NEW_CMG_2016_3_7 by yuto oba

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers NEW_CMG_2016_3_7.cpp Source File

NEW_CMG_2016_3_7.cpp

00001 /*外部依存*/
00002 #include "QEI.h"        //エンコーダ
00003 #include "math.h"
00004 #include "ITG3200.h"    //9軸センサ
00005 
00006 /*固定値*/
00007 #define Samp_T 0.05         //サンプリングタイム
00008 #define Brake_ON 0.0        //電磁ブレーキオン
00009 #define Brake_OFF 1.0       //電磁ブレーキオフ
00010 #define PI 3.1415926
00011 #define Target_RPM 5000.0   //目標ホイール回転数
00012 #define I 0.0039            //ホイール慣性モーメント
00013 
00014 /********************自作関数********************/
00015 /*サーボ読み取り*/
00016 void All_Read(uint16_t a, double b[]);                  //(ID, 測定データ)
00017 /*角度、角速度変化*/
00018 void Position_Change(uint16_t a, double b);             //(ID, 角度(degree))
00019 void Position_Change2(uint16_t a, double b, double c);  //(ID, 角度(degree), 回転速度(rpm))
00020 /*トルクモード変換*/
00021 void Torque_Control_Mode_Enable_Write(uint16_t a);      //(ID) 使用しない
00022 void Torque_Control_Mode_Enable_Read(uint16_t a);
00023 /*目標トルク*/
00024 void Goal_Torque(uint16_t a, double b);                 //(ID, 目標トルク?) 使用しない
00025 /*電流読み取り*/
00026 void Current_Read(uint16_t a, double b[]);              //(ID, 測定データ)
00027 /********************設定********************/
00028 /*シリアル通信*/
00029 Serial pc(p28, p27);        //(Tx, Rx)
00030 Serial servo(p13,p14);      //(Tx, Rx)
00031 /*i2c通信*/
00032 ITG3200 gyr(p9, p10);       //(sda, scl)
00033 /*エンコーダ*/
00034 QEI wheel_1 (p29, p30, NC, 300);
00035 QEI wheel_2 (p25, p26, NC, 300);
00036 /*モータ*/
00037 PwmOut motor(p21);
00038 /*クラッチブレーキ*/
00039 PwmOut Brake(p22);
00040 /*タイマー*/
00041 Timer timer;
00042 
00043 /********************諸定義********************/
00044 int i,j,k;
00045 int Phase = 0;
00046 int Case = 0;
00047 double time2 = 0;
00048 
00049 double Radian = 0;
00050 double Degree = 0;
00051 
00052 int Step = 0;
00053 
00054 int num = 0;
00055 
00056 /*応答遅延時間設定*/
00057 int Wait_time = 2;
00058 
00059 /*ID番号*/
00060 int ID_1 = 1;
00061 
00062 /*エンコーダ*/
00063 int Enc_p = 600;
00064 int enc_1 = 0;
00065 int Enc_1 = 0;
00066 int enc_2 = 0;
00067 int Enc_2 = 0;
00068 double Unit_enc_angle_1 = 0.0;
00069 double Unit_enc_angle_2 = 0.0;
00070 double Enc_omega_1 = 0.0;
00071 double Enc_omega_2 = 0.0;
00072 double Enc_rpm_1 = 0.0;
00073 double Enc_rpm_2 = 0.0;
00074 
00075 /*モータ入力値*/
00076 double i_1 = 1000;
00077 double i_1k = 1000;
00078 
00079 /*サーボ測定データ*/
00080 double Servo_all_data[5];
00081 double Current[1];
00082 
00083 /*サーボ入力*/
00084 double Theta = 180.0;
00085 double Theta_rpm = 0.0;
00086 
00087 /*角運動量_トルク*/
00088 double H = 0.0;
00089 double T = 0.0;
00090 double T2 = 0.0;
00091 double T3 = 0.0;
00092 
00093 /*センサ*/
00094 double gyro_offset_z = 10.0;
00095 double gyro_raw_data = 0.0;
00096 double delta_gyrodata = 0.0;
00097 
00098 /********************外乱オブザーバ********************/
00099 /*機体特性ノミナル値*/
00100 double In = 0.5;
00101 double Csn = 0.5;
00102 double Hn = 2.0;
00103 /*フィルター極*/
00104 double a = 1.0;
00105 /*フィードバックゲイン*/
00106 double F[3] = {-0.01909730,0.799230891,0.439083659};
00107 /*状態変数*/
00108 double X[3];
00109 /*入力指令値*/
00110 double UR = 0.0;
00111 /*入力値*/
00112 double u = 0.0;
00113 double Theta_omega = 0.0;
00114 
00115 /*その他*/
00116 double Rv = 0.0;
00117 double e = 0.0;
00118 double Fai = 0.0;
00119 double Faik = 0.0;
00120 double Delta_fai = 0.0;
00121 double Fai_omega = 0.0;
00122 double Fai_omegak = 0.0;
00123 double Delta_fai_omega = 0.0;
00124 double W = 0.0;
00125 double Wk = 0.0;
00126 
00127 
00128 /********************モータ特性固定用変数********************/
00129 double iR1 = 0.0, iRa1 = 0.0;
00130 double Kn = 10, aa = 15, Jn = 1.0, Dn = 1.0;
00131 double w1 = 0.0, wk1 = 0.0;
00132 double Rv_RPM = Target_RPM;
00133 
00134 /*電磁ブレーキ入力*/
00135 float brake = 1.0;
00136 
00137 /*サーボトルク入力*/
00138 double Input_Torque = 1200;
00139 
00140 /*入力ローパスフィルタ*/
00141 double Lpf_Theta_omega = 0.0;
00142 double Lpf_a = 0.35;
00143 /*########################################メインプログラム########################################*/
00144 int main()
00145 {
00146     /********************初期設定********************/
00147     /*角度変換*/
00148     Degree = 180/PI;
00149     Radian = PI/180;
00150 
00151     /*通信速度*/
00152     pc.baud(115200);
00153     servo.baud(200000);
00154     servo.format(8,Serial::None,1);
00155 
00156     /*PWM周期設定*/
00157     motor.period(0.020);
00158     Brake.period(0.0035);
00159 
00160     /*センサ設定*/
00161     gyr.setLpBandwidth(LPFBW_5HZ);//Set highest bandwidth.
00162 
00163     /*初期サーボ角度*/
00164     /*Position_Change2(1,180,20);
00165     Theta_rpm = 20;
00166     Theta = 180;*/
00167     Position_Change2(1,270,20);
00168     Theta_rpm = 20;
00169     Theta = 270;
00170     
00171     /*電磁ブレーキ動作確認*/
00172     wait_ms(100);
00173     Brake.write(Brake_OFF);
00174     wait_ms(1000);
00175     Brake.write(Brake_ON);
00176     wait_ms(1000);
00177 
00178     /*タイマースタート*/
00179     timer.start();
00180 
00181 
00182 
00183     /********************ループ処理********************/
00184     while(1) {
00185         /*50ms毎の処理*/
00186         if(timer.read_ms() == 50) {
00187 
00188             timer.reset();              //タイマー初期化
00189             time2+=0.05;                //1ループ = 0.05s
00190 
00191             /********************エンコーダ処理********************/
00192             /*50ms間のエンコーダパルス測定*/
00193             enc_1=wheel_1.getPulses();
00194             enc_2=wheel_2.getPulses();
00195             Enc_1+=enc_1;
00196             Enc_2+=enc_2;
00197             wheel_1.reset();
00198             wheel_2.reset();
00199 
00200             /*エンコーダ速度・RPM計算*/
00201             Unit_enc_angle_1 = 1 * ((double)enc_1 / (double)Enc_p) * (2*PI);
00202             Unit_enc_angle_2 = -1 * ((double)enc_2 / (double)Enc_p) * (2*PI);
00203             Enc_omega_1 = Unit_enc_angle_1 / Samp_T;
00204             Enc_omega_2 = Unit_enc_angle_2 / Samp_T;
00205             if(Enc_omega_2 < 0)Enc_omega_2 *= -1;
00206             Enc_rpm_1 = Enc_omega_1 * 60.00 / (2*PI);
00207             Enc_rpm_2 = Enc_omega_2 * 60.00 / (2*PI);
00208 
00209 
00210             /********************姿勢角度測定********************/
00211             gyro_raw_data = gyr.getGyroZ();
00212             delta_gyrodata = gyro_raw_data - gyro_offset_z;
00213             if(delta_gyrodata <= 3 && delta_gyrodata >= -3) delta_gyrodata = 0;     //値をカットしているので、他のセンサで補正してください
00214             Fai_omega = delta_gyrodata/14.375;
00215             if(Fai_omega <= 0.5 && Fai_omega >= -0.5) delta_gyrodata = 0;           //値をカットしているので、他のセンサで補正してください
00216             Fai += Fai_omega * Samp_T;
00217 
00218             /*特異点復帰時センサ初期化用*/
00219             if(Case == 0 && Phase == 0) {
00220                 Fai = 0;
00221                 Fai_omega = 0;
00222                 delta_gyrodata = 0;
00223             }
00224 
00225             /**********************************Case判別1(通常)***********************************/
00226             if(Case == 0) {
00227 
00228                 /*ホイール加速フェイズ*/
00229                 if(Phase == 0) {
00230                     brake = Brake_ON;
00231                     i_1 += 0.5;
00232                     i_1k=i_1;
00233                     if(Enc_rpm_1>=Rv_RPM-2000)Phase = 1;
00234 
00235                     //if(i_1 >= 1010)Phase = 1;         //ホイール加速フェイズ飛ばし用
00236                 }
00237 
00238 
00239                 /*制御フェイズ*/
00240                 else if(Phase == 1) {
00241                     /********************ホイール入力計算********************/
00242                     /*ホイール1_外乱オブザーバ用計算*/
00243                     iR1 = ((Rv_RPM/60)*(2*PI)) / Kn;     //目標rpmを角速度に変更して推力定数ノミナル値で割る?
00244                     iRa1 = iR1 - (aa * Jn * Enc_omega_1 + w1) / Kn;
00245                     wk1 = w1 - Samp_T * aa * (w1 + aa * Jn * Enc_omega_1 - Dn * Enc_omega_1 + Kn * iRa1);
00246                     w1 = wk1;
00247 
00248                     /*モータ入力値、リミット*/
00249                     i_1 = iRa1 + i_1k;
00250                     if (i_1 >= 2500)i_1 = 2500;
00251                     if (i_1 <= i_1k-50)i_1 = i_1k-50;
00252 
00253 
00254                     if(time2 >= 40.00 && time2 <= 45.00) {        //特異点復帰動作実験用
00255                         Case = 1;
00256                         Phase = 2;
00257                     }
00258 
00259                     /*目標姿勢角度*/
00260                     /*if(time2 >= 40 && time2 < 50)Rv = 10;
00261                     else if(time2 >= 50)Rv = 0;*/
00262 
00263                     /********************最適サーボ系計算********************/
00264                     /*Delta_fai_omega = Fai_omega - Fai_omegak;   //角速度差分
00265                     Delta_fai = Fai - Faik;                     //角度差分
00266                     Fai_omegak = Fai_omega;
00267                     Faik = Fai;
00268 
00269                     e = Rv - Fai;*/                               //角度目標誤差
00270 
00271                     /*状態変数*/
00272                     /*X[0] = e * Radian;
00273                     X[1] = Delta_fai * Radian;
00274                     X[2] = Delta_fai_omega * Radian;*/
00275 
00276                     /*入力指令値*/
00277                     /*for(i=0; i<=2; i++)UR += X[i] * F[i];*/
00278 
00279                     /*入力値*/
00280                     /*u = UR + (1/(1*Hn)) * (a * In * (Fai_omega * Radian) + W);
00281                     Wk = W - Samp_T * a * (W +a * In * (Fai_omega * Radian) - Csn * (Fai_omega * Radian) - 1 * Hn * u);
00282                     W = Wk;*/
00283 
00284                     /*サーボ角速度、RPM算出*/
00285                     /*Theta_omega = u/cos((Servo_all_data[0]*Radian) - PI);
00286                     Lpf_Theta_omega = Lpf_a*Theta_omega + (1-Lpf_a)*Lpf_Theta_omega;
00287                     Theta_rpm = (Lpf_Theta_omega*60)/(2*PI);*/
00288 
00289                     /*正値化、リミット、回転方向*/
00290                     /*if(Theta_rpm < 0)Theta_rpm *= -1;
00291                     if(Theta_rpm >= 30)Theta_rpm = 30;
00292                     if(Lpf_Theta_omega < 0)Theta = 90;
00293                     else if(Lpf_Theta_omega > 0)Theta = 270;*/
00294 
00295                     /*特異点対策挙動*/                                                 //完全ではありません,サーボ系から復帰動作に移行するまえに
00296                     if(Servo_all_data[0] >= 265) {                                 //u,UR,W,Wk等初期化しないと復帰後暴走の可能性あり
00297                         Phase = 2;
00298                         Case = 1;
00299                         u = 0.0;
00300                         UR = 0.0;
00301                         W = 0.0;
00302                         Wk = 0.0;
00303                         Input_Torque = 1200;
00304                         //brake = 0.82;
00305                     }
00306 
00307                     if(Servo_all_data[0] <=95) {
00308                         Phase = 3;
00309                         Case = 1;
00310                         u = 0.0;
00311                         UR = 0.0;
00312                         W = 0.0;
00313                         Wk = 0.0;
00314                         Input_Torque = 176;
00315                         //brake = 0.82;
00316                     }
00317                 }
00318 
00319                 else if(Phase == 2) {
00320                 }
00321             }//Case = 0
00322 
00323             /**********************************Case判別2(復帰)***********************************/
00324             //Phase = 0 ホイール回転数減速
00325             //Phase = 1 減速後の復帰動作
00326             //Phase = 2 減速しない復帰動作
00327 
00328             else if(Case ==1) {
00329                 /********************ホイール入力計算********************/
00330                 /*ホイール1_外乱オブザーバ用計算*/
00331                 iR1 = ((Rv_RPM/60) * (2 * PI))/Kn;//目標rpmを角速度に変更して推力定数ノミナル値で割る?
00332                 iRa1 = iR1 - (aa * Jn * Enc_omega_1 + w1) / Kn;
00333                 wk1 = w1 - Samp_T * aa * (w1 + aa * Jn * Enc_omega_1 - Dn * Enc_omega_1 + Kn * iRa1);
00334                 w1 = wk1;
00335 
00336                 /*モータ入力値、リミット*/
00337                 i_1 = iRa1 + i_1k;
00338                 if (i_1 >= 2000)i_1 = 2000;
00339                 if (i_1 <= i_1k-50)i_1 = i_1k-50;
00340 
00341 
00342                 /*減速フェイズ*/
00343                 if(Phase == 0) {
00344                     i_1 = 1000;                     //1000usのニュートラルのときハードブレーキ、ESCでブレーキの強さ設定変更可
00345 
00346                     /*ブレーキ入力*/
00347                     brake = Brake_ON;                 //ジンバル2固定(比較実験用)
00348                     if(Enc_rpm_2 > 10)brake = 0.55;     //現在適当な数値でON-OFF制御しているのでPIDとかで制御してください
00349                     if(Enc_rpm_2 <= 10)brake = 0.68;    //ブレーキ入力はPWMデューティ比入力し、0~1間の値が使用可。Brake.period()の設定によって周期が変動し入力値も変動するので注意
00350 
00351                     if(Enc_rpm_1 <= 100) {
00352                         Phase = 1;
00353                     }
00354                 }
00355 
00356                 /*ジンバル2半ブレーキ減速*/
00357                 else if (Phase == 1) {
00358                     brake = 0.55;
00359                     if (Enc_rpm_2 <= 0.0) {
00360                         brake = Brake_ON;
00361 
00362                         Theta = 180;
00363                         Theta_rpm = 15;
00364 
00365                         Case = 0;
00366                         Phase = 2;
00367 
00368                         /*Rv_RPM = Target_RPM;
00369                          iR1 = 0;
00370                          iRa1 = 0;
00371                          wk1 = 0;
00372                          w1 = 0;*/
00373                     }
00374                 }
00375 
00376                 /*ジンバル復帰*/
00377                 else if(Phase == 2) {
00378                     /*復帰動作*/
00379                     brake = Brake_ON;                 //ジンバル2固定(比較実験用)
00380                     if(Enc_rpm_2 > 5)brake = 1;
00381                     if(Enc_rpm_2 <= 5)brake = 1;
00382 
00383                     Theta = 180;
00384                     Theta_rpm = 1;
00385 
00386                     if (Servo_all_data[0] >= 175 && Servo_all_data[0] <= 185) {
00387                         if(Enc_rpm_2 <= 0) {
00388                             Case = 0;
00389                             Phase = 2;
00390                             brake = Brake_ON;
00391                         }
00392                     }
00393 
00394                 }
00395             }//Case = 1
00396 
00397 
00398 
00399             /********************サーボ入力********************/
00400             Position_Change2(ID_1, Theta, Theta_rpm);
00401             /********************モータ入力********************/
00402             motor.pulsewidth_us(i_1);
00403             /********************ブレーキ入力********************/
00404             Brake.write(brake);
00405             /********************サーボ読み取り********************/
00406             All_Read(ID_1, Servo_all_data);
00407             Current_Read(ID_1,Current);
00408 
00409             /*ホイール角運動量、トルク計測*/ //トルク関連は曖昧なので注意
00410             H = I * Enc_omega_1;
00411             T = H * ((Servo_all_data[1]/60) * (2*PI)) * (cos((180 - Servo_all_data[0])*Radian));    //CMGが出力するトルク
00412             T2 = H * (Fai_omega*Radian) * (cos((180 - Servo_all_data[0])*Radian));                  //機体回転からサーボへ受けるトルク
00413             T3 = H * (Enc_omega_2) * (cos((180 - Servo_all_data[0])*Radian));                       //ジンバル2の回転からサーボへ受けるトルク
00414             /********************表示処理********************/
00415 
00416             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",
00417                       time2,
00418                       timer.read_ms(),
00419                       Case,
00420                       Phase,
00421                       Enc_rpm_1,
00422                       Enc_rpm_2,
00423                       Servo_all_data[0],//角度
00424                       Servo_all_data[1],//RPM
00425                       Servo_all_data[2],//負荷
00426                       //Servo_all_data[3],//電圧
00427                       //Servo_all_data[4],//温度
00428                       Current[0],
00429                       Fai_omega,
00430                       Fai,
00431                       Rv,
00432                       i_1,
00433                       brake,
00434                       Input_Torque,
00435 
00436                       u,
00437                       Lpf_Theta_omega,
00438                       Theta_rpm,
00439                       H,
00440                       T,
00441                       T2,
00442                       T3
00443                      );
00444 
00445         }//if_time
00446     }//while
00447 }//main
00448 
00449 
00450 /*########################################自作関数設定########################################*/
00451 void Position_Change(uint16_t a, double b)   //Position_Change(ID,angle)
00452 {
00453     int i;
00454     uint16_t Tx[9] = {0xff,0xff,0,0x05,0x03,0x1e,0,0,0};
00455     uint16_t Rx[6];
00456     uint16_t input;
00457 
00458     input = b / 0.088;               //入力値算出
00459     Tx[2] = a;                          //ID
00460     /*目標角度 → 下位バイト + 上位バイト*/
00461     Tx[6] = input & 0xff;               //論理積より下位バイト出力
00462     Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
00463     /*Check Sum*/
00464     Tx[8] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7])& 0xff ))&0xff;
00465     /*データ出入力*/
00466     for (i = 0; i <= 8; i++)servo.putc(Tx[i]);
00467     wait_ms(Wait_time);
00468 
00469     //デバック出力
00470     //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]);
00471 
00472     while(servo.readable()) {
00473         for(i=0; i<6; i++)Rx[i]=servo.getc();
00474     }
00475 
00476     //デバック出力
00477     //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]);
00478 }
00479 
00480 /***************************************************/
00481 void Position_Change2(uint16_t a, double b, double c)
00482 {
00483     int i;
00484     uint16_t Tx[11] = {0xff,0xff,0,0x07,0x03,0x1e,0,0,0,0,0};
00485     uint16_t Rx[6];
00486     uint16_t input;
00487     uint16_t input_rpm;
00488     Tx[2] = a;                          //ID
00489 
00490     /*角度*/
00491     input = b / 0.088;               //入力値算出
00492 
00493     /*目標角度 → 下位バイト + 上位バイト*/
00494     Tx[6] = input & 0xff;               //論理積より下位バイト出力
00495     Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
00496 
00497     /*速度*/
00498     input_rpm = c / 0.114;
00499     if(input_rpm == 0)input_rpm = 1;
00500     //pc.printf("%i\r\n",input_rpm);
00501     /*目標速度 → 下位バイト + 上位バイト*/
00502     Tx[8] = input_rpm & 0xff;               //論理積より下位バイト出力
00503     Tx[9] = input_rpm >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
00504     /*Check Sum*/
00505     Tx[10] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7]+ Tx[8]+ Tx[9])& 0xff ))&0xff;
00506     /*データ出入力*/
00507     for (i = 0; i <= 10; i++)servo.putc(Tx[i]);
00508     //wait_ms(Wait_time);
00509 
00510     //デバック出力
00511     //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]);
00512 
00513 
00514     while(1) {
00515         Step++;
00516         if(Step == 50000) {
00517             pc.printf("While Stop1\r\n");
00518             Step = 0;
00519             break;
00520         }
00521         if(servo.readable()==1) {
00522             for(i=0; i<6; i++)Rx[i]=servo.getc();
00523             Step = 0;
00524             break;
00525         }
00526     }
00527 
00528     /*while(servo.readable()) {
00529         for(i=0; i<6; i++)Rx[i]=servo.getc();
00530     }*/
00531 
00532     //デバック出力
00533     //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]);
00534 }
00535 
00536 /******************************************/
00537 void All_Read(uint16_t a, double b[])
00538 {
00539     int i;
00540     uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x24,0x08,0};
00541     uint16_t Rx[14];
00542     Tx[2] = a;            //ID
00543 
00544     Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
00545 
00546     for(i=0; i<8; i++)servo.putc(Tx[i]);
00547     //wait_ms(Wait_time);
00548 
00549     //デバック出力
00550     //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]);
00551     /*while(servo.readable()) {
00552             for(i=0; i<14; i++)Rx[i]=servo.getc();
00553         }*/
00554 
00555 
00556 
00557     /*
00558     timer.read_ms() == 50) {
00559 
00560                 timer.reset();*/
00561     while(1) {
00562         Step++;
00563         if(Step == 50000) {
00564             pc.printf("While Stop2\r\n");
00565             Step = 0;
00566             timer.reset();
00567             break;
00568         }
00569         if(servo.readable()==1) {
00570             for(i=0; i<14; i++)Rx[i]=servo.getc();
00571             Step = 0;
00572             break;
00573         }
00574     }
00575 
00576     if(Rx[4] & 0x20 == 0x20)pc.printf("Error\r\n");
00577 
00578     //デバック出力
00579     //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]);
00580 
00581     b[0] = (double)((Rx[6]<<8)| Rx[5])*0.088;
00582     b[1] = (double)(((Rx[8]<<8)| Rx[7]) & 0x7ff);
00583     if(b[1] >= 1024)b[1] = b[1] - 1024;
00584     b[1] = b[1] * 0.114;
00585     b[2] = (double)(((Rx[10]<<8)| Rx[9]) & 0x7ff);
00586     if(b[2] >= 1024)b[2] = b[2] - 1024;
00587     b[2] = b[2] * 0.1;
00588     b[3] = (double)Rx[11]*0.1;
00589     b[4] = (double)Rx[12];
00590 
00591     //デバック出力
00592     //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]);
00593 
00594 }
00595 /**********************************************/
00596 void Torque_Control_Mode_Enable_Read(uint16_t a)
00597 {
00598     int i;
00599     uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x46,0x01,0};
00600     uint16_t Rx[7];
00601     Tx[2] = a;            //ID
00602 
00603     Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
00604     for(i=0; i<8; i++)servo.putc(Tx[i]);
00605     wait_ms(Wait_time);
00606 
00607     //デバック出力
00608     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]);
00609 
00610     while(servo.readable()) {
00611         for(i=0; i<7; i++)Rx[i]=servo.getc();
00612     }
00613 
00614     //デバック出力
00615     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]);
00616 }
00617 /***********************************************/
00618 void Torque_Control_Mode_Enable_Write(uint16_t a)
00619 {
00620     int i;
00621     uint16_t Tx[8] = {0xff,0xff,0xFe,0x4,0x03,0x46,0,0};
00622     //uint16_t Rx[7];
00623     Tx[6] = a;            //ID
00624 
00625     Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
00626     for(i=0; i<8; i++)servo.putc(Tx[i]);
00627     wait_ms(Wait_time);
00628 
00629     //デバック出力
00630     //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]);
00631 
00632     /*while(servo.readable()) {
00633         for(i=0; i<7; i++)Rx[i]=servo.getc();
00634     }*/
00635 
00636     //デバック出力
00637     //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]);
00638 }
00639 /************************************/
00640 void Goal_Torque(uint16_t a, double b)
00641 {
00642     int i;
00643     uint16_t Tx[9] = {0xff,0xff,0,0x05,0x03,0x47,0,0,0};
00644     uint16_t Rx[6];
00645     uint16_t input;
00646 
00647     input = b;               //入力値算出
00648     Tx[2] = a;                          //ID
00649     /*目標角度 → 下位バイト + 上位バイト*/
00650     Tx[6] = input & 0xff;               //論理積より下位バイト出力
00651     Tx[7] = input >> 8 & 0xff;          //右シフト,論理積より上位バイト出力
00652     /*Check Sum*/
00653     Tx[8] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6]+ Tx[7])& 0xff ))&0xff;
00654     /*データ出入力*/
00655     for (i = 0; i <= 8; i++)servo.putc(Tx[i]);
00656     wait_ms(Wait_time);
00657 
00658     //デバック出力
00659     //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]);
00660 
00661     while(servo.readable()) {
00662         for(i=0; i<6; i++)Rx[i]=servo.getc();
00663     }
00664 
00665     //デバック出力
00666     //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]);
00667 }
00668 /***************************************/
00669 void Current_Read(uint16_t a, double b[])
00670 {
00671     int i;
00672     //double b;
00673     uint16_t Tx[8] = {0xff,0xff,0,0x4,0x02,0x44,0x02,0};
00674     uint16_t Rx[8];
00675     Tx[2] = a;            //ID
00676 
00677     Tx[7] = (~((Tx[2]+Tx[3]+Tx[4]+Tx[5]+Tx[6])& 0xff ))&0xff;            //Check Sum
00678     for(i=0; i<8; i++)servo.putc(Tx[i]);
00679     //wait_ms(Wait_time);
00680 
00681     //デバック出力
00682     //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]);
00683 
00684     /*while(servo.readable()) {
00685         for(i=0; i<8; i++)Rx[i]=servo.getc();
00686     }*/
00687 
00688     while(1) {
00689         Step++;
00690         if(Step == 50000) {
00691             pc.printf("While Stop3\r\n");
00692             Step = 0;
00693             break;
00694         }
00695         if(servo.readable()==1) {
00696             for(i=0; i<8; i++)Rx[i]=servo.getc();
00697             Step = 0;
00698             break;
00699         }
00700     }
00701 
00702 
00703     b[0] = 0.0045 * ((double)((Rx[6]<<8)| Rx[5]) - 2048);
00704     //pc.printf("%10.4f\r\n",b);
00705 
00706     //デバック出力
00707     //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]);
00708 }
00709