独立ステアリング機構のファームウエアから考えるクラス設計
ロボコンで何度もファームウェアを書き直すうち、「現実の部品の階層構造をそのままクラス階層に写し取る」という設計にたどり着きました。実際にとても修正、機能追加がしやすいコードが書けるようになったので、本稿ではこの設計手法を、独立ステアリング機構を題材に紹介します。
次のようなコードが書けるようになります。XML, React のような構造化されたコードを書いたことがある方には馴染みやすいかもしれません。
Steering steering {
{{
SteeringUnit{ // 1つめのステアリングユニット
WheelUnit{
Motor{ 1, 2, 3 },
},
WheelTurnUnit{
Motor{ 4, 5, 6 },
Encoder{ 30, 31 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 2つめのステアリングユニット
WheelUnit{
Motor{ 7, 8, 9 },
// ...
};
void setup()
{
steering.begin();
}
void loop()
{
steering.move(x, y, turn);
}
前提知識
- C++ の知識 (クラス、コンストラクタ、参照)
- Arduino の知識を少し
やうやう肥大になりゆく ino
私が最初にファームウエアを書いたとき、コードはこんな調子で始まりました。
// モーター 1 のピン
const int MOTOR1_PIN_A = 1;
const int MOTOR1_PIN_B = 2;
const int MOTOR1_PIN_P = 3;
// モーター 2 のピン
const int MOTOR2_PIN_A = 4;
// ...
// エンコーダーのピン
const int ENC1_PIN_A = 30;
const int ENC1_PIN_B = 31;
// ...
// PID ゲイン (とりあえず全タイヤ共通)
double kp = 0.5, ki = 0.0, kd = 0.1;
// 各タイヤの目標角度 / 現在角度 / 積分項 / ...
double target[4];
double current[4];
double integral[4];
// その他、状態を表す変数やカウンターなど
int state = 0; // 非常がかかっているかとか
int counter = 0; // なんかのカウンター
double lastAngle = 0; // ジャイロ補正用
// ...
void setup()
{
pinMode(MOTOR1_PIN_A, OUTPUT);
pinMode(MOTOR1_PIN_B, OUTPUT);
// ...
attachInterrupt(...);
// ...
}
void loop()
{
// スティック値読み取り
// 4輪分の目標角度計算
// 4輪分のエンコーダー読み取り
// 4輪分の PID 計算
// 4輪分のモーター出力
// ...
}
このコードは仕様変更にとても脆弱です。例えば「4 輪から 3 輪に変えたい」「エンコーダーが通信経由になった」「モーター基板の入れ替え」等が行われると考えてみると、狂いそうですね。
そしてこれは、コードが構造化されていない故のしんどさだと気づきました。
ロボットは様々な部品から構成されていると思いますが、よくよく見てみると、単に部品が集合してロボットを成しているのではなく、「部品同士が集合して大きな部品を成し、さらに大きな部品同士が集合してロボットを成す」というように、各部品は階層構造を成している、と見ることが出来るのではないでしょうか。
つまり、ロボットの部品はすでに現実世界の上で構造化されています。にもかかわらず、ソースコードの中ではそれらの境界が溶け、フラットになってしまっている。プログラマは常に具体コードを見続けるしかなく、全体像を把握しづらい。これがコードの肥大化や仕様変更への弱さを生んでいるのではないかと考えました。
部品の階層構造をクラス階層に写し取る
ロボットに限らず、階層構造は構造体・クラスを用いると表現できます。
まずは単純な例を考えてみます。3 つの関節から成り、各関節にモーターとエンコーダーがついているロボットアームを考えます。この場合、次のように階層構造を設計できます。
// モーターを表すクラス
class Motor {};
// エンコーダーを表すクラス
class Encoder {};
// 関節はモーターとエンコーダーから成る
class Joint
{
Motor motor;
Encoder encoder;
};
// ロボットは 3 つの関節から成る
class Robot
{
Joint joint0;
Joint joint1;
Joint joint2;
};
Robot robot;
すると変数 robot は次のような階層構造をもちます。
ファイル構造のように表すと次のようになります。
robot
├─ joint0
│ ├─ motor
│ └─ encoder
├─ joint1
│ ├─ motor
│ └─ encoder
└─ joint2
├─ motor
└─ encoder
このように、メンバ変数に他のクラスのインスタンスを持つことで、部品の階層構造を表現できます。これが「部品の階層構造をクラス階層に写し取る」の意味になります。
クラス階層構造を決める
タイトルの通り、独立ステアリング機構の階層構造を考えてみます。一般には次のようになっていると思います。
次にデータ構造を決めます。ブロックをポコポコ組み合わせるように、クラスを組み合わせていきます。
class Motor {};
class Encoder {};
// 車輪旋回ユニット
class WheelTurnUnit
{
Motor motor;
Encoder encoder;
};
// 車輪
class WheelUnit
{
Motor motor;
};
// ステアリングユニット
class SteeringUnit
{
WheelUnit wheel;
WheelTurnUnit turnUnit;
};
// 独立ステアリング機構
class Steering
{
std::array<SteeringUnit, 4> units;
};
Steering steering;
メンバ関数を決める
次に動作を定義するメンバ関数を決めます。
各部品にどんな動作を期待するか、をもとにメンバ関数を決めるとすんなり決められます。例えば、車輪旋回ユニットにしてほしいことは、「車輪をある角度に回転させる」です。つまり角度を引数にとり、かつ回転することが分かる名前のメンバ関数となります。このように自然言語とソースコードを対応させるように実装すると書きやすいです。
メンバ関数の数は少ない方が楽です。また、ある関数を呼んだ後、ある関数を呼ぶ必要がある、等の関数間の依存関係は少ない方が良いです。今回は「begin 関数でピン等の初期化を行い、move 関数で動作を行う」というシンプルな設計にします。
class Motor
{
public:
void begin();
void move(int power);
};
class Encoder
{
public:
void begin();
std::optional<int> getCount() const; // 値を取得できない場合は std::nullopt を返す
};
// 車輪旋回ユニット
class WheelTurnUnit
{
Motor motor;
Encoder encoder;
PidController turnPid; // フィードバック制御器
public:
void begin();
void move(double targetAngleRad);
};
// 車輪
class WheelUnit
{
Motor motor;
public:
void begin();
void move(int power);
};
// ステアリングユニット
class SteeringUnit
{
WheelUnit wheel;
WheelTurnUnit turnUnit;
public:
void begin();
void move(double wheelAngleRad, int wheelPower);
};
// 独立ステアリング機構
class Steering
{
std::array<SteeringUnit, 4> units;
public:
void begin();
void move(int x, int y, int turn);
};
Steering steering;
void setup()
{
steering.begin();
}
void loop()
{
...
steering.move(x, y, turn);
}
コンストラクタを用いて依存性注入を行う
下位階層のクラスのコンストラクタに引数がある場合、どうやって値を渡すか疑問に思うかもしれません。これはコンストラクタ注入と呼ばれる手法で解決できます。例として WheelTurnUnit クラスのコンストラクタを実装してみます。
// モーター
class Motor
{
int pinA;
int pinB;
int pinP;
public:
Motor(int pinA, int pinB, int pinP)
: pinA{ pinA }
, pinB{ pinB }
, pinP{ pinP }
{}
};
// 車輪ユニット
class WheelTurnUnit
{
Motor motor;
Encoder encoder;
PidController turnPid;
public:
WheelTurnUnit(Motor&& motor, Encoder&& encoder, PidController&& turnPid)
: motor{ std::move(motor) }
, encoder{ std::move(encoder) }
, turnPid{ std::move(turnPid) }
{}
};
// 1つめの車輪旋回ユニット
WheelTurnUnit turnUnit {
Motor{ 4, 5, 6 }, // モーターのピン番号
Encoder{ 30, 31 }, // エンコーダーのピン番号
PidController{ 0.5, 0.0, 0.1 } // PID 制御器のゲイン
};
// 2つめの車輪旋回ユニット
WheelTurnUnit turnUnit {
Motor{ 7, 8, 9 }, // モーターのピン番号
Encoder{ 32, 33 }, // エンコーダーのピン番号
PidController{ 0.5, 0.0, 0.1 } // PID 制御器のゲイン
};
このように、下位クラス(Motor, Encoder, PidController)のインスタンスを、上位クラス WheelUnitのコンストラクタの引数として渡す事で、下位階層に値を渡してのインスタンス化ができます。これはコンストラクタ注入と呼ばれる手法で、依存性注入の一種です。
Motor&&について
Motor&&、Encoder&& 等は 右辺値参照型と呼ばれるもので、一時的に生成されたオブジェクト(右辺値)を受け取るために使います。ここでの右辺値は、Motor{ 1, 2, 3 } 等を指します。
右辺値参照で受け取ると、コンストラクタ内では通常の変数(左辺値)のように振舞います。そこで std::move を使って右辺値にキャストし、モータクラスのムーブコンストラクタを呼び出すようにします。ただし、メンバーが int や double などトリビアルな型のみで構成されているため、暗黙生成されるムーブコンストラクタはコピーと同等の動作をします。したがって std::move してもパフォーマンスが上がることは実質ありません。上位層は下位層の実装(ムーブコンストラクタがあるとかないとか)を気にせず実装したい、という意図でムーブしています。
全クラスにコンストラクタを実装
全クラスにコンストラクタを実装したら完成です。本稿ではメンバ関数の実装は省略します。
全クラスのコンストラクタ実装
モーター
class Motor
{
int pinA;
int pinB;
int pinP;
public:
Motor(int pinA, int pinB, int pinP)
: pinA{ pinA }
, pinB{ pinB }
, pinP{ pinP }
{}
void begin();
// ある出力値でモーターを動かす
void move(int power);
};エンコーダー
class Encoder
{
int pinA;
int pinB;
int count;
public:
Encoder(int pinA, int pinB)
: pinA{ pinA }
, pinB{ pinB }
, count{}
{}
void begin();
// エンコーダーのカウント値を取得する
std::optional<int> getCount() const;
};PID 制御器
class PidController
{
double kp; // 比例ゲイン
double ki; // 積分ゲイン
double kd; // 微分ゲイン
public:
PidController(double kp, double ki, double kd)
: kp{ kp }
, ki{ ki }
, kd{ kd }
{}
// 現在値と目標値をもとに制御量を求める (戻り値が制御量)
double calc(double target, double current);
};車輪旋回ユニット
class WheelTurnUnit
{
Motor motor;
Encoder encoder;
PidController turnPid;
public:
WheelTurnUnit(Motor&& motor, Encoder&& encoder, PidController&& turnPid)
: motor{ std::move(motor) }
, encoder{ std::move(encoder) }
, turnPid{ std::move(turnPid) }
{}
void begin();
// ある角度に車輪を回転させる
void move(double targetAngleRad);
};車輪
class WheelUnit
{
Motor motor;
public:
WheelUnit(Motor&& motor)
: motor{ std::move(motor) }
{}
void begin();
// ある出力値で車輪を動かす
void move(int power);
};ステアリングユニット
class SteeringUnit
{
WheelUnit wheel;
WheelTurnUnit turnUnit;
public:
SteeringUnit(WheelUnit&& wheel, WheelTurnUnit&& turnUnit)
: wheel{ std::move(wheel) }
, turnUnit{ std::move(turnUnit) }
{}
void begin();
// ある角度に車輪を回転させ、ある出力値で車輪を動かす
void move(double wheelAngleRad, int wheelPower);
};ステアリング機構全体
class Steering
{
std::array<SteeringUnit, 4> units;
public:
Steering(std::array<SteeringUnit, 4>&& units)
: units{ std::move(units) }
{}
void begin();
// X,Y方向に平行移動し、旋回を行う
void move(int x, int y, int turn);
}; メインファイル
Steering steering {
{{
SteeringUnit{
WheelUnit{
Motor{ 1, 2, 3 },
},
WheelTurnUnit{
Motor{ 4, 5, 6 },
Encoder{ 30, 31 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{
WheelUnit{
Motor{ 7, 8, 9 },
},
WheelTurnUnit{
Motor{ 10, 11, 12 },
Encoder{ 32, 33 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{
WheelUnit{
Motor{ 13, 14, 15 },
},
WheelTurnUnit{
Motor{ 16, 17, 18 },
Encoder{ 34, 35 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{
WheelUnit{
Motor{ 19, 20, 21 },
},
WheelTurnUnit{
Motor{ 22, 23, 24 },
Encoder{ 36, 37 },
PidController{ 0.5, 0.0, 0.1 },
},
},
}},
};
void setup()
{
steering.begin();
}
void loop()
{
// コントローラーからのスティックの値を取得する
const int x = pad.getLeftX();
const int y = pad.getLeftY();
const int turn = pad.getRightX();
steering.move(x, y, turn);
}
これでクラスの設計は終了です。インスタンス化のコードにも現実世界の部品名、階層構造、部品ごとの定数が現れていて、非常に見やすいのではないでしょうか。
これはコツですがインスタンス化の際、コンストラクタの最後の引数の後ろにコンマをつけることで、自動整形の際に形が崩れにくくなります。また、実際の開発ではクラスごとにファイル分割すると見やすいです。
Motor{ 22, 23, 24 },
Encoder{ 36, 37 },
PidController{ 0.5, 0.0, 0.1 }, <===
}, <===
}, <===
}}};
クラスの階層化が難しくなるパターン
今まで「1 つのインスタンスが 1 つの部品を表す」ことを前提に進めてきましたが、実際そう上手くはいきません。特に通信を介して制御する場合大変になってきます。
例えば 1 つのモーター基板が複数のモーターを制御しており、モーター基板とメイン基板が通信しているような構成を考えます。この構成はよくあるのではないでしょうか。
I2C 通信を例に考える
例えばI2Cで通信していたとしましょう。モーター基板へは、全モーターの出力値をまとめてバイト列にし送るのが一般的です。しかし現在のモータークラスをどう改造しようにも、データのまとめ役がいません。つまりインスタンス間で送信バッファを共有する必要が出てきます。
これは参照やポインタを使うと上手く解決できます。
次の図のようにモーター基板を表すクラスを作り、送信バッファを持たせます。そしてモータークラスはモーター基板クラスの参照を持ち、参照経由で送信バッファにアクセスして値を更新するようにします。
モーター基板を表すクラス
#include <Wire.h> // I2C 通信を行うためのライブラリ
using SlaveAddress = int; // スレーブアドレスを表す型
using MotorIndex = size_t; // 何番目のモーターかを表す型
class MotorPCB
{
TwoWire& wire; // I2C管理クラスの参照
SlaveAddress address; // モーター基板のスレーブアドレス
uint8_t powers[4]; // 送信バッファ モーター4つ分
public:
MotorPCB(TwoWire& wire, SlaveAddress address)
: wire{ wire }
, address{ address }
{}
void setPower(MotorIndex index, uint8_t power)
{
powers[index] = power;
}
void update()
{
wire.beginTransmission(address);
wire.write(powers, sizeof powers);
wire.endTransmission();
}
};
モータークラス
class Motor
{
MotorPCB& motorPCB; // モーター基板の参照
MotorIndex index;
public:
Motor(MotorPCB& motorPCB, MotorIndex index)
: motorPCB{ motorPCB }
, index{ index }
{}
void move(int power)
{
motorPCB.setPower(index, map(power, -255, 255, 0, 255)); // 1バイトの範囲に収める
}
};
メインファイル
MotorPCB motorPCBRight { Wire, SlaveAddress{ 0x10 } }; // 右モーター基板
MotorPCB motorPCBLeft { Wire, SlaveAddress{ 0x11 } }; // 左モーター基板
Steering steering {
{{
SteeringUnit{ // 右上
WheelUnit{
Motor{ motorPCBRight, MotorIndex{ 0 } }, // 右モーター基板の 0 番モーター
},
WheelTurnUnit{
Motor{ motorPCBRight, MotorIndex{ 1 } }, // 右モーター基板の 1 番モーター
Encoder{ 30, 31 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 右下
WheelUnit{
Motor{ motorPCBRight, MotorIndex{ 2 } }, // 右モーター基板の 2 番モーター
},
WheelTurnUnit{
Motor{ motorPCBRight, MotorIndex{ 3 } }, // 右モーター基板の 3 番モーター
Encoder{ 32, 33 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 左下
WheelUnit{
Motor{ motorPCBLeft, MotorIndex{ 0 } }, // 左モーター基板の 0 番モーター
},
WheelTurnUnit{
Motor{ motorPCBLeft, MotorIndex{ 1 } }, // 左モーター基板の 1 番モーター
Encoder{ 34, 35 },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 左上
WheelUnit{
Motor{ motorPCBLeft, MotorIndex{ 2 } }, // 左モーター基板の 2 番モーター
},
WheelTurnUnit{
Motor{ motorPCBLeft, MotorIndex{ 3 } }, // 左モーター基板の 3 番モーター
Encoder{ 36, 37 },
PidController{ 0.5, 0.0, 0.1 },
},
},
}},
};
void setup()
{
Wire.begin();
steering.begin();
}
void loop()
{
// <略> コントローラーからのスティックの値を取得する
steering.move(x, y, turn);
motorPCBRight.update(); // 右モーター基板へ送信
motorPCBLeft.update(); // 左モーター基板へ送信
}
このような感じです。
CAN 通信を例に考える
ちなみに、CAN 通信を使う場合かなり楽に書けます。
CAN 通信では送信ノードと受信ノードをマイコン内に複数もつことができます。なので1 つのノードに 1 つの部品を割り当てると、クラス化しやすいです。次のような構造になります(ソースコードは省略します)。
メインファイル
CanBus canbus; // CAN 通信を管理するクラス
Steering steering {
{{
SteeringUnit{ // 右上
WheelUnit{
Motor{ canbus, CanId{ 0x000 } },
},
WheelTurnUnit{
Motor{ canbus, CanId{ 0x001 } },
Encoder{ canbus, CanId{ 0x100 } },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 右下
WheelUnit{
Motor{ canbus, CanId{ 0x002 } },
},
WheelTurnUnit{
Motor{ canbus, CanId{ 0x003 } },
Encoder{ canbus, CanId{ 0x101 } },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 左下
WheelUnit{
Motor{ canbus, CanId{ 0x004 } },
},
WheelTurnUnit{
Motor{ canbus, CanId{ 0x005 } },
Encoder{ canbus, CanId{ 0x102 } },
PidController{ 0.5, 0.0, 0.1 },
},
},
SteeringUnit{ // 左上
WheelUnit{
Motor{ canbus, CanId{ 0x006 } },
},
WheelTurnUnit{
Motor{ canbus, CanId{ 0x007 } },
Encoder{ canbus, CanId{ 0x103 } },
PidController{ 0.5, 0.0, 0.1 },
},
},
}},
};
void setup()
{
canbus.begin();
steering.begin();
}
void loop()
{
// <略> コントローラーからのスティックの値を取得する
steering.move(x, y, turn);
steering.update(); // CAN 通信で各ノードへ送信
}
コラム 初期化を begin 関数で行う理由
今回挙げたコード例は、begin 関数でピンの初期化等を行っています。コンストラクタで初期化していないのには少し複雑な事情がありますので、段階的に説明します。
前提として「グローバル領域でインスタンス化されたインスタンス」のコンストラクタは main 関数が呼び出される以前に呼び出されます。
struct Foo
{
Foo() { std::cout << "Foo::Foo" << std::endl; }
};
Foo foo; //< グローバル領域でインスタンス化されたインスタンス
int main()
{
std::cout << "main" << std::endl;
}
Foo::Foo // main 関数が呼び出される前に Foo のコンストラクタが呼ばれている
main
続いて Arduino の main 関数を見てみます。Arduino では main 関数は隠蔽されていますが、大まかに次のように実装されています。init 関数はタイマーなどのハードウェアの初期化を行う関数です。
int main()
{
init();
setup();
for (;;)
{
loop();
}
}
ここで重要なのは pinMode 関数等は、init 関数でハードウエアが初期化された後に呼ばれることを前提としている、ということです。つまり、コンストラクタで pinMode 等を呼ぶと、コンストラクタが呼ばれるタイミングではハードウェアが初期化されていないため、意図した通りに動作しない可能性があります(マイコンによる)。そのためハードウェアの初期化が終わった後に初期化を行うよう、begin 関数に実装しています。
余談ですが、main 関数以前の関数呼び出しは、リンカーが .init_array セクションという領域に関数ポインタを配置し、実行時にこのセクションの関数を C ランタイムが呼び出すことで実現されています。
コラム モックテストの導入
ここまでの設計で、Steering, SteeringUnit, WheelTurnUnit といった上位階層のクラスを見直してみると、面白いことに気づきます。これらはどこにも Arduino に依存していません。
つまり上位階層のコードは、純粋な C++ で、PC 上でコンパイルして動かすことができます。そこで多態性を生かして下位階層を差し替えられるようにしてみましょう。
class IMotor
{
public:
virtual ~IMotor() = default;
virtual void begin() = 0;
virtual void move(int power) = 0;
};
// 実機用 (Arduino に依存)
class ArduinoMotor : public IMotor
{
int pinA, pinB, pinP;
public:
ArduinoMotor(int pinA, int pinB, int pinP) : ... {}
void begin() override { pinMode(pinA, OUTPUT); ... }
void move(int power) override { analogWrite(pinP, abs(power)); ... }
};
// テスト用 (純粋 C++)
class MockMotor : public IMotor
{
int currentPower = 0;
public:
void begin() override {}
void move(int power) override { currentPower = power; }
int getPower() const { return currentPower; }
};
上位階層は std::unique_ptr<IMotor> を持つようにしておけば、実機ビルドでは ArduinoMotor を、テストビルドでは MockMotor を注入できます。このようにロボットを動かさなくても上位ロジックのテストが書けるようになります。ただこれは少しやりすぎかも。
さらに一歩進めてシミュレーションへ
MockMotor と MockEncoder を単に値を保持するだけなく、簡易的な仮想物理エンジンとして実装すると、面白いことができます。
例えば MockMotor が受け取った出力値を「タイヤに与えられた力」と解釈し、簡易的な運動方程式を時間積分して仮想的な角速度・角度を計算します。同じ仮想世界に存在する MockEncoder がその角度を読み取って返すようにすれば、PC 上でフィードバック制御系のシミュレーションができます。
ステアリング機構を見れば、各タイヤの旋回角と出力から車体の並進・旋回を計算でき、ロボットがフィールド上をどう動くか再現できます。これを OpenSiv3D のような GUI ライブラリと繋げてやると、PC で動くステアリングロボットのシミュレータの出来上がりってわけ👍🤩👍
最後に
この設計のおいしいところは、書いたものを翌年以降も使い回せるところにあります。階層が綺麗に切れているということは、各層が独立して取り出せるということでもある。Motor, Encoder, PidController あたりは、機構が変わっても使いまわせます。
そして再利用が前提になると、自然とライブラリ化が視野に入ってきます。ここで効いてくるのが、コラムで触れたモックテストです。毎試合モックテストを組むのはやりすぎ感が否めませんが、ライブラリ単位となれば別でしょう。上位階層が Arduino に依存していないおかげで、PC 上でテストが組める。ライブラリ化したい人にとって、これは大きな利点になり得るのではないでしょうか。
もう一つおいしいのは、命名と理解にほとんどコストが掛からないことです。クラス名も変数名も現実の部品名をそのまま流用すればよく、迷いがありません。同じ理由で、メンバーもコードと実機を行き来しながら追えるので、分担であったりフォローしやすいです。
一方で、この設計が苦手とする場面もあります。最大のものは、下位階層のインスタンス同士が直接やり取りしたい場合です。「あるタイヤが旋回中はアームの特定関節を動かしたくない」のような階層を横断する制約は、ツリー構造には素直に乗りません。どうしても上位階層を経由させることになり、コードが少し回りくどくなります。
もうひとつ、書き手側の心理的な落とし穴として、完璧な階層構造を求めすぎてしまうという罠があります。完璧設計を目的化してはいけない、というのは自戒としておきます。
総じてロボコンのファームウェアのように、物理的に分かれた部品を協調させる種類のソフトウェアとは相性が良いと思われます。参考になれば嬉しいです。