メモ

今回のメモはステージ制御。

ベーステーブル上にＴＨ軸（回転テーブル）。

その上にＸ軸とＹ軸（Ｘ・Ｙステージ）が乗っかっていて、一番上にワークが乗っかる。

このワークの外周をトレースしたい訳です。

トレースはベーステーブル基準になります。

ココでポイントになるのは、Ｘ・Ｙ・ＴＨ の順番で、

ＴＨ軸が一番下にある場合、ＴＨ軸より上のモノはＴＨの角度によって座標系が変わります。

この為、テーブルのＡＢＳ（アブソリュート）とＸ・Ｙ軸のＡＢＳは別々で存在します。

※ＴＨ軸が一番上に有る場合は、ワークのみＴＨの角度によって座標系が変わる。

よって、演算では「Table ABS 系」 と「Stage ABS 系」の二種類の座標系を演算します。

コレが今回のステージ系の概要図です。

この条件で、ティーチングを行うポイント、演算の為に入力するパラメータを設計します。

Rに面取りされた四角がワークの外周になります。

ワーク自体を動かし、ワーク外周をベースに固定されている青い三角形の部分（AKNX・AKNY）でトレースして行くような演算になります。

 

ティーチングポイントとパラメータは、TH軸が0 °の時にワークが0°と言うのが前提で設計しました。

 

ティーチングポイントは３点

○ ATCX・ATCY　　： TH軸中心座標　※今回はX軸・Y軸の原点はTH軸の中心に合わせた為、0 となります。

○ AWOX・AWOY　： ワーク中心座標　※今回はTH軸の中心に合わせた為、0 となります。

○ AKNX・AKNY　　：　トレースポイント座標

この座標をTable ABS 系でティーチングします。

Table ABS 系では何処でも原点を定義出来るので、分り易くする為に、

ATCX・ATCY を0,0 に設定し、ソレに合わせX軸の原点とY軸の原点をとります。

実は、AWOX・AWOYはATCX・ATCY だけで対応出来るのですが、

他機種になった場合、ワークによっては治具がセンターで取れない様なモノも有るかも知れないので、

念の為AWOX・AWOYを設けて有ります。

今回は0,0 になります。

トレースしたいポイントは、図面なりティーチングなりで、設定したTable ABS 原点（TH軸中心）からの座標になります。

 

パラメータは３点

○ IWW　： ワークの幅（X軸方向長さ）

○ IWH　： ワークの高さ（Y軸方向長さ）

○ ICR　： ワークのコーナーR（面取り半径）

この数値は、座標値では無く、寸法値を設定します

※寸法値は、ABS 系の値と単位を合わせる。

 

これらのティーチングポイントとパラメータでトレースを行います。

実際のプログラムでは、ワークの全周を演算しておりますが、

ココでは演算式の説明を行う為、１ポイントのみ演算します。

使用する式は全周すべてのポイントにおいて１つの演算式で行います。

 

トレースを行うワーク外周ポジションのTable 座標系を割出します。

直線部位はそのままAWOX・AWOYとIWW・IWH の計算でOK。

ワークコーナーのRに関しては、先にICR からTable 座標系を割出し、

その値をAWOX・AWOYとIWW・IWH に足したり引いたりして計算します。

このR からTable 座標系を計算する式は、

RX=-R×COS・θ

RY=R×SIN・θ

R の角度辺りのRX・RY を出しております。

割出したトレースポジションのTable 座標系の値を

　・　TPX

　・　TPY

　　　※Fig.2 の図のASP*X・Y やAEP*X・Y の箇所等

 

とします。

ワークが四角いので、４辺で角度系が０°系、９０°系、１８０°系、２７０°系となります。

※R部位も、その角度系から次の角度系に移る際に９０°変化する。

その為、トレースポイントが角度によってどのStage 座標系の位置に移動するかを演算します。

　・　TKNX=((AKNX-ATCX)×COS・θ)+((AKNY-ATCY)×SIN・θ))+ATCX

　・　TKNY=-((AKNX-ATCX)×SIN・θ)+((AKNY-ATCY)×COS・θ))+ATCY

これはアフィン変換の式で、回転中心がズレていた場合に対応する為、ATCX・ATCYを入れてます。

Stage 座標系のトレースポイントが判明すれば、後はトレースポジションのTable 座標系から引けばStage 座標が出ます。

　・　STPX=TPX-TKNX

　・　STPY=TPY-TKNY

 

この位置にX軸・Y軸を移動させればトレースポイントとトレースポジションが同じ位置になります。

因みに、下記の計算式で、TH軸が回転した後のトレースポジションのTable 座標位置もが計算できます。

　・　TTPX=((TPX-ATCX)×COS・θ)+((TPY-ATCY)×SIN・θ)+ATCX

　・　TTPY=-((TPX-ATCY)×SIN・θ)+((TPY-ATCY)×COS・θ)+ATCY

この位置は特に使用しませんが、一応同様のアフィン変換でTH軸回転後の移動値が確認できます。

 

 

実際の動作には三軸速度補間が必要になります。

基準軸をTH 軸にして、速度を決定します。

直線移動時（X軸・Y軸のみ）の移動速度を、LSP(mm/Sec) とした時に、

R 箇所移動時（X軸・Y軸・TH軸）の移動速度を出します。

先ず、R箇所の長さを出します。

　・　RL=(R×2×π)÷4

この長さが90°動く際に移動するワークの距離ですので、

　・　DRL=RL÷90

で１°あたりの移動距離が出ます。

コレとLSP でTH軸の回転速度が出ます。

　・　RSP=LSP×DRL

このRSP を基準にX軸・Y軸のスピードを補間すれば直線移動時とR 箇所移動時でトレーススピードが同じになります。

 

と、言う事で・・・

次回の何かの時の為のメモ残し終了と！！