2013年12月31日火曜日

でむさんのHMC5883Lキャリブレーションを追試TJ3B

C-Style Ver.20130611でビルドしています。
オリジナルよりLCD関係を削除しました。コメントも一部変更させていたたきました。
成功すると緑のLEDが、失敗すると赤1のLEDが点灯するはずです。
また実行中は赤2が、終了すると赤3が点灯するはずです。
まだ、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ブログのプログラム等を参考
//にさせて頂きました。
//------------------------------------------------------------------------------
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);
}

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

// HMC5883L用のキャリブレーション関数 by でむ UINT get_dir_cal(BYTE dno)
// キャリブレーションは場所が変わる毎に1回実施すること。
// 制作者:でむ

#define PI 3.14159
UINT get_dir(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 = read_EE(0x00);
offset_x.L = read_EE(0x01);
offset_y.H = read_EE(0x02);
offset_y.L = read_EE(0x03);
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, LED_ON); //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;


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


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, LED_OFF);

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

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

while (1) { // 無限ループ
set_Led(2, LED_ON); //CALIBRATION
if (success == 1) set_Led(0, LED_ON); //SUCCEEDED
  else              set_Led(1, LED_ON); //FAILED

wait_ms(1000);
set_Led(2, LED_OFF); //CALIB. END
set_Led(3, LED_ON); //POWER OFF
wait_ms(1000);
}

return (UINT) dir;
}

追記
ビルドエラーは出ませんが、eepromの読み書きが正しく行われているかどうか自信がありません。
実物で確認していません。ごめんなさい。
read_EEとwrite_EEのデータの型とか当っているんでしょうか?。


0 件のコメント: