2: 2013-08-02 (金) 19:00:59 yoshida ソース 現: 2013-08-07 (水) 20:32:08 yoshida ソース
Line 1: Line 1:
TITLE:ロボットハンドプログラムの修正 TITLE:ロボットハンドプログラムの修正
** プログラムの修正 [#w9305379] ** プログラムの修正 [#w9305379]
 +IKで求めた角度をモーションに組み込みます。
#html{{ #html{{
<pre class="brush:c;toolbar:false"> <pre class="brush:c;toolbar:false">
Line 10: Line 11:
const THomePosition HomePos = { 512, 512, 512 }; const THomePosition HomePos = { 512, 512, 512 };
const TSpec Spec[] = { const TSpec Spec[] = {
-  { 1, DEV_AX12, fd_AxisOfs(0), +341, { 257, 768},  0, 1023, { 1, 1, 32,  32} }, +  { 1, DEV_AX12, fd_AxisOfs(0), +341, { 0, 1023},  0, 1023, { 1, 1, 32,  32} }, 
-  { 2, DEV_AX12, fd_AxisOfs(1), +341, { 390, 818},  0, 1023, { 1, 1, 32,  32} }, +  { 2, DEV_AX12, fd_AxisOfs(1), +341, { 0, 1023},  0, 1023, { 1, 1, 32,  32} }, 
-  { 3, DEV_AX12, fd_AxisOfs(2), +341, {   64, 512},  0, 1023, { 1, 1, 32,  32} },+  { 3, DEV_AX12, fd_AxisOfs(2), -341, { 0, 1023},  0, 1023, { 1, 1, 32,  32} },
}; };
-const int L[4] = { 42, 27, 68, 40 };+const int L[4] = { 42, 27, 83, 55 }; 
TApplyPart HandParts = {  // 3DOFロボットハンド TApplyPart HandParts = {  // 3DOFロボットハンド
  Priority: 1,   Priority: 1,
Line 21: Line 23:
}; };
// * モーションデータ // * モーションデータ
-TPose WorkMotion [] = { +TPose WorkMotion = { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, };
-  { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, +
-  { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, +
-  { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, +
-};+
double Degree (double rad) { double Degree (double rad) {
  return (rad / (atan(1) * 4)) * 180.0;   return (rad / (atan(1) * 4)) * 180.0;
 +}
 +
 +// プッシュボタンが押下されるとプログラムが終了します
 +void USER_TASK1 (void) {
 +  while (!fd_GetPB ()) {
 +   fd_Wait (10);
 +  }
 +  fd_SoftReset ();
} }
void main (void) { void main (void) {
  double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3;   double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3;
 +  double pos[3];
 +  int in[3], i;
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);   fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);   fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);   DX_ChangeBaudrate (1000000);
 +
 +  fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));
 +  fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts);  // ホームポジションへ移動
 +
  fd_Wait (2000);   fd_Wait (2000);
 +
 +  act_tsk (TASK1);
  D2D3  = L[2];   D2D3  = L[2];
  D3POS = L[3];   D3POS = L[3];
-  D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); +  while (1) { 
-  D2j = pos[2] - (L[0] + L[1]); +   fd_puts ("\nset position (x,y,z) = "); 
-  jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); +   fd_scanf ("%d,%d,%d", &in[0], &in[1], &in[2]);  // 座標の指示 
-  tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); +   for (i = 0; i < 3; i++) pos[i] = (double)in[i]; 
-  tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); +   D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); 
-  cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); +    D2j = pos[2] - (L[0] + L[1]); 
-  radD1 = atan2 (pos[1], pos[0]); +    jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); 
-  radD2 = acos(tmpcos2) - acos(tmpcos1); +    tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); 
-  radD3 = acos(-1) - acos (cosD3); +    tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); 
-  WorkMotion[1].+    cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); 
 +    radD1 = atan2 (pos[1], pos[0]); 
 +    radD2 = acos(tmpcos2) - acos(tmpcos1); 
 +    radD3 = acos(-1) - acos (cosD3); 
 +    WorkMotion.Structure[0] = (int)(Degree(radD1) * 10);  // D1の角度 
 +   WorkMotion.Structure[1] = (int)(Degree(radD2) * 10);  // D2の角度 
 +   WorkMotion.Structure[2] = (int)(Degree(radD3) * 10);  // D3の角度 
 +   fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts);    // モーションの再生 
 +   while (fd_GetMotionStat (&HandParts)) fd_Wait (10); 
 +  }
} }
</pre> </pre>
}} }}
 +このプログラムはSIMPLE TERM(ターミナルプログラム)から座標を指定できるようになっており、プッシュボタンが押されると終了してしまいます。
 +
 +** まとめ [#o814be71]
 +本章ではFREEDOM III libraryに用意された関数を使用してモーションでロボットハンドを動かしてみました。~
 +又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。
 +
 +** 課題 [#xf0a3d27]
 +指定された座標によってはプログラムの途中で計算が破綻をきたし、不正な角度を設定してしまいます。これを回避するよう、プログラムを修正して下さい。~
 +ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。~
 +又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。~
 +#ref(agenda_arm.png)
 +
 +[[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom