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