小型の電動ヘリが出始めの頃、ジャイロセンサーは高級なイメージだったように思います。 ジャイロといえば、地球ゴマが一定の姿勢を保つ動きが連想されますね。（古い？）
現在は、携帯機器の普及で簡単に電子工作にも手軽に使えるパーツになりました。
ジャイロセンサーの利用は初めてでしたが、漠然と「角度の偏移が出力される」などと考えていたのですが、実際は「角速度」という馴染みのない物でした。
今回は、秋月電子のL3GD20 3軸ジャイロセンサーモジュールを使用しました。
ジャイロセンサー L3GD20
角速度と角度の関係について、詳しく解説している動画があります。 大変参考になったのですが難しい…。
私なりの理解は、角速度と角度の関係は、速度と距離の関係に似ているということでした。
一定時間単位で角速度を計測し、前回計測値からの移動量を積算していく。 上のグラフの青色の面積を求めれば近似値が得られるということですね。 簡略積分法です。
論理はともかくまずは実験ということでとりかかったのですが、これがなかなか大変でかなり時間を費やしました。
静止時の変動や振動などによるドリフト、値はなかなか安定しないのですが、先人たちの知恵と実験につぐ実験により、なんとか使えるコードが書けました。
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#define MOTOR1 22 // GPIO22
#define MOTOR2 23 // GPIO23
#define MOTOR3 24 // GPIO24
#define MOTOR4 25 // GPIO25
#define LEFT_STR "l" // 左旋回指示用文字
#define RIGHT_STR "r" // 右旋回指示用文字
int main(int argc, char **argv)
{
int i2c_fd; // I2Cデバイスファイル
int i2cAddress = 0x6a; // L3GD20のI2Cアドレス
int i,regH,regL,out_z,adj,backup,kai;
float data,deg;
// I2Cデバイスファイルをオープン
i2c_fd = wiringPiI2CSetup(i2cAddress);
// L3GD20 イニシャライズ
if((wiringPiI2CWriteReg8(i2c_fd,0x20,0x0f))<0){
printf("write error register 0x20");
}
printf("write register:0x20 = 0x0f\n");
// 旋回指定値の取得
kai = atoi(argv[2]);
// WiringPi イニシャライズ
if(wiringPiSetupGpio() == -1) return;
// モーター停止
digitalWrite(MOTOR1, 0);
digitalWrite(MOTOR2, 0);
digitalWrite(MOTOR3, 0);
digitalWrite(MOTOR4, 0);
// 静止時の補正値を計測
for(i=0; i<20; i++){
// デバイスからデータ取得
regL = wiringPiI2CReadReg8(i2c_fd,0x2c); // Lレジスタ読み出し
regH = wiringPiI2CReadReg8(i2c_fd,0x2d); // Hレジスタ読み出し
out_z = (regH<<24|regL<<16)>>16; // H,L合成
adj = adj + out_z;
usleep(10000);
}
adj = adj / 20; // 平均化し補正値とする
//左旋回動作
if ( strcmp( argv[1], LEFT_STR ) == 0 ){
digitalWrite(MOTOR1, 1);
digitalWrite(MOTOR4, 1);
}
//右旋回動作
if ( strcmp( argv[1], RIGHT_STR ) == 0 ){
digitalWrite(MOTOR2, 1);
digitalWrite(MOTOR3, 1);
}
// ジャイロデータを取得
for(i=0; i<10000; i++){
// デバイスからデータ取得
regL = wiringPiI2CReadReg8(i2c_fd,0x2c);
regH = wiringPiI2CReadReg8(i2c_fd,0x2d);
out_z = (regH<<24|regL<<16)>>16;
// データを補正し角度算出（台形法）
data += ((out_z - adj) + backup) / 2;
backup = out_z - adj;
deg = abs(data / 9700); // 9700は角度変換用係数
//printf(" 測定値 : %5.2f 角度 : %5.2f\n",data,deg);
//角度をチェックし、指定値に達していたら停止
if (deg > kai ){
//ブレーキ
digitalWrite(MOTOR1, 1);
digitalWrite(MOTOR4, 1);
digitalWrite(MOTOR2, 1);
digitalWrite(MOTOR3, 1);
usleep(100000);
//停止
digitalWrite(MOTOR1, 0);
digitalWrite(MOTOR4, 0);
digitalWrite(MOTOR2, 0);
digitalWrite(MOTOR3, 0);
return;
}
usleep(10000);
}
return;
}
上記コードをコンパイル
gcc gyro_turn.c -o gyro_turn -I/usr/local/include -L/usr/local/lib -lwiringPi
実行は
sudo ./gyro_turn r 90
と、することにより右に90度旋回します。（実際には停止距離を考慮し角度を決定）
若干、動作にばらつきもありますが、ほぼ納得できる結果が得られました。
関連記事
ジャイロセンサーの測定プロセスを優先する ジャイロセンサーを使った旋回制御動作の際、指定した回転角より多く回転する動きを見せることがあります。 ドリフトや振動などの影響もありますが […]
ハウスローバーのハードウェア仕様 ハウスローバーのブロック図です。
インターフェース基板には、モータードライバ、ジャイロセンサー、RTC […]
View Direction Rotation Control System ハウスローバーを無線LAN経由でコントロールする場合、webカメラの映像や命令には遅延が生じます。直進の場合は距離センサーのデータを参考に […]
ロボット探査車の製作 気ままに製作中のロボット探査車。 作業中は記録に残すことを忘れてしまうので、以前の実験結果などが残っていません。 ドキュメントは、必要です […]
Webカメラに超音波センサーを組み込む webカメラへパン・チルト機構を設けますが、超音波センサーや照明用LEDも同じ方向を向くような構成にしたいと思います。
購入前には計画し […]
2016年の製作計画 謹賀新年。
昨年の年頭にも開発計画を立て、8割位は実行できたと思います。 年末には駆け込みで新型シャーシの製作も行いました。 総括とし […]
