[[ジャイロを使ってみよう]]
差分
このページの2つのバージョン間の差分を表示します。
| 次のリビジョン | 前のリビジョン | ||
| ジャイロを使ってみよう [2011/11/21 22:57] – 作成 member | ジャイロを使ってみよう [2011/11/22 00:34] (現在) – member | ||
|---|---|---|---|
| 行 46: | 行 46: | ||
| - | とりあえずここまで保存。 | + | ===== ジャイロ値の読み出しと積分 ===== |
| + | まず、[[物理量ベースとは|物理量ベース]]になっていないと、ジャイロを使うのは難しいです。ジャイロを導入しようと思うのであればこちらを先に完成させてください。 | ||
| + | |||
| + | まず、ジャイロの出力から実際の角速度[rad/ | ||
| + | アナログ出力の場合を示します。デジタル出力の場合は、感度が[deg/ | ||
| + | <code c> | ||
| + | |||
| + | /* | ||
| + | 外部で、chのAD変換結果を電圧で返す、float get_ad(int ch); や、 | ||
| + | AD変換が行われるまで待つ、void wait_next_ad(void); | ||
| + | */ | ||
| + | |||
| + | |||
| + | #define DEG2RAD(x) | ||
| + | #define RAD2DEG(x) | ||
| + | |||
| + | // ジャイロ関連マクロ | ||
| + | #define GYRO_CH | ||
| + | #define GYRO_SENSITIVITY (???? | ||
| + | #define GYRO_REF_NUM | ||
| + | #define GYRO_SIGN | ||
| + | |||
| + | float gyro_ref_volt; | ||
| + | |||
| + | // ジャイロのリファレンス電圧をリセットする。 | ||
| + | // 単なる平均でなく、メディアンフィルタ等と組み合わせても良い。 | ||
| + | void resetGyroReference(void) | ||
| + | { | ||
| + | int i; | ||
| + | |||
| + | gyro_ref_volt = 0.0f; | ||
| + | for(i = 0; i < GYRO_REF_NUM; | ||
| + | { | ||
| + | wait_next_ad(); | ||
| + | gyro_ref_volt += get_ad(GYRO_CH); | ||
| + | } | ||
| + | |||
| + | gyro_ref_volt /= GYRO_REF_NUM; | ||
| + | } | ||
| + | |||
| + | |||
| + | // 角速度を返す | ||
| + | float getAngularVelocity(void) | ||
| + | { | ||
| + | return DEG2RAD( GYRO_SENSITIVITY * ( get_ad(GYRO_CH) - gyro_ref_volt ) ); | ||
| + | } | ||
| + | |||
| + | </ | ||
| + | 上記のように、簡単に角速度の出力が得られます。 | ||
| + | \\ またジャイロの特徴として、高精度なので積分しても生じるずれが少なく、角度も高精度で計測できます。 | ||
| + | |||
| + | 角度についても、次のように簡単に求められます。 | ||
| + | <code c> | ||
| + | #define SYSTEM_PERIOD (0.001f) | ||
| + | |||
| + | |||
| + | float angle; | ||
| + | float angvel; // | ||
| + | |||
| + | |||
| + | /* | ||
| + | ジャイロから得た角速度を積分し、角度を得る。 | ||
| + | |||
| + | angvelには、他の場所で取得した角速度が入っているとする。 | ||
| + | この関数はSYSTEM_PERIOD秒間隔で呼び出されるとする。 | ||
| + | */ | ||
| + | |||
| + | void integrateGyro(void) | ||
| + | { | ||
| + | angle += angvel * SYSTEM_PERIOD; | ||
| + | } | ||
| + | |||
| + | |||
| + | </ | ||
| + | |||
| + | これだけです。が、長い間(回路やジャイロによっても異なるが、数十秒くらい)積分し続けると、ずれが生じてくるので、どこかで積分をリセットする必要があります。 | ||
| + | |||
| + | |||
| + | ===== ジャイロを使った制御 ===== | ||
| + | ジャイロから得た角速度、角度を使って、実際に制御を行います。 | ||
| + | 角度をP、角速度をD、角度の積分をIとして普通のPID制御を行うと、綺麗に制御できます。 | ||
