|
一覧
検索
最新
ページへ戻る
履歴
FDIII-HC Starter Kit Guide/Chapter10.5
のバックアップソース(No.4)
knowledge
:
FDIII-HC Starter Kit Guide
/
Chapter10.5
のバックアップソース(No.4)
差分
を表示
現在との差分
を表示
FDIII-HC Starter Kit Guide/Chapter10.5
へ行く。
« Prev
Next »
TITLE:ロボットハンドプログラムの修正 ** プログラムの修正 [#w9305379] IKで求めた角度をモーションに組み込みます。 #html{{ <pre class="brush:c;toolbar:false"> #include <math.h> #include <fd.h> #define fd_SetSpec(a,b,c) fd_SetSpec((PHomePosition)a,(PSpec)b,c) #define fd_PlayMotion(a,b,c,d,e) fd_PlayMotion((PPose)a,b,c,d,e) const THomePosition HomePos = { 512, 512, 512 }; const TSpec Spec[] = { { 1, DEV_AX12, fd_AxisOfs(0), +341, { 0, 1023}, 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, { 0, 1023}, 0, 1023, { 1, 1, 32, 32} }, }; const int L[4] = { 40, 28, 68, 40 }; TApplyPart HandParts = { // 3DOFロボットハンド Priority: 1, PartNum: 3, Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2) } }; // * モーションデータ TPose WorkMotion = { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }; double Degree (double rad) { return (rad / (atan(1) * 4)) * 180.0; } void main (void) { double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); fd_SetUVThreshold (7.4); DX_ChangeBaudrate (1000000); fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec)); fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); // ホームポジションへ移動 fd_Wait (2000); D2D3 = L[2]; 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]))); D2j = pos[2] - (L[0] + L[1]); jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); 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)(Degrees(radD1) * 10); // D1の角度 WorkMotion.Structure[1] = (int)(Degrees(radD2) * 10); // D2の角度 WorkMotion.Structure[2] = (int)(Degrees(radD3) * 10); // D3の角度 fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); // モーションの再生 while (fd_GetMotionStat (&HandParts)) fd_Wait (10); } </pre> }} ** まとめ [#o814be71] 指定された座標から角度を算出してモーションでアーム部分を動かすことができるようになりました。~ 範囲外の座標が指示された場合は途中で計算が破綻をきたしますが、acosの前に値のチェックを入れることで回避することができます。又、atan2はx、y共に0の場合エラーとなりますので、こちらも事前にチェックを行っておきましょう。~ さて、IKで角度を算出することはできましたが、実際にモーションとして使用するには移動にかかる時間の指定や軌道の計算、速度の調節等様々な処理が必要となります。自前で用意するには大変なので、今回はFREEDOM III libraryに用意された関数を使用しました。~ ** 課題 [#xf0a3d27] 取り外したハンド部分のAX-12+を分解してアーム部分を延長してみましょう。~ 産業用のロボットアームに近い構成にすることができます。~ #ref(agenda_arm.png) 又、これまでのプログラムでは指定座標のXが-(マイナス)になるとD1が反転してしまいます。これを反転させないようにするにはどうすればよいでしょうか。~ ヒントは指定座標からD3をベクトルとし、Y軸を面として考えることです。 [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る
« Prev
Next »
FDIII-HC Starter Kit Guide/Chapter10.5 のバックアップ一覧
FDIII-HC Starter Kit Guide/Chapter10.5 のバックアップソース(No. All)
1: 2013-08-02 (金) 17:24:32
yoshida
2: 2013-08-02 (金) 19:00:59
yoshida
3: 2013-08-02 (金) 20:27:27
yoshida
4: 2013-08-05 (月) 13:10:51
yoshida
5: 2013-08-06 (火) 20:21:18
yoshida
現: 2013-08-07 (水) 20:32:08
yoshida
Copyright© 1989,2024 BestTechnology Co.,Ltd.
メニュー
トップ
ニュース
お知らせ
新商品情報
更新情報
マニュアル類
技術情報
チュートリアル
FAQ
お問合せ
技術的なお問合せ
その他お問合せ
お見積り
ブログ
会社案内
ショッピング
ご利用案内
検索
カテゴリ 一覧
ロボットキット
Dynamixelシリーズ
マイコンボード
相撲ロボット
センサ
通信機器
お客様ページ