SCARAロボットを作ってみる かきかけ

SCARAロボット．水平多関節ロボット

日本で開発された，垂直方向に剛で水平方向に柔(高コンプライアンス)な構造で，

アバウトな位置決めでちょっとずれててもねじ締め組立作業などをこなすロボット．ゆるふわ．

水平方向に2つの回転軸を持ち，その先に垂直直動機構がつくことで

ティーチングや座標指令でXYZ3軸の位置決めを可能にするものを基本とします．

LinuxCNCにはSCARAロボット専用の制御モードがあるのでやってみた．

その際，手元にはSCARAロボットなどないので作る必要があります．

ちょうどラジコンサーボが扱えるようになったのでそれで製作してみることにしました．

設定ファイルはサンプルのSCARAロボットと，wikiで配布されているRCservotestを混ぜたようなものになるが，

出来る限り「よくわからないけどおまじないで書いてある」行を減らすため，

命令のコマンドリファレンスを参照したり，場合によってはコマンドを構成するプログラムのソースを眺めたりもしました．

(実際，PWMGENに関しては，設定ファイルに書いてある理解は違うのではないかというところに遭遇しました)

いつものように，zipでまとめた設定はページ下に，記事にも記載する(とりあえず読んでみたり検索に引っかかるように)．

そのあと，テスト用ロボットを作る話について．

・SCARA-RCservo.hal 設定のメインファイル

## core HAL config file for a SCARA-type arm robot - 2+2 axis with RC-servo motors

## load RT modules

## リアルタイムモジュールの呼び出し

## [___]は.iniの当該セクション参照のリンク．

loadrt scarakins

loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD num_joints=[TRAJ]AXES

## 10000 gives 1 degree per period, or 1 degree resolution, but many PC's may

## crash with this setting. 20000 gives 2 degrees per fast-thread period, or 2 degree resolution.

## "base-thread period"を「10000」にすると0.01ms刻みの分解能が得られるが、多くのPCでは動作が追いつかない

## 20000だと0.02msの分解能になる

## 分解能の計算は {(2200000-800000)/(BASE_PERIOD)}*(最大回転角度) ※サーボにより回転角や信号範囲が異なる

## パラレルポートを呼ぶ。

loadrt probe_parport

## プラグアンドプレイ対応のパラレルポートをチェック

loadrt hal_parport cfg="0 out "

## 利用するパラレルポートの指定と，出力メインで使うか入力を多く取るかの決定．

## IEEE1284の規格上，入出力切替可のポートとどちらか1方向の固定ポートがあるので，どう設定しても固定ポートは変更できない．

## 端末コマンド「dmesg」のログにparport読み込みの記録がある場合、

## cfg="" はparport番号での指定も可。もちろんIOアドレスで"0x--- in"でも良い。

loadrt pwmgen output_type=0,0,0,0

## 通常タイプのPWMモジュールを4セットぶんロード．その他の形式はPWMGENのドキュメントを参照

## ここまでに使うことにしたfunctionがどの間隔で実行されるか．

## 即時反応しないといけないものはbase-thread，遅れても構わない操作はservo-threadで指定

addf pwmgen.update servo-thread

addf pwmgen.make-pulses base-thread

## パルス生成をbase-thread periodで、PWM値変更をservo-thread periodで

## ということは，servo-thread period毎にしか角度指令できないということだが，

## 元からPWM型ラジコンサーボは約10-20msごとの指令を受け取るようにできているので、がんばっても概ねこの程度でしか動かせない．

addf parport.0.read base-thread

addf parport.0.write base-thread

addf parport.0.reset base-thread

addf motion-command-handler servo-thread

addf motion-controller servo-thread

## estop loopback

net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in

## 緊急停止信号のループ ケーブルが外れた時用

## また，ループの途中にB接点のスイッチを仕込むと手動緊急停止ボタンにもできる

## scarakinsの設定

#scarakins − kinematics for SCARA-type robots

setp scarakins.D1 300

setp scarakins.D2 120

setp scarakins.D3 50

setp scarakins.D4 150

setp scarakins.D5 180

setp scarakins.D6 10

## scarakins.D1 (default 490) ※"setp 〜" で明示しなければD1-D6はデフォルト値が入る

## 地面から肩関節j0の中心までの高さ

## Vertical distance from the ground plane to the center of the inner arm.

## scarakins.D2 (default 340)

## 肩関節j0から肘関節j1までの距離．二の腕の長さ

## Horizontal distance between joint[0] axis and joint[1] axis, ie. the length of the inner arm.

## scarakins.D3 (default 50)

## 二の腕と腕の高さ方向のずれ．アーム形状によって負の値になることもある．上空方向が正の値

## Vertical distance from the center of the inner arm to the center of the outer arm.

## May be positive or negative depending on the structure of the robot.

## scarakins.D4 (default 250)

## 肘関節j1から手首j2までの距離．腕の長さ

## Horizontal distance between joint[1] axis and joint[2] axis, ie. the length of the outer arm.

## scarakins.D5 (default 50)

## 手首からワークとの接触面までの高さ方向の変化．正の値だと，地面に近づく側．

## Vertical distance from the end effector to the tooltip.

## Positive means the tooltip is lower than the end effector, and is the normal case.

## scarakins.D6 (default 50)

## 手先回転軸と手首垂直軸とのオフセット距離

## Horizontal distance from the centerline of the end effector

## (and the joints 2 and 3 axis) and the tooltip.

## Zero means the tooltip is on the centerline. Non-zero values should be positive,

## if negative they introduce a 180 degree offset on the value of joint[3].

##ラジコンサーボ用設定など

##********************************************************************

## 主にpwmgen の設定．値は小数点ありで．

#joint0 肩

setp pwmgen.0.min-dc 0.08

## PWM幅最小値の指示。値の範囲は-1から1．

## サーボモータは機械的な限界を超えても動こうとするので，この値で制限する．

## (デューティ%) = [min-dc]*100

## 実際のパルス幅は，(pwmgen.0.scale)*(pwmgen.0.min-dc)で計算．

## ニュートラルの値から7ms短い値，8msのパルス幅としている

## サーボモータのメーカや品種によって値が異なるかもしれないので，可能なら調べる．

## 無負荷で動作させてモータ動作音がし続けるようならより大きく調整の必要がある ．

setp pwmgen.0.max-dc 0.22

## PWM幅最大値の指示。値の範囲は-1から1．

## ニュートラルの値から7ms長い値．2.2msのパルス幅になる．

## サーボモータのメーカや品種によって値が異なるかもしれない．

## デジタルサーボの場合、振り幅を超えた指令は無視する機種もある。

setp pwmgen.0.offset 0.15

## ゼロ点のオフセット．値の範囲は-1から1．

## offset値のぶんだけデューティの中心がオフセットされる．

## サーボのニュートラル位置設定に使う．通常1.5msだが,メーカにより微妙に異なるので確認すること

setp pwmgen.0.scale 1000.00

## 倍率

## (実際のデューティ%) = {(value/scale)+offset}*100

setp pwmgen.0.pwm-freq 100.00

## PWM周波数．値はHz，小数点可．

## 変更可だが、周波数変わると他の設定もそれに合わせ変更の必要がある

## 100Hzってことは，10ms単位でパルスが出力される．

setp pwmgen.0.enable TRUE

## could be connected to a pyVCP button

## 出力オンオフ，

net j0position-command axis.0.motor-pos-cmd => pwmgen.0.value

net j0position-command => axis.0.motor-pos-fb

net j0PWMout pwmgen.0.pwm => parport.0.pin-02-out

## サーボ信号出力ポート設定

net j0enable axis.0.amp-enable-out

#amp control

#net J0pos <= axis.0.motor-pos-cmd

## モーションモジュールから位置をもらってくる

#net j0pos => axis.0.motor-pos-fb

## 現在位置のフィードバック．今回は指令値をそのまま戻す．

## 位置エンコーダがある機械の時は、データを整えてからここに入力する。

#joint1 肘

setp pwmgen.1.min-dc 0.08

setp pwmgen.1.max-dc 0.22

setp pwmgen.1.offset 0.15

setp pwmgen.1.offset 0.15

setp pwmgen.1.scale 1000.00

setp pwmgen.1.pwm-freq 100.00

setp pwmgen.1.enable TRUE

net j1position-command axis.1.motor-pos-cmd => pwmgen.1.value

net j1position-command => axis.1.motor-pos-fb

net j1PWMout pwmgen.1.pwm => parport.0.pin-03-out

#net j1position <= axis.1.motor-pos-cmd

#net j1position => axis.1.motor-pos-fb

net j1enable axis.1.amp-enable-out

#joint2 手首垂直軸

setp pwmgen.2.min-dc 0.08

setp pwmgen.2.max-dc 0.22

setp pwmgen.2.offset 0.15

setp pwmgen.2.offset 0.15

setp pwmgen.2.scale 1000.00

setp pwmgen.2.pwm-freq 100.00

setp pwmgen.2.enable TRUE

net j2position-command axis.2.motor-pos-cmd => pwmgen.2.value

net j2position-command => axis.2.motor-pos-fb

net j2PWMout pwmgen.2.pwm => parport.0.pin-05-out

net j2enable axis.2.amp-enable-out

#joint3 エンドエフェクタ

setp pwmgen.3.min-dc 0.08

setp pwmgen.3.max-dc 0.22

setp pwmgen.3.offset 0.15

setp pwmgen.3.offset 0.15

setp pwmgen.3.scale 1000.00

setp pwmgen.3.pwm-freq 100.00

setp pwmgen.3.enable TRUE

net j3position-command axis.3.motor-pos-cmd => pwmgen.3.value

net j3position-command => axis.3.motor-pos-fb

net j3PWMout pwmgen.3.pwm => parport.0.pin-06-out

net j3enable axis.3.amp-enable-out

## GUI

loadusr -W scaragui #scara向けGUI呼び出し

net j0position-command => scaragui.joint0

net j1position-command => scaragui.joint1

net j2position-command => scaragui.joint2

net j3position-command => scaragui.joint3

## GUIへ現在位置を教える

## create signals for tool loading loopback

net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared

net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed

・SCARA-RCservo.ini

## EMC controller parameters for a SCARA-type robot arm with RC-servo motors.

## General note: Comments can either be preceded with a # or ; - either is

## acceptable, although # is in keeping with most linux config files.

## # と; 以後はコメントとみなすけど，#のほうがlinuxの設定ファイルとしては一般的．

##

## Settings with a + at the front of the comment are likely needed to get

## changed by the user.

## Settings with a - at the front are highly unneeded to be changed

## 「#+」マークのコメントのついてる設定は，ユーザ環境に応じて変更する箇所，

## 「#-」マークのコメントのついてる設定は，通常，変更する必要がないものです

###############################################################################

## General section

###############################################################################

[EMC]

VERSION = 1.0

MACHINE = SCARA-RCservo

## 設定名.ご自由に

DEBUG = 0

# DEBUG = 0x00000007

# DEBUG = 0x7FFFFFFF

## デバッグモードをバイナリで表す。0は未使用。

## 他のオプションはソースを見るsrc/emc/nml_int/emcglb.h

###############################################################################

## Sections for display options

###############################################################################

[DISPLAY]

#+ Name of display program, e.g., xemc

DISPLAY = axis

# DISPLAY = usrmot

# DISPLAY = mini

# DISPLAY = tkemc

## どのGUIを使うか．通常はaxis

EDITOR = gedit

## emcからgコード修正に入るとき利用するテキストエディタ

#- Initial display setting for position, RELATIVE or MACHINE

POSITION_OFFSET = MACHINE

## 起動時に表示する座標系。MACHINE or RELATIVE から選択。

## 機械原点からのワールド座標系か指定したユーザ座標系か、ということ。起動後の変更も可。

#- Initial display setting for position, COMMANDED or ACTUAL

POSITION_FEEDBACK = ACTUAL

## ACTUAL or COMMANDED から選択。サーボ系ドライバで，画面表示する値をコンピュータ指示値にするか、

## ドライバのポジションセンサ(エンコーダ)からの値を標示するか。起動後変更可。

## 凝ったことしなければラジコンサーボには関係ない

#+ Highest value that will be allowed for feed override, 1.0 = 100%

MAX_FEED_OVERRIDE = 2.0

## jog及び移動コードでの最大オーバライド倍率。最高ステップ速度よりは速くならない。

## スライドバーで調整するので，倍率を大きく取りすぎると細かい調整ができなくなる。

INTRO_GRAPHIC = linuxcnc.gif

## 最初のペンギンさんロゴ。変更可

INTRO_TIME = 10

## ペンギンさん最小表示時間sec。ロード時間により、これよりも長くなることはある．

PROGRAM_PREFIX = /home/linuxcnc/linuxcnc/SCARA-NC

## NCコードファイル探索先．デスクトップにファイルを置きたい場合などの際に変更する．

#- Cycle time, in seconds, that display will sleep between polls

CYCLE_TIME = 0.200

## 画面更新間隔(秒)

HELP_FILE = tklinuxcnc.txt

#PYVCP = scara.xml ## 各軸の角度メータ表示設定

###############################################################################

## Read file filter section

###############################################################################

[FILTER]

## 読み込んだファイルでNCコードでないものを、どうやって処理するか

PROGRAM_EXTENSION = .png,.gif,.jpg Greyscale Depth Image

PROGRAM_EXTENSION = .py Python Script

png = image-to-gcode

gif = image-to-gcode

jpg = image-to-gcode

py = python

###############################################################################

## Task controller section

###############################################################################

[TASK]

## Name of task controller program, e.g., milltask

TASK = milltask

#- Cycle time, in seconds, that task controller will sleep between polls

CYCLE_TIME = 0.010

###############################################################################

## Part program interpreter section

###############################################################################

[RS274NGC]

#- File containing interpreter variables

PARAMETER_FILE = scara.var

###############################################################################

## Motion control section

###############################################################################

[EMCMOT]

EMCMOT = motmod

#- Timeout for comm to emcmot, in seconds

COMM_TIMEOUT = 1.0

#- Interval between tries to emcmot, in seconds

COMM_WAIT = 0.010

BASE_PERIOD = 10000

#- Servo task period, in nanoseconds

SERVO_PERIOD = 2000000

#- Trajectory Planner task period, in nanoseconds - will be rounded to an

# integer multiple of SERVO_PERIOD

TRAJ_PERIOD = 10000000

###############################################################################

## Hardware Abstraction Layer section

###############################################################################

[HAL]

HALFILE = SCARA-RCservo.hal #メインのhalファイル

# POSTGUI_HALFILE = scara_postgui.hal ##pyvcpにj1-j4を定義するファイル

#- list of halcmd commands to execute

#HALCMD = save neta

## commands are executed in the order in which they appear

##load halui to enable

HALUI = halui

###############################################################################

## Trajectory planner section

###############################################################################

[TRAJ]

#+ machine specific settings

AXES = 6

## 機械の自由度

COORDINATES = X Y Z C

## 可動軸の担当位置.XYZABCUVWから

HOME = 0 0 0 0

LINEAR_UNITS = mm

## 直線動作の単位、mm inch から選択．この選択は移動の原単位となるので動作量に関わる

ANGULAR_UNITS = deg

## 角度の単位 degree (deg) radian (rad) grad gon(1回転を400に分割)から選択

CYCLE_TIME = 0.010

DEFAULT_VELOCITY = 10.0

## 起動時のG00速度。mm(またはinch)/sec．

## 起動後変更可だが，事故防止のためにあらかじめ節度ある設定にしておく

MAX_VELOCITY = 100.0

## 最高速度。たぶん多軸の合成ベクトルの値。

#DEFAULT_ACCELERATION = 100.0

#MAX_ACCELERATION = 200.0

###############################################################################

## Axes sections

###############################################################################

#[AXIS_number] 説明用ダミー

#TYPE = LINEAR

## 軸の運動形式。直線軸XYZのほか、回転軸ABCやアームのような角度軸が可能

#HOME = 130.0

## 原点復帰で拾った機械原点をいくらの座標値とするか

#MAX_VELOCITY = 15.0

## この軸の最高速度。MAX_LINEAR_VELOCITYと衝突した場合、遅い方採用と思いたい

#MAX_ACCELERATION = 490.0

## 最大加速度。mm(inch)/s^2 速すぎると脱調。モータのデータシートに記載の最大スルーイングより大きくはできない。

## クリーンな状態が最大で、設備を載せてイナーシャが増加するほど加速時に脱調しやすくなる。

## リニアガイドの運動制限もある。 ＊なお、未設定時も、スゲー加速悪いが動きはする

#STEPGEN_MAXACCEL = 612.5

## パルス生成上の制限。上の値より大きくないといけない。1-10％増が推奨。バックラッシュ設定がある場合は1.5-2倍を推奨

#SCALE = 2500.0

## 1unit(mm/inch)進むのに必要なパルス数。

## stepconf_wizではプーリ比やらマイクロステップやら入れましたが、最終的に必要なのはこれだけ。

#FERROR = 1

## PID制御時に許容送り誤差(mm/inch)指示座標とエンコーダの差がこれを越えるとエラーになる

#MIN_FERROR = .25

## ごく遅い送りの時の、指示座標とエンコーダ座標の偏差。

#MIN_LIMIT = 0.0

## ネガ側ソフトウェアリミット位置。Homing後有効。EMC上でカウントした原点からの距離がこれを越えないよう停止される。

#MAX_LIMIT = 140.0

## ポジ側ソフトリミット位置。ハードウェアリミットとは別動作です。

## ソフトリミットは機械原点からの距離を追った状態で減速停止します。

## ncコードがこれを超えそうなときは警告が出ます．

## ハードリミットは拾った瞬間F２停止になるので、機械は守れますがワークはゴミになります

#HOME_OFFSET = 0.000000

## 原点拾った後オフセットさせる量。オフセット後の位置がHOMEで設定した値になる。

#HOME_SEARCH_VEL = 0.050000

## 原点スイッチにHitするまでの速度。

## 設定した加速度で減速する間にセンサを通り過ぎたり端にぶつかったりしないように設定。

## 数値の前に「-」をつけるとサーチ方向を反転できる。

## 0.0にすると，EMCは軸に原点スイッチはないと判断し、その場を原点位置とする

#HOME_LATCH_VEL = 0.050000

## 原点スイッチから抜けるまでの速度。スイッチから抜けた瞬間が0となる。

## 速くすると誤差が出るので、ゆっくり。

## ここでも-符号で移動方向が変更できる。センサから抜ける方向が決まる．

#HOME_FINAL_VEL = 0.0

## オフセットする際の速度。0.0や設定なしの時はG00の速度が適用。必ず正の値であること。

#HOME_IGNORE_LIMITS = NO

## ハードウエアリミットと原点を単一スイッチが兼ねているときはYES。

#HOME＿IS＿SHARED = number

## 複数の軸の原点の信号を1pinにまとめて取り込むとき使用。

## 1にしておくとどこかでセンサが反応している間は原点復帰操作に入れない．

#HOME_SEQUENCE = number

## 同時に複数軸の原点復帰を行う「Home_All」とき、復帰する優先順位。0、1、2、、の順に実行される。

## 未設定の軸はスキップされる。優先順位は複数軸が同一順位でも良い。

## ＊Z軸は先に復帰しないとワークを引きずるとか、諸々の機械の事情を考慮する。

## 例えば一般的なフライスなら、Z(工具軸)のみを0、X、Yを1にする。

#+ First axis 肩

[AXIS_0]

TYPE = ANGULAR

HOME = 0.000

MAX_VELOCITY = 30.0

MAX_ACCELERATION = 200.0

BACKLASH = 0.500

SCALE = 1

#INPUT_SCALE = 50

#OUTPUT_SCALE = 1.000

MIN_LIMIT = -80.0

MAX_LIMIT = 80.0

FERROR = 2.000

MIN_FERROR = 0.100

HOME_OFFSET = 0.0

HOME_SEARCH_VEL = 0.0

HOME_LATCH_VEL = 0.0

HOME_USE_INDEX = NO

HOME_IGNORE_LIMITS = NO

HOME_SEQUENCE = 0

#+ Second axis 肘

[AXIS_1]

TYPE = ANGULAR

HOME = 0.000

MAX_VELOCITY = 60.0

MAX_ACCELERATION = 400.0

BACKLASH = 1.000

SCALE = 1

#INPUT_SCALE = 50

#OUTPUT_SCALE = 1.000

MIN_LIMIT = -155.0

MAX_LIMIT = 155.0

FERROR = 1.000

MIN_FERROR = 0.200

HOME_OFFSET = 0.0

HOME_SEARCH_VEL = 0.0

HOME_LATCH_VEL = 0.0

HOME_USE_INDEX = NO

HOME_IGNORE_LIMITS = NO

HOME_SEQUENCE = 0

#+ Third axis 手首Z軸

[AXIS_2]

TYPE = LINEAR

HOME = 0.000

MAX_VELOCITY = 30.0

MAX_ACCELERATION = 200.0

BACKLASH = 0.000

SCALE = 1

#INPUT_SCALE = 4000

#OUTPUT_SCALE = 1.000

MIN_LIMIT = -10.0

MAX_LIMIT = +10.0

FERROR = 0.500

MIN_FERROR = 0.200

HOME_OFFSET = 25.0

HOME_SEARCH_VEL = 0.0

HOME_LATCH_VEL = 0.0

HOME_USE_INDEX = NO

HOME_IGNORE_LIMITS = NO

HOME_SEQUENCE = 0

#+ Fourth axis エンドエフェクタ

[AXIS_3]

TYPE = ANGULAR

HOME = 0.000

MAX_VELOCITY = 30.0

MAX_ACCELERATION = 200.0

BACKLASH = 0.000

SCALE = 1

#INPUT_SCALE = 4000

#OUTPUT_SCALE = 1.000

MIN_LIMIT = -180.0

MAX_LIMIT = 180.0

FERROR = 2.000

MIN_FERROR = 0.200

HOME_OFFSET = 0.0

HOME_SEARCH_VEL = 0.0

HOME_LATCH_VEL = 0.0

HOME_USE_INDEX = NO

HOME_IGNORE_LIMITS = NO

HOME_SEQUENCE = 0

###############################################################################

## section for main IO controller parameters

###############################################################################

[EMCIO]

#- Name of IO controller program, e.g., io

EMCIO = io

#- cycle time, in seconds

CYCLE_TIME = 0.100

#- tool table file

TOOL_TABLE = scara.tbl

###########################################################################################################

で，テスト用ロボット．

さっと作るためにタミヤのユニバーサルモジュールを駆使してみたのですが．

モジュール以前にサーボの付け根が．しなる．Oh..

もうグネグネ過ぎて，とても耐えられそうにないのでZ軸を外した．

実際に動かしたのはこんな感じ．

ようつべ

今回はGコードモードに持ち込む方法がわからなかったので，各関節に回転指令を出しています．

それから，シミュレータの画面，ここのアームの長さその他の設定はscarakinsの設定と全く独立していて，

別のところにある読み取り専用ファイルをスーパユーザ権限で編集しないとシミュレータの腕の長さなどは変化しません．

このへんの融通聞かなさはLinuxCNCと相容れない気がしますが，特殊な機構に関してはどうもそんなスタンスみたいです．