yuto
Dependencies: ITG3200 QEI mbed
Fork of NEW_CMG_2016_3_7 by
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
Generated on Fri Jan 26 2024 16:48:15 by
1.7.2
