ARCS Ver.5.1-SF (Advanced Robot Control System) FRONT PAGE

コレは何?/What is this?

Linuxでロボットの下位レベル制御をするためのフレームワークのようなもの。 通常のLinuxのユーザ空間上において、リアルタイム性の確保もしてくれる。 言語は今のところC++11。



ARCS Ver.5.1-SF の主な機能

ダウンロード

ソースコード一式は以下からダウンロードできます。

REV.51SF160419 までは gcc 4.6.3 以上を推奨。REV.51SF160513以降はC++11に対応したため,gcc 4.8以上が必須。

推奨環境

リアルタイム性を確保するために、下記の環境、条件が満たされている必要があります。

バージョン更新ログ (Change Log)

バージョン更新ログ一覧

ディレクトリ構造

依存先関係図・呼び出し関係図・ファイル-クラス-メンバ関数-定義一覧 (REV.51SF170515)


[ARCS5.1-REV.51SF170729]
├── ARCS                     : 実行ファイル (makeするとデキる)
├── ControlFunctions.cc      : 制御用周期実行関数 (ここに所望の制御系のコードを書く)
├── ControlFunctions.hh      :
├── GraphPlotter.m           : MATLABで実験データをグラフ化し、EPSファイルを出力するためのMファイルのサンプルコード
├── Makefile                 : メイクファイル
├── ReadMe.txt               : 私を読んで
│
├──[src]
│   ├── ARCS.cc             : ARCSのすべての始まりと終わり(main関数 エントリポイント)
│   ├── ARCSassert.cc       : 使い方 ARCS用assertクラス (設定した条件で緊急停止できる)
│   ├── ARCSassert.hh       :
│   ├── ARCSprint.cc        : 使い方 デバッグプリント&イベントログ関連 (変数&行列の表示,イベントログの表示&保存)
│   ├── ARCSprint.hh        :
│   ├── ARCSscreen.cc       : ARCS画面描画関連 (各種数値の表示,状態の表示,波形の描画,コマンドの入力を制御する)
│   ├── ARCSscreen.hh       :
│   ├── ConstParams.cc      : 定数パラメータ格納クラス
│   ├── ConstParams.hh      :
│   ├── ControlFunctions.cc : 制御用周期実行関数(コンパイル用, ここに書き込んでも上書きされるので無意味)
│   ├── ControlFunctions.hh :
│   ├── EventLog.txt        : イベントログ(実行するとデキる)
│   ├── Interface.cc        : 使い方 インターフェース関連 (ADC,DAC,カウンタとロボットの軸との対応を記述する)
│   ├── Interface.hh        :
│   ├── Makefile            : メイクファイル (CPUアーキテクチャに従って要変更!)
│   ├── Makefile.depend     : 依存関係ファイル
│   ├── VariableParams.cc   : 変数パラメータ格納クラス
│   └── VariableParams.hh   :
│
├──[lib]                              : ライブラリ用ディレクトリ
│   ├── ActuatorSim.cc               : モータを模擬するクラス (電流指令と負荷外力から位置応答を計算する)
│   ├── ActuatorSim.hh               : 
│   ├── ChirpGenerator.cc            : チャープ信号生成器 (周波数掃引された信号を出力する)
│   ├── ChirpGenerator.hh            :
│   ├── ClassTemplate.cc             : クラステンプレート(クラスを追加する場合は,このクラステンプレートを基に作ってネ)
│   ├── ClassTemplate.hh             :
│   ├── CuiPlot.cc                   : グラフ描画クラス 詳細は→ CuiPlot
│   ├── CuiPlot.hh                   :
│   ├── DataStorage.cc               : 測定データ保存クラス (実験データを格納しCSV又はTAB区切りDATファイルを出力する)
│   ├── DataStorage.hh               :
│   ├── DeadBand.cc                  : 不感帯関数
│   ├── DeadBand.hh                  :
│   ├── Differentiator.cc            : 使い方 擬似微分器クラス G(s)=(s*gpd)/(s+gpd) (双一次変換)
│   ├── Differentiator.hh            :
│   ├── Differentiator2.cc           : 2階の擬似微分器クラス G(s)=s^2*w^2/(s^2 + w/Q*s + w^2) (双一次変換)
│   ├── Differentiator2.hh           :
│   ├── Discretize.cc                : 使い方 離散化関数 (連続系状態方程式のA行列とB行列をサンプリング時間Tsで離散化する関数)
│   ├── Discretize.hh                : (MATLABでいうところの「c2d関数」と同等)
│   ├── DistObsrv_FullState0order.cc : 同一次元0次外乱オブザーバ (ゼロ次ホールド離散化, q軸電流とモータ側速度からモータ側外乱トルクを推定する)
│   ├── DistObsrv_FullState0order.hh :
│   ├── DQtransform.cc               : DQ変換行列 (dq軸電流とuvw相電流の相互変換をする)
│   ├── DQtransform.hh               :
│   ├── FirstDistObsrv.cc            : 1次外乱オブザーバ (ゼロ次ホールド離散化)
│   ├── FirstDistObsrv.hh            :
│   ├── FixedAverage.cc              : 平均値計算器 (時刻ゼロから現在時刻までの信号の平均を計算する)
│   ├── FixedAverage.hh              :
│   ├── FrameGraphics.cc             : フレームバッファグラフィックスクラス 詳細は→ FrameGraphics
│   ├── FrameGraphics.hh             :
│   ├── Hadamard.cc                  : アダマール行列 (ただのアダマール行列)
│   ├── Hadamard.hh                  :
│   ├── HighPassFilter.cc            : 1次ハイパスフィルタクラス(HPF) G(s)=s/(s+g) (双一次変換)
│   ├── HighPassFilter.hh            :
│   ├── HighPassFilter2.cc           : 2次ハイパスフィルタクラス(HPF) G(s)=s^2/(s^2 + w/Q*s + w^2) (双一次変換)
│   ├── HighPassFilter2.hh           :
│   ├── HysteresisComparator.cc      : ヒステリシス比較器
│   ├── HysteresisComparator.hh      :
│   ├── I_P_I_Pcontroller.cc         : I-P-I-P制御器クラス (後退オイラー)
│   ├── I_P_I_Pcontroller.hh         : 
│   ├── I_PDcontroller.cc            : I-PD制御器クラス (ゼロ次ホールド離散化)
│   ├── I_PDcontroller.hh            : 
│   ├── Integrator.cc                : 積分器クラス G(s)=1/s (前進オイラー,後退オイラー,双一次変換)
│   ├── Integrator.hh                : 
│   ├── Integrator2.cc               : 2階の積分器クラス G(s)=1/s^2 (双一次変換)
│   ├── Integrator2.hh               :
│   ├── License.txt                  : ライセンス情報
│   ├── Limiter.cc                   : リミッタ (任意の数値で入力を制限して出力する)
│   ├── Limiter.hh                   :
│   ├── LowPassFilter.cc             : 使い方 1次ローパスフィルタ(LPF) G(s)=g/(s+g) (双一次変換)
│   ├── LowPassFilter.hh             :
│   ├── LowPassFilter2.cc            : 2次ローパスフィルタ(LPF) G(s)=w^2/(s^2 + w/Q*s + w^2) (双一次変換)
│   ├── LowPassFilter2.hh            :
│   ├── Makefile                     : メイクファイル (CPUアーキテクチャに従って要変更!)
│   ├── Makefile.depend              : 依存関係ファイル
│   ├── Matrix.cc                    : 使い方 行列/ベクトル計算クラス(行列/ベクトルの様々な計算を行う)
│   ├── Matrix.hh                    :
│   ├── NotchFilter.cc               : ノッチフィルタ G(s)=( s^2 + w^2 )/( s^2 + w/Q*s + w^2 ) (双一次変換)
│   ├── NotchFilter.hh               :
│   ├── PCI-3133.cc                  : Interface社製 A/Dコンバータ PCI-3133用クラス
│   ├── PCI-3133.hh                  :
│   ├── PCI-3180.cc                  : 使い方 Interface社製 A/Dコンバータ PCI-3180用クラス
│   ├── PCI-3180.hh                  :
│   ├── PCI-3340.cc                  : Interface社製 D/Aコンバータ PCI-3340用クラス
│   ├── PCI-3340.hh                  :
│   ├── PCI-3343A.cc                 : 使い方 Interface社製 D/Aコンバータ PCI-3343用クラス
│   ├── PCI-3343A.hh                 :
│   ├── PCI-6205.cc                  : Interface社製 エンコーダカウンタ PCI-6205C用クラス (旧バージョン) 廃止
│   ├── PCI-6205.hh                  :
│   ├── PCI-6205C.cc                 : 使い方 Interface社製 エンコーダカウンタ PCI-6205C用クラス
│   ├── PCI-6205C.hh                 :
│   ├── PCIe-AC01.cc                 : ARCS-HFDLインターフェースボード PCIe-AC01用クラス (試作中)
│   ├── PCIe-AC01.hh                 :
│   ├── PDcontroller.cc              : 使い方 比例微分制御器(PD制御器)クラス G(s) = Kp + Kd*s*gpd/(s+gpd) (双一次変換)
│   ├── PDcontroller.hh              :
│   ├── PhaseLeadLag.cc              : 位相補償器クラス G(s)=(1+s*a/g)/(1+s/g) (双一次変換)
│   ├── PhaseLeadLag.hh              :
│   ├── PIcontroller.cc              : 比例積分制御器(PI制御器)クラス G(s) = Kp + Ki/s (双一次変換)
│   ├── PIcontroller.hh              :
│   ├── PIDcontroller.cc             : 比例積分微分制御器(PID制御器)クラス G(s) = Kp + Ki/s + Kd*s*gdis/(s+gdis) (双一次変換)
│   ├── PIDcontroller.hh             :
│   ├── RingBuffer.cc                : リングバッファクラス (その名の通り。移動平均のようなのに使える)
│   ├── RingBuffer.hh                :
│   ├── RPi2GPIO.cc                  : 使い方 ラズベリーパイ2用 GPIO制御クラス (ラズパイ2のI/Oポートを制御する)
│   ├── RPi2GPIO.hh                  :
│   ├── SFmultthread.cc              : SCHED_FIFOリアルタイムマルチスレッドクラス 詳細は→ SCHED_FIFOリアルタイム
│   ├── SFmultthread.hh              :
│   ├── SFthread.cc                  : SCHED_FIFOリアルタイムスレッドクラス 詳細は→ SCHED_FIFOリアルタイム
│   ├── SFthread.hh                  :
│   ├── Sigmoid.cc                   : シグモイド関数
│   ├── Sigmoid.hh                   :
│   ├── Signum.cc                    : シグナム関数(符号関数)
│   ├── Signum.hh                    :
│   ├── SquareWave.cc                : 方形波発振器 (任意の周波数の方形波を出力する)
│   ├── SquareWave.hh                :
│   ├── StairsWave.cc                : 階段波発生器
│   ├── StairsWave.hh                :
│   ├── Statistics.cc                : 統計処理クラス (平均,分散,標準偏差,共分散,相関係数を計算する)
│   ├── Statistics.hh                :
│   ├── TimeDelay.cc                 : 時間遅延生成クラス (むだ時間,通信遅延なんかを模擬するのに使える)
│   ├── TimeDelay.hh                 :
│   ├── TriangleWave.cc              : 三角波発振器
│   ├── TriangleWave.hh              :
│   ├── TrqbsdVelocityObsrv.cc       : トルクセンサベース速度オブザーバ (ゼロ次ホールド離散化)
│   ├── TrqbsdVelocityObsrv.hh       :
│   ├── TwoInertiaSimulator.cc       : 使い方 2慣性共振系シミュレータ (2慣性共振系を模擬する)
│   ├── TwoInertiaSimulator.hh       :
│   ├── TwoStepGenerator.cc          : 2段ステップ発生器
│   ├── TwoStepGenerator.hh          :
│   ├── UDPReceiver.cc               : UDP通信用 受信器クラス (TCP/IPネットワーク上の端末から変数値を受け取る)
│   ├── UDPReceiver.hh               :
│   ├── UDPTransmitter.cc            : 使い方 UDP通信用 送信器クラス (TCP/IPネットワーク上の端末へ変数値を送る)
│   ├── UDPTransmitter.hh            :
│   ├── kbhit.cc                     : キーボード押下検出 (キーが押されたのを検出する)
│   └── kbhit.hh                     :
│
├──[mod]                        : カーネルモジュール用ディレクトリ
│   ├── Makefile               : カーネルモジュールメイク用メイクファイル
│   └── pcie_ac01              : ARCS-HFDLインターフェースボード用ディレクトリ
│       ├── Makefile           : デバイスドライバメイク用メイクファイル
│       ├── Module.symvers     :
│       ├── PCIeReconfig.sh    : PCI-Express通信用FPGAのリコンフィグシェルスクリプト
│       ├── StartModule.sh     : モジュール起動スクリプト
│       ├── StopModule.sh      : モジュール停止スクリプト
│       └── pcie_ac01.c        : ARCS-HFDLインターフェースボード用デバイスドライバ (試作中)

画面サイズの設定

ディスプレイの解像度に従った設定が必要である。 ./src/ConstParams.hh をテキストエディタで開いて、以下の部分を変更する。


     1 :	// 画面サイズの設定 (モニタ解像度に合うように設定すること)
     2 :	// 1024×600 の場合に下記をアンコメントすること
     3 :	//static constexpr int SCR_VERTICAL_MAX = 36;        // [文字] 画面の最大高さ文字数
     4 :	//static constexpr int SCR_HORIZONTAL_MAX = 127;    // [文字] 画面の最大幅文字数
     5 :	// 1024×768 の場合に下記をアンコメントすること
     6 :	static constexpr int SCR_VERTICAL_MAX = 47;        // [文字] 画面の最大高さ文字数
     7 :	static constexpr int SCR_HORIZONTAL_MAX = 127;    // [文字] 画面の最大幅文字数
     8 :	// 1920×1080 の場合に下記をアンコメントすること
     9 :	//static constexpr int SCR_VERTICAL_MAX = 66;        // [文字] 画面の最大高さ文字数
    10 :	//static constexpr int SCR_HORIZONTAL_MAX = 239;    // [文字] 画面の最大幅文字数

デフォルトでは 1024x768 になっている。上記の解像度以外は各自で最適な数値を探してネ。 下記のように、この設定によりどんなモニタでも対応できる。 ただし、画面がはみ出るので1024x600より下は推奨しない。



1024×600に設定


1024×768に設定


1920×1080に設定

空いた領域は、グラフ表示などに好きに使ってネ。

コンパイル方法と実行方法

1. コンパイルオプション(CPUアーキテクチャ)の設定

./lib/Makefile と ./src/Makefile をテキストエディタで開き、ふさわしい物を選んでアンコメントする。 見当たらない場合は、gccの本家で march について調べて適宜記入する。 よく分からん場合はデフォルトでおk。


# コンパイラに渡すオプション  (注意! 下記は処理系によって変更すること!)
# <汎用処理系の場合>
CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++
# <Intel Corei7汎用機の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=corei7
# <Intel Core2汎用機の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=core2
# <Intel nehalem系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=nehalem
# <Intel westmere系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=westmere
# <Intel sandybridge系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=sandybridge
# <Intel ivybridge系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=ivybridge
# <Intel haswell系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=haswell
# <Intel broadwell系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=broadwell
# <Intel skylake系の場合>
#CFLAGS  = -I. -I/usr/src/linux/include -Wall -Weffc++ -march=skylake
# <Raspberry Pi2の場合>
#CFLAGS  = -I. -I../lib -I/usr/src/linux/include -Wall -Weffc++ -fomit-frame-pointer -fforce-addr -ffast-math -mfpu=neon-vfpv4 -mfloat-abi=hard -march=armv7-a

2. 浮動小数点変数の型の設定

double型の演算の方が得意なのか、それともfloat型の方が得意なのか、CPUによって異なる。 例えば、x86系ならdouble型の方が得意、ARM系(組み込み系)ならfloat型の方が得意だったりする(処理系によるので一概に言えないが)。 そこで、すべてのソースコードの浮動小数点変数の型をまるごと置換する機能を追加した。 コマンドラインから下記の太字を入力する。

デフォルトではdouble型になっているので、大抵は何もしなくておk。 Raspberry Pi の場合はfloat型に変換したほうが良いかもしれない。 sinをsinfにしたり、cosをcosfにしたりはしないので、そこは各自手動で。 ただし、floatとdoubleの関数オーバーロードを実装したときは、コンパイルエラーとなるので、その場合はこの機能は使用禁止。

3. コンパイル&リンク

いよいよコンパイル。下記の太字を入力。

[root@hostname ARCS5.1-REV.51SFxxxxxx]# make

最初の一回目は長い。二回目以降は必要最小限のコンパイルが行われる。 ncursesが無いというコンパイルエラーが起きるときは、apt-getやyumでncursesとncursesw関連をインストールすること。 その他、エラーが発生するときはメールを送って下さい。時間があれば対応します。

4. 実行方法

下記でARCS起動。

[root@hostname ARCS5.1-REV.51SFxxxxxx]# ./ARCS

操作方法は ARCS4 と同じ。

5. 中間ファイル等を消したいとき

理由あって中間ファイル等々を消したいときは、

[root@hostname ARCS5.1-REV.51SFxxxxxx]# make clean

とすると消える。どうみてもおかしくないのに、「undefined reference to ...」のリンクエラーが発生するときは、 中間ファイルをクリーンすると解決する場合がある。

6. gdbでデバッグしたいとき

gdbを使ってデバッグしたいときは、

[root@hostname ARCS5.1-REV.51SFxxxxxx]# make debug

とすると、最適化なしでコンパイルして自動でgdbを起動する。

7. ライブラリを追加したとき

libディレクトリに独自のライブラリを追加したときは、

[root@hostname ARCS5.1-REV.51SFxxxxxx]# make dep

とすると、依存関係ファイルを更新する。これをやらないとコンパイルが正常にされないので忘れないこと。

8. すべてのソースファイルの行数を確認したいとき

ARCS全体のソースコードの行数を知りたいときは、

[root@hostname ARCS5.1-REV.51SFxxxxxx]# make num_of_lines

とすると、

wc -l *.cc *.hh ./src/*.cc ./src/*.hh ./lib/*.cc ./lib/*.hh
203 ControlFunctions.cc
66 ControlFunctions.hh
129 ./src/ARCS.cc
・・・中略・・・
75 ./lib/UDPTransmitter.hh
22 ./lib/kbhit.hh
10681 合計

のように、コメント文も含む行数がカウントされ表示される。 (いつの間にやら1万行超えてた…!)

制御系のコーディングの一例

./ControlFunctions.cc に制御系のソースコードを書き込む。 下記の関数が制御周期ごとに呼ばれるので、// ここにやりたい制御を書け!! の部分に所望のコードを書く。


     1 :	#include "ControlFunctions.hh"
     2 :	#include "ExperimentalEquipment.hh"
     3 :	#include "Matrix.hh"
     4 :	#include "ARCSprint.hh"
     5 :	#include "Differentiator.hh"
     6 :	
     7 :	using namespace ARCS;
     8 :	
     9 :	// スレッド間での値のやり取りにはグローバル変数を使う
    10 :	// 但し,無名名前空間内で宣言し変数のスコープをこのソース内に留めること
    11 :	// Spinlock or Mutex による排他制御をすべきだが実際に問題が出てから実装する
    12 :	namespace { // 無名名前空間
    13 :	    volatile double t=0;            // [s]  時刻 (ControlFunction1を基準とする)
    14 :	    volatile double ElectAngle[ConstParams::ACTUATOR_MAX]={0};  // [rad] モータ電気角
    15 :	    volatile double PositionRes[ConstParams::ACTUATOR_MAX]={0}; // [rad] 位置応答
    16 :	    volatile double TorqueRes[ConstParams::ACTUATOR_MAX]={0};   // [Nm]  トルク応答
    17 :	    volatile double AccelerationRes[ConstParams::ACTUATOR_MAX]={0}; // [rad/s^2] 加速度応答
    18 :	    volatile double CurrentRef[ConstParams::ACTUATOR_MAX]={0};  // [A]   q軸電流指令
    19 :	    volatile double CurrentUVRef[ConstParams::ACTUATOR_MAX]={0};// [A]   UV相電流指令
    20 :	}
    21 :	
    22 :	// 制御用周期実行関数群
    23 :	// 以下の関数は初期化モード若しくは終了処理モードのときに非実時間空間上で動作する
    24 :	// 周期モードのときは実時間スレッド( SFmultthread.cc で生成された SFthread.cc の RealTimeThread関数 )
    25 :	// から関数ポインタを経由して,以下の関数が呼ばれる
    26 :	
    27 :	void ControlFunctions::ControlFunction1(ControlFunctions* pCF){
    28 :	    // 制御用周期実行関数1
    29 :	    
    30 :	    // 制御用定数設定
    31 :	    const double Ts = ConstParams::SAMPLING_TIME[0]*1e-9;   // [s]      制御周期
    32 :	    const double gpd = 3140;    // [rad/s]  擬似微分の帯域
    33 :	    
    34 :	    // プラントパラメータ
    35 :	    const double Ktn = 0.49;    // [Nm/A]       トルク定数
    36 :	    const double Jmn = 1.44e-4; // [kgm^2]      モータ側慣性
    37 :	    
    38 :	    // 制御用変数宣言
    39 :	    static bool ZclearFlag = true;  // Z相クリアフラグ
    40 :	    static double Iref_1 = 0;   // [A] q軸電流指令 モータ1
    41 :	    static double Iref_2 = 0;   // [A] q軸電流指令 モータ2
    42 :	    static double tau_s1 = 0;   // [Nm] ねじれトルク測定値
    43 :	    static double tau_s2 = 0;   // [Nm] ねじれトルク測定値
    44 :	    static double TrqOfst_1 = 0;// [Nm] トルクセンサオフセット計測値
    45 :	    static double TrqOfst_2 = 0;// [Nm] トルクセンサオフセット計測値
    46 :	    static double alpha_l1 = 0; // [rad/s^2] 負荷側加速度
    47 :	    static double theta_m1 = 0; // [rad]モータ側位置
    48 :	    static double theta_m2 = 0; // [rad]モータ側位置
    49 :	    static double omega_m1 = 0; // [rad]モータ側速度
    50 :	    static double omega_m2 = 0; // [rad]モータ側速度
    51 :	    
    52 :	    // 制御器等々
    53 :	    static ExperimentalEquipment* ExperimentalSys;  // 実験装置用クラス
    54 :	    static Differentiator* VelocityCalc1;   // 擬似微分器
    55 :	    static Differentiator* VelocityCalc2;   // 擬似微分器
    56 :	    
    57 :	    if(pCF->CmdFlag==CTRL_INIT){
    58 :	        // 初期化モード (ここは制御開始時に1度だけ呼び出される(非実時間空間なので重い処理もOK))
    59 :	        ExperimentalSys = new ExperimentalEquipment(Ts);    // 実験装置用クラス(これは実験装置の仕様に従って各自で実装)
    60 :	        VelocityCalc1 = new Differentiator(gpd,Ts); // 速度計算用擬似微分器
    61 :	        VelocityCalc2 = new Differentiator(gpd,Ts); // 速度計算用擬似微分器
    62 :	    }
    63 :	    if(pCF->CmdFlag==CTRL_LOOP){
    64 :	        // 周期モード (ここは制御周期 SAMPLING_TIME[0] 毎に呼び出される(実時間空間なので処理は制御周期内に収めること))
    65 :	        pCF->count++;       // ループカウンタを進める
    66 :	        t=pCF->count*Ts;    // 時刻の計算
    67 :	        
    68 :	        // 制御ここから
    69 :	        // センサ情報の取得
    70 :	        pCF->pIF->GetTrqAndAcc(TorqueRes, AccelerationRes); // [Nm],[rad/s^2] トルク,加速度応答の取得
    71 :	        pCF->pIF->GetPosition(ElectAngle, PositionRes);     // [rad] 位置応答の取得
    72 :	        tau_s1 = TorqueRes[0] - TrqOfst_1;  // [Nm] ねじれトルク測定値(オフセット補正済み)
    73 :	        tau_s2 = TorqueRes[1] - TrqOfst_2;  // [Nm] ねじれトルク測定値(オフセット補正済み)
    74 :	        alpha_l1 = AccelerationRes[0];      // [rad/s^2] 負荷側加速度の取得
    75 :	        theta_m1 = PositionRes[0];          // [rad] モータ位置の取得
    76 :	        theta_m2 = PositionRes[1];          // [rad] モータ位置の取得
    77 :	        omega_m1 = VelocityCalc1->GetSignal(theta_m1);  // [rad] モータ位置の計算
    78 :	        omega_m2 = VelocityCalc2->GetSignal(theta_m2);  // [rad] モータ位置の計算
    79 :	        
    80 :	        if(ExperimentalSys->InitComplete() == false){   // 初期化完了フラグの確認
    81 :	            // 初期化動作フェーズ
    82 :	            pCF->Initializing = true;
    83 :	            ExperimentalSys->InitOperations(t, CurrentUVRef, ElectAngle, PositionRes, TorqueRes, TrqOfst_1, TrqOfst_2, ZclearFlag); // 実験装置初期化動作
    84 :	            pCF->pIF->SetZpulseClear(ZclearFlag);   // Z相クリア設定
    85 :	        }else{
    86 :	            // 制御動作フェーズ
    87 :	            pCF->Initializing = false;
    88 :	            
    89 :	            // ここにやりたい制御を書け!!
    90 :	            Iref_1 = 0.2;
    91 :	            Iref_2 = 0.2;
    92 :	            
    93 :	            CurrentRef[0] = Iref_1;
    94 :	            CurrentRef[1] = Iref_2;
    95 :	            ExperimentalSys->ConvCurrentRef(CurrentRef, ElectAngle, CurrentUVRef);  // q軸電流指令からUV相電流指令へ変換
    96 :	        }
    97 :	        
    98 :	        pCF->pIF->SetCurrent(CurrentUVRef); // [A] UV相電流指令の出力
    99 :	        // 制御ここまで
   100 :	        
   101 :	        // 任意変数値表示用
   102 :	        pCF->IndicVars[0] = tau_s1; // 表示変数
   103 :	        pCF->IndicVars[1] = tau_s2; // 表示変数
   104 :	        pCF->IndicVars[2] = alpha_l1;// 表示変数
   105 :	        pCF->IndicVars[3] = 0;// 表示変数
   106 :	        pCF->IndicVars[4] = 0;  // 表示変数
   107 :	        pCF->IndicVars[5] = 0;  // 表示変数
   108 :	        pCF->IndicVars[6] = 0;  // 表示変数
   109 :	        pCF->IndicVars[7] = 0;  // 表示変数
   110 :	        
   111 :	        // グラフプロット用
   112 :	        pCF->PlotVarsA[0] = Iref_1;
   113 :	        pCF->PlotVarsA[1] = Iref_2;
   114 :	        pCF->PlotVarsB[0] = omega_m1;
   115 :	        pCF->PlotVarsB[1] = omega_m2;
   116 :	        pCF->PlotVarsC[0] = tau_s1;
   117 :	        pCF->PlotVarsC[1] = tau_s2;
   118 :	        pCF->PlotVarsC[2] = 0;
   119 :	        pCF->PlotVarsD[0] = alpha_l1;
   120 :	        
   121 :	        // データの保存用
   122 :	        pCF->Data[0] = t;   // [s]  時刻の保存
   123 :	        pCF->Data[1] = 0;   // 保存変数
   124 :	        pCF->Data[2] = 0;   // 保存変数
   125 :	        pCF->Data[3] = 0;   // 保存変数
   126 :	        pCF->ExpData->PutData(pCF->Data,ConstParams::DATA_NUM); // データ格納
   127 :	    }
   128 :	    if(pCF->CmdFlag==CTRL_EXIT){
   129 :	        // 終了処理モード (ここは制御終了時に1度だけ呼び出される(非実時間空間なので重い処理もOK))
   130 :	        pCF->pIF->SetZeroCurrent(); // 電流指令を零に設定
   131 :	        delete VelocityCalc1; VelocityCalc1 = nullptr;
   132 :	        delete VelocityCalc2; VelocityCalc2 = nullptr;
   133 :	        delete ExperimentalSys; ExperimentalSys = nullptr;  // 実験装置用クラス
   134 :	    }
   135 :	}

インタフェース部分のコーディングの一例 (ソース側)

./src/interface.cc は以下のような感じで書く。下記はPCI-3180,PCI-3343A,PCI-6205Cを使った場合の例。 ヘッダ側は→ ヘッダ側の書き方


     1 :	#include "Interface.hh"
     2 :	
     3 :	using namespace ARCS;
     4 :	
     5 :	Interface::Interface()
     6 :	    // コンストラクタ
     7 :	    : ENC(), ADC(), DAC()
     8 :	{
     9 :	    // ENCボード初期化処理
    10 :	    ENC = new PCI6205C(
    11 :	        ConstParams::PCI6205C_ADDR1, ConstParams::PCI6205C_ADDR2,
    12 :	        ConstParams::PCI6205C_ADDR3, ConstParams::PCI6205C_ADDR4,
    13 :	        ConstParams::ACTUATOR_NUM, ConstParams::ENC_MULT4
    14 :	    );
    15 :	    ADC = new PCI3180(ConstParams::PCI3180_ADDR, PCI3180::RANGE_B_5V);  // ADCボード初期化処理(入力レンジを±5Vに設定)
    16 :	    DAC = new PCI3343A(ConstParams::PCI3343A_ADDR);                     // DACボード初期化処理
    17 :	}
    18 :	
    19 :	Interface::~Interface(){
    20 :	    // デストラクタ
    21 :	    delete ENC; ENC = NULL; // ENCボード終了処理
    22 :	    delete ADC; ADC = NULL; // ADCボード終了処理
    23 :	    delete DAC; DAC = NULL; // DACボード終了処理
    24 :	}
    25 :	
    26 :	void Interface::GetTrqAndAcc(volatile double* torque, volatile double* acceleration){
    27 :	    // トルク応答と加速度応答を取得する関数
    28 :	    double V1, V2, V3, V4;
    29 :	    ADC->ConvStart();                   // AD変換開始
    30 :	    ADC->WaitBusy();                    // AD変換が完了するまで待機(ブロッキング動作)
    31 :	    ADC->GetVoltage(&V1, &V2, &V3, &V4);// [V] 電圧値の取得
    32 :	    torque[0] = ConstParams::TRQSEN_TO_NM*V1;   // [Nm] 電圧からモータ1のトルク応答へ換算
    33 :	    torque[1] = ConstParams::TRQSEN_TO_NM*V2;   // [Nm] 電圧からモータ2のトルク応答へ換算
    34 :	    acceleration[0] = ConstParams::ACCSEN_TO_MPS2/ConstParams::ACCSEN_RADIUS*V3;    // [rad/s^2] 電圧からモータ1の加速度応答へ換算
    35 :	}
    36 :	
    37 :	void Interface::GetPosition(volatile double* pos_e, volatile double* pos_m){
    38 :	    // 位置応答を取得する関数
    39 :	    long count[PCI6205C::MAX_CH] = {0};
    40 :	    ENC->GetCount(count);   // エンコーダカウント数を取得
    41 :	    // モータ電気角の取得
    42 :	    pos_e[0] = ConvElectAngle( count[0]);   // [rad] 1軸目
    43 :	    pos_e[1] = ConvElectAngle(-count[1]);   // [rad] 2軸目 (エンコーダが逆に取り付けられているので注意)
    44 :	    // モータ機械角の取得
    45 :	    pos_m[0] = ConvMotorAngle( count[0]);   // [rad] 1軸目
    46 :	    pos_m[1] = ConvMotorAngle(-count[1]);   // [rad] 2軸目 (エンコーダが逆に取り付けられているので注意)
    47 :	}
    48 :	
    49 :	void Interface::SetZpulseClear(bool flag){
    50 :	    // Z相クリア設定
    51 :	    ENC->ZpulseClear(flag);
    52 :	}
    53 :	
    54 :	void Interface::SetCurrent(volatile double* current){
    55 :	    // 電流指令を設定する関数
    56 :	    double V1, V2, V3, V4;
    57 :	    
    58 :	    // 以下に「モータ軸番号」と「D/Aコンバータの各チャネル」の対応を書く
    59 :	    // 電流リミッタをかけてから電圧信号へ換算
    60 :	    V1 = -ConstParams::SRV_A_TO_V*Limiter(current[0], ConstParams::ACT_MAX_CURRENT[0]); // U相電流指令 モータ1
    61 :	    V2 = -ConstParams::SRV_A_TO_V*Limiter(current[1], ConstParams::ACT_MAX_CURRENT[0]); // V相電流指令 モータ1
    62 :	    V3 = -ConstParams::SRV_A_TO_V*Limiter(current[2], ConstParams::ACT_MAX_CURRENT[1]); // U相電流指令 モータ2
    63 :	    V4 = -ConstParams::SRV_A_TO_V*Limiter(current[3], ConstParams::ACT_MAX_CURRENT[1]); // V相電流指令 モータ2
    64 :	    
    65 :	    DAC->SetVoltage(V1, V2, V3, V4);    // DA変換電圧更新
    66 :	}
    67 :	
    68 :	void Interface::SetZeroCurrent(void){
    69 :	    // 電流指令をゼロに設定する関数
    70 :	    DAC->SetVoltage(0,0,0,0);
    71 :	}
    72 :	
    73 :	double Interface::ConvMotorAngle(long count){
    74 :	    // モータ機械角 [rad] への換算
    75 :	    return ConstParams::ENC_TO_RADIAN*(double)count;
    76 :	}
    77 :	
    78 :	double Interface::ConvElectAngle(long count){
    79 :	    // モータ電気角 [rad] への換算 (-2π~+2πの値域制限あり)
    80 :	    return ConstParams::ENC_TO_RADIAN*(double)(ConstParams::ENC_POLEPARE*( count % (ConstParams::ENC_MAX_COUNT/ConstParams::ENC_POLEPARE) ));
    81 :	}

リアルタイム性の確認

リアルタイム性を確認するために、オシロスコープで制御周期を計測。 下記のデータが得られ、カーネルはそのまま且つユーザ空間でのコーディングで、制御周期200μsでも十分なリアルタイム性があることを確認。


制御周期の計測: 平均値 200.0μs ± 標準偏差 0.3140μs

使用機材は Raspberry Pi 2 Model B (Coretex-A7@900MHz) と YOKOGAWA DLM2034 (2.5Gsps, 350MHz)。
プロット点の縦軸の大きさが離散的なのは、おそらく、オシロの時間分解能のせいだと思われる。

リアルタイム制御の実現方法

どうやってリアルタイム性を確保しているかの詳細は→ SCHED_FIFOリアルタイム制御


ROS(Robot Operating System)との違い

ROSを知っている人からすれば,こんなもん独自で作る必要ないじゃんとか思うかもしれない。 しかしながら,我々の業界では,制御周期は 200μs から 100μs,場合によっては 50μs が要求される。 さらに,可能な限り少ないジッタ量が要求される。 リアルタイム性が命である。 ROSでその要求を満たすのは中々難しいはず。 我々からするとROSは上位層用である。 なので,ROSで上位層を,ARCSで下位層を作るという住み分けが正解。

今後の実装予定

ARCSに今後実装を予定している機能&改善点等々は以下の通り。

ロゴ

なんでこんなん作ったかというと,ある競争資金の予算申請書に必要だったので… (ちょっとデザインを変更)

ライセンス/Licenses

Copyright (c) 2011-2017, Yuki YOKOKURA
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FreeBSD Project.





- 97661 -

研究室の横の倉庫 - Side Warehouse of Laboratory
Copyright(C), Side Warehouse, All rights reserved.