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;
}

でむさんの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のデータの型とか当っているんでしょうか?。


でむさんのHMC5883Lセルフテストを追試TJ3B

C-Style Ver.20130611でビルドしています。
オリジナルよりLCD関係を削除しました。コメントも一部変更させていたたきました。
合格なら緑のLEDが、不合格だと赤1のLEDが点灯するはずです。
またテスト中は赤2が、終了すると赤3が点灯するはずです。
まだ、HMC5883Lの製品を購入していません。どなたかお試しくださいませんか?
オプション Setupボタンの表示 AdvancedModeチェック Dir.Sensorチェック
適当にプログラムをビルドしてください。動かなかったらごめんなさい。ここまでしか
力がありません。
書き換えるのは、D_I2C.cだけてす。バックアップを忘れずに自己責任でどうぞ。
でむさん、ありがとうごさいます。

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 でむ
// 地磁気センサx, y軸の値が243以上、575以下なら合格
// 制作者:でむ
UINT get_dir(BYTE dno) // セルフテスト 
{
int init_dir, diff; // 初期角度, 差
int dir, i, data_no;
int x_min = 1000, x_max = -1000, y_min = 1000, y_max = -1000;
U_UINT dx,dy,dz;
BYTE adrs_write  = 0x3C; // I2C address 8bit write
BYTE adrs_read = 0x3D;   // I2C address 8bit read
float ave_x, ave_y;
static float counter = 0;

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

// 初期化
if (counter++ < 5) {
gI2C_Buf[0] = 0x00; // Write CRA(00)
gI2C_Buf[1] = 0x71; // Positive Self test measurement
if (i2c_send(adrs_write, 2) == false) return 999;

gI2C_Buf[0] = 0x01; // Write CRB(01)
gI2C_Buf[1] = 0xA0; // Gain 5
if (i2c_send(adrs_write, 2) == false) return 999;

gI2C_Buf[0] = 0x02; // Write Mode (02)
gI2C_Buf[1] = 0x00; // Continuous
if (i2c_send(adrs_write, 2) == false) return 999;

Delay1KTCYx(536);
return 999;
}

dx.W = 999; dy.W = 999;
set_Led(2, LED_ON); //SELF TEST BEGIN

    for (i = 0; i < 100; i++)
{
if (i2c_recv(adrs_read, 6) == false) return 999;

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;

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

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

set_Led(2, LED_OFF); //SELF TEST END
if (((243 < x_min) && (x_max < 575)) && ((243 < y_min) && (y_max < 575)))
set_Led(0, LED_ON); //SUCCEEDED
  else          
set_Led(1, LED_ON); //FAILED

wait_ms(10000);
set_Led(3, LED_ON); //POWER OFF
wait_ms(30000); // 30秒待つ

return (UINT) dir;
}

2013年12月15日日曜日

モジュールいろいろ3

Arduino UNO互換ボード   1セット ¥2,290(税込)
http://akizukidenshi.com/catalog/g/gM-07044/
ATMEGA328・ATmega16U2搭載のArduino UNO互換ボード

モータードライブ回路内蔵Arduino互換機  1枚 ¥3,450(税込)
http://akizukidenshi.com/catalog/g/gM-07028/
L298P DCモーターコントローラー内蔵

Arduino互換ボード  1枚 ¥3,990(税込)
http://akizukidenshi.com/catalog/g/gM-07045/
2chフルブリッジドライバIC“L298”を搭載

スクリューシールドボード  1枚 ¥750(税込)
http://akizukidenshi.com/catalog/g/gM-07039/

Arduino用プロトタイピングボード  1枚 ¥1,100(税込)
http://akizukidenshi.com/catalog/g/gM-07033/

LCD Keypad Shield For Arduino  1セット ¥1,390(税込)
http://akizukidenshi.com/catalog/g/gM-07029/

DFRduino Pro Mini  1個 ¥1,400(税込)
http://akizukidenshi.com/catalog/g/gM-06682/

勝手に数か所リンク8

Dagu Hi-Tech Electronic
http://www.aliexpress.com/store/929243

安いモーターコントローラーボード
http://www.aliexpress.com/store/product/DAGU-4-channel-controller-board-Rover-5-chassis-motor-contoller-acessorios/929243_1316054468.html

http://www.linuxcircle.com/2013/07/19/day-4-motor-controller/

2013国際ロボット展
http://www.nikkan.co.jp/eve/irex/

共同開発 1:レゴ用特注オムニホイール
http://www.tosadenshi.co.jp/cargo/goodslist.cgi?in_kate=40-5


ロボカップジュニア2014 日本大会のリアル大会の開催は無理か。

勝手なことを書いて申し訳ありません。すべて想像の産物です。


このまま、スポンサー無しと想像すると

1.ジュニア日本大会のリアル大会開催中止、サッカーBのみ審査で代表選定。
 世界大会参加費は全額自己負担。 

2.会場変更、参加費を値上げして、1/4の規模に縮小、1日開催、
 世界大会参加費は全額自己負担。 

ロボカップジュニア日本大会は今回をもって終了。
今後は、4年に1回の開催になる。次は、東京都オリンピックの年か。

これは、いやなので、大口スポンサーがほしいです。
OBや関係者に個人スポンサーをお願い出来ませんか。
私も方法が決まれば1万円ぐらいなら出しますよ。
1,000名分集めて今回だけでも全国大会が開催出来ればいいなと思います。
他の人の負担増は考慮していません。スミマセン。
寄付金募集のサイトとかにもジュニアは登録されていたようですし、
活用できると思います。


72ノード、21ブロックぐらいですかね。3000人。
開催費1000万としてノードあたり14万円の上納金が必要か。

ジャパンオープン2014の情報

http://www.robocupjunior.jp/
ロボカップジュニア・ジャパンオープン2014の開催に関する情報は、今しばらくお待ち下さい。
だそうです。

http://www.robocup2014.org/
世界大会のホームページでしょうか。

勝手に数か所リンク7

平成 23 年度ロボカップジュニアジャパン実行委員会会議 議事録http://www.robocupjunior.jp/jimu/soukai20120330.pdf

日本ロボット学会誌
http://www.rsj.or.jp/databox/journal/kaikoku/31/KAI3102.pdf
共催されていたんですね。

The history of "M&Y"   競技活動はこれで終了
http://chiku.blogzine.jp/weblog/2013/09/2013_2464.html#comments
長い間おつかれさまでした。