2013年12月31日火曜日

でむさんのHMC5883L方位取得関数を追試TJ3B

C-Style Ver.20130611でビルドしています。
オリジナルよりコメントを一部変更させていたたきました。
まだ、HMC5883Lの製品を購入していません。どなたかお試しくださいませんか?
オプション Setupボタンの表示 AdvancedModeチェック Dir.Sensorチェック
適当にプログラムをビルドしてください。動かなかったらごめんなさい。ここまでしか
力がありません。センサーモニターでも動作を確認できるようです。
書き換えるのは、D_I2C.cだけてす。バックアップを忘れずに自己責任でどうぞ。
もちろんキャリプレーションしてからじゃないと意味ありませんよ。
でむさん、ありがとうごさいます。

D_I2C.cの中の
#include "D_Main.h"
#include "D_I2C.h"
の次の行に
#include "C:\Daisen\mcc18_v337\h\math.h"
の一行を追加する。人によってファイルのある場所が違うかもしれないので
そのときはmath.hのある場所を自分で探して直してください。

D_I2C.cの中の
#ifdef ADV_SETUP
の前の行に次のプログラムを挿入してください。

//マニアの為の園芸教室様やsnafさん様のPIC18F2550ブログのプログラム等を参考
//にさせて頂きました。karabisaa
//------------------------------------------------------------------------------
char read_EE(unsigned char addr)
{
        EEADR = addr;
   EECON1bits.EEPGD = 0; // EEPROMポイント
   EECON1bits.CFGS = 0; // EEPROMアクセス
       EECON1bits.RD = 1;   // EEPROMを読み込む
       return EEDATA;     // 読み込んだデータを返す
}

void write_EE(unsigned char addr, char data)
{
        EEADR = addr;
       EEDATA = data;
   EECON1bits.EEPGD = 0; // EEPROMポイント
   EECON1bits.CFGS = 0; // EEPROMアクセス
   EECON1bits.WREN = 1; // EEPROMへの書き込みを許可する
   INTCONbits.GIE = 0; // 割り込みを禁止する
   EECON2 = 0x55;        // 
    EECON2 = 0xaa;        // 
   EECON1bits.WR = 1; // EEPROMへ書き込む
        INTCONbits.GIE = 1; // 割り込みを許可する
       EECON1bits.WREN = 0; // EEPROMへの書き込みを禁止する
}
//------------------------------------------------------------------------------

D_I2C.cの中の

// dno:0(dir), 1(pitch), 2(roll)
UINT get_dir(BYTE dno)
{
U_UINT d;
BYTE adrs = DIR_ADRS;
BYTE n = 0;

d.W = 999;
gI2C_Buf[n++] = 0x20 + dno * 2;
if (i2c_send(adrs, n) == false) return(d.W);
if (i2c_recv(adrs, 2) == false) return(d.W);
n = 0;
d.H = gI2C_Buf[n++];
d.L = gI2C_Buf[n++];
if (0 < dno){
d.W += 90;
}
return(d.W);
}

を次のプログラムに書き換えて上書きして保存してください。


// ハネウェル社の3軸デジタルコンパスモジュール HMC5883L用の方位取得関数
// コンパスモジュールのy軸がロボットの進行方向とし、地面と水平に取り付ける。
// dnoが0(dir)のときだけ方位を計算、ロール、ピッチは計算しないで999を返す。
// 作成者:でむ
#define PI 3.14159
UINT get_dir(BYTE dno) // get_dit_HMC5883L
{
int dir; // 方位
U_UINT d, dx,dy,dz; // 地磁気センサ x, y, z軸の値を記憶する変数
// 地磁気センサを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;   // HMC5993L I2C address 8bit read
static float counter = 0;  // この関数の呼び出される回数を数える変数

offset_x.W = 999, offset_y.W = 999; // 初期値 999はエラー

if ((dno == 1) || (dno == 2)) return 999; // ロール、ピッチは計算しません

// EEPROMから保存していたオフセット値を読み込む
if (counter == 0) {
offset_x.H = read_EE(0x00); // x軸の値
offset_x.L = read_EE(0x01); // x軸の値
offset_y.H = read_EE(0x02); // y軸の値
offset_y.L = read_EE(0x03); // y軸の値
}

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

if (i2c_recv(adrs_read, 6) == false) return 999; // I2Cからデータ6バイトの読み込み

dx.H = gI2C_Buf[0]; // BYTE型、実はunsigned char. D_main.hで定義
dx.L = gI2C_Buf[1]; // x軸の値
dz.H = gI2C_Buf[2]; // z軸の値 上位バイト
dz.L = gI2C_Buf[3]; // z軸の値 下位バイト
dy.H = gI2C_Buf[4]; // y軸の値 上位バイト
dy.L = gI2C_Buf[5]; // y軸の値 下位バイト

// 方位計算
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; // 角度を0°~359°にするための処理
if (dir > 360) dir -= 360; // 角度を0°~359°にするための処理

gI2C_Buf[0] = 0x03; // 最初のデータが格納されているデータレジスタ03へ指す
if (i2c_send(adrs_write, 1) == false) return 999; // 999はエラー

Delay1KTCYx(536); // 67ms待つ。HMC5883Lは標準で毎秒15回データ取得

return (UINT) dir;
}

2 件のコメント:

匿名 さんのコメント...

HMC588Lは、秋月電子通商で3000円で
売っていますが、Amazonでは、980円でした。

karabisaa さんのコメント...

コメントありがとうございます。
自分はまだ購入していないのですが、
使用例や感想をご公開されていらっしゃいましたら是非、ご紹介させてください。