ワークが四角いので、4辺で角度系が0°系、90°系、180°系、270°系となります。※R部位も、その角度系から次の角度系に移る際に90°変化する。座標系の位置に移動するかを演算します。TKNY=-((AKNX-ATCX)×SIN・θ)+((AKNY-ATCY)×COS・θ))+ATCYこれはアフィン変換の式で、回転中心がズレていた場合に対応する為、ATCX・ATCYを入れてます。STPY=TPY-TKNYこの位置にX軸・Y軸を移動させればトレースポイントとトレースポジションが同じ位置になります。座標位置もが計算できます。TTPY=-((TPX-ATCY)×SIN・θ)+((TPY-ATCY)×COS・θ)+ATCYこの位置は特に使用しませんが、一応同様のアフィン変換でTH軸回転後の移動値が確認できます。実際の動作には三軸速度補間が必要になります。軸にして、速度を決定します。箇所移動時(X軸・Y軸・TH軸)の移動速度を出します。R箇所の長さを出します。DRL=RL÷90で1°あたりの移動距離が出ます。TH軸の回転速度が出ます。箇所移動時でトレーススピードが同じになります。PageTwitter。