e-gadgetやTJ3で地磁気センサHMC5883Lを使う方法2/3(キャリブレーション)

ロボカップジュニアで良く使われているダイセン電子工業のe-gadgetやTJ3でハネウエルの地磁気センサモジュールHMC5883Lを使えるようにしましたので紹介します。PICを始めて使ったので間違い等があるかもしれません。ご使用される場合は自己責任でお願いします。試合に負けたり、損害や被害があった場合など、私は保証できません。

次の3ステップが必要です。
1. セルフテスト
2. キャリブレーション
3. 実際に使用する

まず、ここではHMC5883Lのキャリブレーションの方法を説明します。以下のget_dir_cal関数をc:\Daisen\C-Style for e-Gadget\Build\Build_V120911\D_I2C.cのUINT get_dir(BYTE dno)と置き換えて保存してください。もとのget_dir関数は消さないでget_dir_org()などと名前を変えると良いでしょう。


なお、ここでのキャリブレーションは、地磁気センサx, y軸のセンサ値をブロットしてできる楕円の中心をオフセットしています。具体的にはx, y軸とも(最大値+最小値)/2としています。本来はノイズなどがあるので中間値(メディアン)を取る方が望ましいのですが、PICで大きな配列を簡単に使えなかったので、簡単な方法をとっています。また、楕円のままだと、方位の精度にばらつきがでるので円に変換すると更に良くなると思います。

また、数学関数atan2を使っているので以下をこのファイル(D_I2C.c)の28行目#include “D_I2C.hの下に次の1行を追加してください。
#include “../../../mcc18_v337/h/math.h”

後は、C-styleを使いビルドし、ロボットへダウンロードして、STARTボタンを動かすとロボットが2回程度回転し、オフセット値となる地磁気センサx, y軸の値の平均をEEPROMに書き込み、キャリブレーションが終わると成功するとSUCCCEEDEDと表示され、失敗するとFAILEDと表示され、続いてPOWER OFFと表示されるので電源を切ってください。

次回は実際にセンサを使ってみましょう。


// HMC5883L用のキャリブレーション関数 by でむ UINT get_dir_cal(BYTE dno)
// 実行するときはこの関数名をget_dirに変更し、ビルドして生成されて実行ファイル(Hex)
// を自分の好きな名前に例えばHMC5883L.Hexに変更して保存する。
// C-styleのダウンロードボタンを押し、HMC5883L.Hexを選び、ロボットにダウンロードして、
// 実行するれば。ロボット(e-gadget-TT, e-gadget-RB)はその場回転してキャリブレーションを
// 行いパラメータをEEPROMに保存して終了する。キャリブレーションが成功したときは
// LCD画面に"SUCCEEDED", 失敗したときは"FAILED"と表示して終了する。
// キャリブレーションは場所が変わる毎に1回実施すること。
// 制作者:でむ
// なお、関数readEE(), writeEE()は次の書籍のP473と同じなので省略しています。
// 標準的なEEPROMへの読み書きの関数です。詳細は以下の資料をご覧ください。
// 改定版 電子工作のためのPIC18F 本格活用ガイド
// 後閉哲也著、技術評論社 ISBN978-4-7741-3449-9
UINT get_dir_cal(BYTE dno) // キャリブレーション HMC5883L用 get_dir_cal
{
	int speed = 8; // ロボットのスピード。適宜変更すること。
	int steps = 300; // キャリプレーションのループ数。上と合わせて適宜変更すること。
	int dir, i, data_no;
	int x_min = 1000, x_max = -1000, y_min = 1000, y_max = -1000;
	int success = 1;

	U_UINT dx,dy,dz;
	// 地磁気センサをz軸を中心に360°回転させたときx, y軸は軌跡は楕円となる。
	// offset_x, offset_yは楕円の中心
	U_UINT offset_x, offset_y; 
	BYTE adrs_write  = 0x3C; // HMC5883L I2C address 8bit write 変更不可
	BYTE adrs_read   = 0x3D; // HMC5883L I2C address 8bit read 変更不可
	float ave_x, ave_y;
	static float counter = 0;

	if ((dno == 1) || (dno == 2)) return 999;

	dx.W = 999; dy.W = 999;
	offset_x.W = 999, offset_y.W = 999;

	if (counter == 0) {
		offset_x.H = readEE(0x00);
		offset_x.L = readEE(0x01);
		offset_y.H = readEE(0x02);
		offset_y.L = readEE(0x03);
		lcd_putX(1, (ROMC *) "READ OFFSET"); 
		myprintLCD2(2, offset_x.W, offset_y.W);
		wait_ms(3000); // wait 3s
	}

	motor(-speed, speed);      // その場回転

	// 初期化
	if (counter++ < 5) {
		gI2C_Buf[0] = 0x02; // モードレジスタ
		gI2C_Buf[1] = 0x00; // 連続計測モード
		if (i2c_send(adrs_write, 2) == false) return 999;
		//Delay100TCYx(480);  // 6ms待ち
		Delay1KTCYx(536);
		return 999;
	}

	set_Led(2,1);
	lcd_putX(1, (ROMC *) "CALIB. BEGIN"); 
    for (i = 0; i < steps; i++)
	{
		
		if (i2c_recv(adrs_read, 6) == false) {
			success = 0;
			break;
		}

		dx.H = gI2C_Buf[0]; // BYTE型、実はunsigned char. D_main.hで定義
		dx.L = gI2C_Buf[1];
		dz.H = gI2C_Buf[2];
		dz.L = gI2C_Buf[3];
		dy.H = gI2C_Buf[4];
		dy.L = gI2C_Buf[5];

		if (dx.W == 999) continue;
		if (dy.W == 999) continue;
		if ((int) dx.W < x_min) x_min = (int) dx.W;
		if ((int) dx.W > x_max) x_max = (int) dx.W;
		if ((int) dy.W < y_min) y_min = (int) dy.W;
		if ((int) dy.W > y_max) y_max = (int) dy.W;

		if ((offset_x.W == 999) || (offset_y.W == 999)) 
			dir = (int) (atan2((int)dy.W, (int)dx.W) * (180.0/PI));
		else 
			dir = (int) (atan2((int)dy.W - (int) offset_y.W, 
						       (int)dx.W - (int) offset_x.W) * (180.0/PI));
		
	
		if (dir <   0) dir += 360;
		if (dir > 360) dir -= 360;
	
		// LCDに値を表示。デバッグ用。
		myprintLCD(2, dir);
		// myprintLCD2(2, dx.W, dy.W);

		// ログを取る場合はコメントを外す。デバッグ用。
		// log_serial((int) dx.W, (int) dy.W);
	 
		gI2C_Buf[0] = 0x03; // 最初のデータが格納されているデータレジスタ03へ指す
		if (i2c_send(adrs_write, 1) == false) {
			success = 0;
			break;
		}

		// ログを取る場合はコメントを外す。デバッグ用。
		log_serial((int) dx.W, (int) dy.W);

		Delay1KTCYx(536*2); // 67×2ms待つ。HMC5883Lは標準で毎秒15回データ取得
	}
	motor(0, 0);

	ave_x = (x_min + x_max)/2;
	ave_y = (y_min + y_max)/2;

	set_Led(2,0);

	//myprintLCD2(2, ave_x, ave_y);
	// 最小値、最大値、平均の表示
	myprintLCD3(1, x_min, x_max, ave_x); // X軸成分
	myprintLCD3(2, y_min, y_max, ave_y); // Y軸成分

	wait_ms(1000);
	offset_x.W = ave_x;
	offset_y.W = ave_y;

	writeEE(0x00, offset_x.H);
 	Delay10KTCYx(40); // 50ms
	writeEE(0x01, offset_x.L);
	Delay10KTCYx(40); // 50ms
	writeEE(0x02, offset_y.H);
	Delay10KTCYx(40); // 50ms
	writeEE(0x03, offset_y.L);

	// 電源を切れとLCDに表示
	while (1) { // 無限ループ
		set_Led(2,1);
		lcd_putX(1, (ROMC *) "   CALIBRATION"); 
		if (success == 1) lcd_putX(2, (ROMC *) "*** SUCCEEDED ***");
 		else              lcd_putX(2, (ROMC *) "***   FAILED  ***");

		wait_ms(1000);
		set_Led(2,0);
		lcd_putX(1, (ROMC *)  "*** POWER OFF ***"); 
		myprintLCD2(2, (int) offset_x.W, (int) offset_y.W); 
		wait_ms(1000);
	}

	return (UINT) dir;
}
2 Comments
  1. 以前質問したきたみねです。北大のサークルのホームページに質問したところ my print という行を消しset up画面上でDir.sensorと16×2LCDをチェックすると
    ビルトでき、またtj3bにはLCDがついていないので替わりにLEDを使ってセルフテストまでは行えました。なのでキャリブレーションの際に出たエラーは次の通りです。C:\Daisen\C-Style for Tj3B\Build\Build_V130219>setlocal

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Mcu=18f2620

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Dst=\Daisen\mcc18_v337

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Src0=c018i

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Src1=D_Main

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Src2=D_I2c

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set Src3=D_Sio

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set SrcU=User

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set OBJ0=c018i.o

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set OBJ1=D_Main.o

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set OBJ2=D_I2c.o

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set OBJ3=D_Sio.o

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>set OBJU=User.o

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>mcc18 -p 18f2620 -I \Daisen\mcc18_v337\h;. c018i.c

    C:\Daisen\C-Style for Tj3B\Build\Build_V130219>mcc18 -p 18f2620 -I \Daisen\mcc18_v337\h;. D_Main.c
    よろしくおねがいします。

    • このエラーメッセージでは良くわかりません。全角空白でも入っていませんか?近いうちにTJ3Bを入手しますので、試してみます。

コメントを残す

メールアドレスが公開されることはありません。