色々なセンサーを使う



　ここにある全てのプログラムは完全には検証しておりませんので、正しく動かない場合もあります。その時は自分で考えて解決してください。また、バグもあるかもしれません、発見した場合には教えてください。
printfなどのライブラリとして、これを用いてください。

センサーなどを壊す原因
  1. センサーとCPUの電源電圧が異なる。
  2. センサーの出力にCPUの出力を繋げる。あるいは出力同士を繋げた。
  3. 半田付けが下手くそで隣のピンとショートした。
  4. 電源を入れたまま金属、あるいは鉛筆で回路を触る。（鉛筆の芯は電気を良く通す）
  5. センサーあるいはCPUのポートの端子を電源あるいはグランドにショートさせる。
  6. あるいはどこかおかしな電圧に接触させ過大電流を流して壊す。
  7. 超音波レンジファインダーおよびデジタルコンパスのように入出力ピンを共有させて、間違ったプログラミングを行うと上記のような状況が生じて壊れる。
  8. CPUのグランド(0V)とセンサーのグランドは同じゼロボルトだから、同電位になるように繋がなければならないが、異なった場合、異常な電流が流れて壊れる。

　電源を入れることは壊すことに等しいと思いながらスイッチを入れてください。すばやく動作チェックを行い、正常に動作していなければ直ちにスイッチを切ってください。ICなどが熱くなっていたりすると壊れた可能性があります。熱くなっていなくても壊れていることはあります。壊れたか壊れていないかを判定できるようにセンサーのハードウェアの知識及びソフトウェアの理解を深めておいてください。
　プログラムで入出力方向を間違えてはいけない。正しく動作していることをオシロスコープで必ず確認すること。確認の手を抜くと1秒に10μ秒だけ異常が起こると、一見分かりません。この場合はジワジワと壊れていくかもしれません。何もしないのに壊れたというのはこれが原因かもしれません。
すべて自分の目でチェックしてください。ここのページにあるプログラムも疑いの目で見てください。偶然動いているだけのプログラムかもしれません。
  1. はじめは、壊れにくい超音波レンジファインダーです。
  2. カラーセンサー
  3. 加速度センサモジュール
  4. デジタルコンパス

超音波レンジファインダー


通信プロトコル（計測の方法）

  1. 用いるIOビットを出力に設定する。
  2. 1を出力する。
  3. 約5μ秒1を維持する。
  4. 0を出力する。
  5. そのビットを入力に変更する。
  6. 1になるまで待つ。待つが、最大約18.5msまでしか待たない。
  7. 0になるまでカウントをする。0になるまで待つが、最大約18.5msまでしか待たない。
  8. カウントした数が距離に比例する。
  9. 6,7でタイムアウトの時は1に戻るか、エラーを知らせる。


使用するセンサーの電源は５Vです。コントロールするCPUの電源は3.3Vなので直接繋げると、CPUあるいはセンサー、あるいは両方壊れる可能性がありますが、このセンサーは、入出力とも3.3VのCPUに繋げる事が出来る特殊なポート構造になっています。ですからそのままつなげることが出来ます。しかし、電源は5Vでないと動作しません。
　他にも5Vを使うので、００６Ｐ型の電池から三端子レギュレータを用いて５Vに落とします。基板上にこの回路を作っても良いのですが、バッテリースナップ（電池スナップ）プラスチック製 の蓋を取り、そこに組み込み、グルースティックにて封止してみました。


距離を計測する関数の例です。
ping.c 
/************************************************/
/* PORT7 bit0に超音波レンジファインダーを繋げる */
/************************************************/
int ping(void)
{
  int i,count;

  IO.PCR7 = 1;   // 出力に設定
  IO.PDR7.BIT.B0=1;
  for(i=0; i<11; i++) ;  // 5.28μs パルス
  IO.PDR7.BIT.B0=0;
  IO.PCR7 = 0;   // 入力に設定
  count = 0;
  while(!IO.PDR7.BIT.B0  && (count++<15000));  // パルス開始を待つ
  if(count <= 15000) {                         // 18.24msで断念
    count = 0;
    while(IO.PDR7.BIT.B0  && (count++<15000));  // パルス終了を待つ
  }
  IO.PCR7 = 1;   // 出力に設定
  IO.PDR7.BIT.B0=0;
  return count;
}


注意:
入出力を行うにはst_io構造体を用います。この構造体は共用体とビットフィールドで定義されています。ビットフィールドについてはデジタルコンパスの部分で簡単に説明してありますが、その他も含めてちゃんとしたＣ言語の本（授業で使っているものは入門なのであまり書いてありません）を各自読んで理解してください。
　また、Ｃ言語を知っていてもCPUの入出力の設定方法、動作などが理解されていないとプログラムは作れません。使っているCPUのマニュアルは全部で404ページもありますが、全部を読む必要はありません。重要なところだけ読むだけである程度使えます。ここで使うポート7についての記述は、PDFのページで134ページにあります。ここからポートコントロールレジスタ(PCR7)、ポートデータレジスタ(PDR7)の説明がありますからこれを読んで理解してください。
　PCR7にアクセスするにはst_io構造体の変数IOを利用して、IO.PCR7となります。PCR7は書き込みのみで、読めないレジスタで、バイト単位のアクセスであることに注意してください。また、PDR7はビットフィールドで定義されているのでビット単位で読み書きが出来ることに注意してください。PDR7のビット0を1にするには、上記のプログラムのように
IO.PDR7.BIT.B0=1;
とします。
　約5μ秒の時間を作るには空ループのfor文を用います。実測値で11回空回りさせると5.28μsでした。

　ニ階へ行く通路にはラインがないので、これを右側または左側につけて壁からの距離を測りながら前進するようにプログラムします。

カラーセンサーの組み立て

デジタルカラーセンサＳ９７０６ (マニュアル)は非常に小さいので、シール基板を通常の基板に張り付け、そこに半田付けして固定します。また、使うときには下向きにするので入射光が少ないので白色LEDで照らします。ここで用いているLEDは非常に小さいですが、結構な光度です。LED用の抵抗は100Ωを用いています。
LED及び抵抗は瞬間接着剤で張り付けています。抵抗は半田付けしにくいので1/6Wの抵抗を用いた方が楽だと思います。
  1. 以下のような大きさの基板とシール基板(サンハヤトICB-060)を用意します。

  2. シール基板はカラーセンサーの大きさにあわないので、半分に切ります。

  3. 半分に切ったシール基板を普通の基板に貼り付けます。

  4. シール基板上にカラーセンサーを半田付けします。

  5. 丸ピンＩＣソケット・両端オスピンを４ピンずつ用意します。

  6. 足の付け根が三角のほうがソケットなどに入るほうなので、平らのほうを基板側に挿入して半田付けします。

  7. つぎにこれをチェックするためのソケットを作り、配線します。

  8. プログラムを作り、動作チェック後にLEDを取り付けます。LEDをセンサーのすぐ近くに配置するとLEDからの反射光ではなく、直接入ってしまいますので少し離しましょう。あるいは衝立でもつけて隔離しましょう。

通信プロトコル（計測の方法）



プログラム
ここでは、CN9に接続した例を紹介します。Range(4pin)は高感度とするために、Vddに接続しておきます。Gate(6pin)はP24、Dout(1pin)はP75、CK(5pin)は、P20に繋ぎます。P75は入力、それ以外(P20, P24)は出力にプログラムしておきます。
color_sensor.c 
// 関数の値として明るさを返す。配列でRGBの割合を%で返す。
unsigned getRGB(unsigned int RGB[])
{
  unsigned int i, j, coldata, intensity=0; 

  IO.PDR2.BIT.B4=1;    // Gate=1
  Wait(10);            // 10ms 積算時間(不正確)
  IO.PDR2.BIT.B4=0;    // Gate=0
  for(i=0; i<3; i++) {
    coldata=0;
    for(j=0; j<12; j++) {
      IO.PDR2.BIT.B0=1;    // CK=1  パルス幅このままで1.5μs
      coldata>>=1;
      if(IO.PDR7.BIT.B5) coldata|=0x800; 
      IO.PDR2.BIT.B0=0;    // CK=0
    }
    RGB[i]=coldata;
    intensity += coldata;
  }
  for(i=0; i<3; i++) RGB[i] = RGB[i]*100/intensity;
  return intensity;
}


説明
　この関数を呼び元には大きさ3のunsigned int型の配列を作っておく必要があります。関数を呼ぶにはその配列名を引数にします。また、明るさを関数値として返しますので以下のようになります。
unsigned int rgb[3], light;
light = getRGB(rgb);
　プログラム中Wait関数は不正確なので後で出てくるデジタルコンパスのプログラム中にあるdelay関数を利用した方が良いかもしれません。

加速度センサモジュール(KXM52-1050)

説明書に間違いがあるので以下が正しい説明です。 
1: Vdd (2.7～5.5Vを供給する）
2: PSD（パワーシャットダウン。Vddに接続で通常動作。GNDか無接続でシャットダウン）
3: GND（グラウンドへ接続）
4: Parity（内部EEPROMのパリティチェック用。通常は無接続）
5: SelfTest（通常はGNDに接続。Vddにつなぐと出力が1G増える）
6: OutX（X軸のアナログ出力）
7: OutY（Y軸のアナログ出力）
8: OutZ（Z軸のアナログ出力）
電源は入出力の電圧の違いが出ないようにCPUと同じ3.3Vを使用します。
これはアナログ出力なのでそのままADコンバータの値を読む関数(AdRead(ch))を呼べばプログラムは簡単にできます。
CPUボードのCN10に4～7チャンネルの入力端子が出ています。 
CN10　チャンネル
1: PB7　7
2: PB6　6
3: PB5　5
4: PB4　4
なお、初期のvs-wrc003.cのAdRead関数にはバグか故意かは分かりませんが、PB7とPB6に接続しても計測できません。
if(ch<0x06)
となっているところを
if(ch<0x08)
に変更してください。なお、このページの一番上からダウンロードできるライブラリは変更済みです。
　現実的には前後方向の傾きが分かれば良いのでＸ軸あるいはＺ軸のみの値を読むだけで判断できます。もし角度を知りたい場合には、下のデジタルコンパスで使用するIntegerMath.cを利用します。

坂に入って傾いたときにラインセンサー動作から超音波レンジファインダー動作に切り替え、 また、上に到達した時をこれで感知して次の動作に切り替えるために用います。

デジタルコンパス(HM55BCompass)

HM55BCompassモジュールの電源は５Vです。しかし、CPUボードの動作電圧は3.3Vですので直接繋ぐのは危険です。このHM55BCompassモジュールを眺めてみると基板上で５Ｖを3.3Vに変換しているのが分かります。従って、内部回路(マニュアル)は3.3Vで動作しているので、この変換部分を飛び越してあげれば、このモジュールを3.3V動作として用いることが出来ます。これを行うには、図のようにVdd(6pin)から直接３端子レギュレータの出力に結線します。(写真では白い線)
　また、配線を簡略化するためにDin(1pin)とDout(2pin)をモジュール上で繋いでしまっています。このようにしても問題はありませんが、プログラミングを間違えるとセンサーあるいはCPUを壊します。



通信プロトコル



コマンド



ここでは、CPUのポート8を用い、デジタルコンパスのDinDoutをbit5, Clkをbit6, Enをbit7に接続してテストしてみます。
mainプログラムを含むソースプログラム(compass.c)は以下のようになります。
compass.c 
#include "vs-wrc003.h"
#include "common.h"

void startInit(char* str);

void main(void)
{
  startInit("************ デジタルコンパスのテスト ****************\n");
  SetCompassHM55B(port8, 5, 6, 7);
  CompassStart(); // Start a measurement
  while(true) {
    
    if( CompassPoll()) {
      printf("%3d\n", getCompassAngle());
      Wait(100);
      CompassStart(); // Start a measurement
    }
  }
}

void startInit(char* str)
{
  const BYTE MainCycle = 100;
  Init((BYTE)MainCycle);  //CPUの初期設定
  InitSci3(CBR_115200,non,1);
  Mtr_Run(0, 0, 0, 0);
  while(!getSW() && !inkey());
  printf(str);
}


共通に使われるヘッダ(common.h)は次のようになります。
#ifndef _COMMON
#define _COMMON
の二行は最後の#endifに対応します。
#ifndef _COMMON　は今までに_COMMONが#defineで定義されていなかった場合に#endifまでコンパイルを行うという意味です。様々なヘッダは色々なCのソースファイルから参照(#includeで)されたり、他のヘッダファイルから参照される場合があり、二重定義になり、特に異なった定義になるとバグの原因になったりします。これを避けるためにその固有の部分の範囲に名前を付けます。ここでは_COMMONですが、通常、普通に使わないような単語にするために、大文字にして先頭に_を一つかそれ以上付けて命名します。#ifndef _COMMONはそれをチェックしており、#define _COMMONで定義して再度読み込まれて二重定義がなされないようにします。ちなみに、未定義にするには#undefというのがあります。
真ん中より下のあたりに同様に#ifndefがあります。これは、vs-wrc003.hを読み込む前にコンパイラがCのソースファイルを読んでしまってコンパイルエラーが起こるのを防いでいます。vs-wrc003.hの先頭に
#ifndef _VSWRC003_H
#define _VSWRC003_H
があるのを確認してみましょう。
#ifndefが入れ子構造になっているのに注意してください。
common.h 
#ifndef _COMMON
#define _COMMON

#define null -1
#define false 0
#define true 1

#define port1  0xFFD4
#define port2  0xFFD5
#define port3  0xFFD6
#define port5  0xFFD8
#define port6  0xFFD9
#define port7  0xFFDA
#define port8  0xFFDB
#define portB  0xFFDD

#ifndef _VSWRC003_H
typedef unsigned char UBYTE;
typedef unsigned char BYTE;
typedef signed short SWORD;
typedef unsigned short UWORD;
typedef signed int SINT;
typedef unsigned int UINT;
typedef signed long SDWORD;
typedef unsigned long UDWORD;
typedef unsigned char bool;
#endif

#endif


次はデジタルコンパスの関数群(CompassHM55B.c)です。その前に、理解するための予備知識です。

ビットフィールド
struct ioBit{                   //  Bit  Access 
  unsigned char B7:1;           //    Bit 7    
  unsigned char B6:1;           //    Bit 6    
  unsigned char B5:1;           //    Bit 5    
  unsigned char B4:1;           //    Bit 4    
  unsigned char B3:1;           //    Bit 3    
  unsigned char B2:1;           //    Bit 2    
  unsigned char B1:1;           //    Bit 1    
  unsigned char B0:1;           //    Bit 0    
} ;
一般的には次のような形のものを言います。shortは16bitの変数で、その16bitの一つの変数の中に、それぞれ1,4,5,6bitの領域に名前をつけてアクセスできるようにする方法です。
struct tag {  
   short a:1;  // 1 ビットのフィールド  
   short b:4;  // 4 ビットのフィールド  
   short c:5;  // 5 ビットのフィールド  
   short d:6;  // 6 ビットのフィールド  
};
ioBitの構造体は8bitの領域をすべて1bitで分けていることになります。H8のようなマイクロコントローラの入出力は1つのアドレス8bitで、個々のbit単位で入出力が出来ます。これをＣ言語から使用可能にする方法がビットフィールドです。ここでは、使用しても意味ありませんが、ビット フィールドは、通常の整数と同じように、加減乗除やビット演算に使用できます。ただし、& 演算子でアドレスを取得したり、sizeof 演算子でサイズを取得することはできません。ちなみに、変数領域の下のbitから名前が割り当てられるか、上のbitからかはコンパイラへの指令で変更できます。


アクセス制限
static unsigned m_Port;
という文があります。これは大域変数（グローバル変数）ですが、staticが先頭に付くとこのソースファイルでのみの大域変数で、他のソースファイルからは参照できないようにしています。
const static unsigned short RESET = 0x0000;
は同様ですが、さらに値を変更されないように定数にしています
これらはオブジェクト指向言語(C++, java, C#)を意識した作り方になっています。コンストラクタというものはＣ言語には存在しませんが、コメントに入っていたりします。


volatile型
次のプログラムを見てください。このプログラムはportという変数をチェックしてbit1が立っていれば処理1を行うことを永久に実行する内容です。 
　int port;
　while (1) { 
　　if ((port&2) != 0) { 
　　　処理1　// portに対する代入はないものとする 
　　} 
　}
ところが、賢いコンパイラは（最適化レベルを高めると。普通これがデフォールトだったりする）、portの代入による変更がないので、効率が良くなるように、次のように変更してコンパイルします。普通の変数だったら結果の分かっている計算を何回もするという無駄な時間が費やされます。 
　int port; 
　if ((port&2) != 0) {
　　while (1) { 
　　　処理1 
　　} 
　}
従って、portのチェックは一回しか行わず、IOのポートの場合ではおかしなことになります。
volatile型はこのような最適化をさせない型で、組み込み用のプログラムには必須の型です。


デジタルコンパスの関数群(CompassHM55B.c)です。 後でC++化することを考えているのでちょっとややこしくなっています。
CompassHM55B.c 
#define SHIFT_MSB  -1
#define POST_CLOCK_MSB  1
#include "common.h"

void delay(unsigned wait);
int pulseIn(unsigned timeout, unsigned port, int bit, bool pinState);
unsigned char readPin(unsigned int port, int bit);
void setInput(unsigned int port, int bit);
void writePin(unsigned int port, int bit, bool value);
void writePort(unsigned int port, unsigned char value);
void shiftOut(unsigned dataPort, int dPin, unsigned clockPort, int cPin, int bitCount, unsigned short data);
int shiftIn(unsigned dataPort, int dPin, unsigned clockPort, int cPin, int bitCount); 
void pulseOut(unsigned length, unsigned port, int bit);
void setOutput(unsigned int port, int bit);
int readPort(unsigned int port);

void CompassReset();
int interpolation(const float x);

struct ioBit{                   //  Bit  Access 
  unsigned char B7:1;           //    Bit 7    
  unsigned char B6:1;           //    Bit 6    
  unsigned char B5:1;           //    Bit 5    
  unsigned char B4:1;           //    Bit 4    
  unsigned char B3:1;           //    Bit 3    
  unsigned char B2:1;           //    Bit 2    
  unsigned char B1:1;           //    Bit 1    
  unsigned char B0:1;           //    Bit 0    
} ;             
 
/**
 * この関数の使い方:
 *
 *  //　compassのピンの定義
 * const int compass_DinDout = 3;
 * const int compass_Clk = 0;
 * const int compass_En = 4;
 *
 *  // compass のデフォールトの設定をする
 *  SetDefaultCompassHM55B();
 *  // compassの詳細な設定をする
 *    SetCompassHM55B(port, compass_DinDout, compass_Clk, compass_En);
 *
 *  CompassStart()を呼んで計測を開始させる。
 *  次のメソッドがある。
 *  計測が終了したかどうかを調べるには、CompassPoll() を呼ぶ。
 *  trueがもどっときたら、getCompassX(), getCompassY とか getCompassAngle()を呼ぶ。
 *  getCompassAngle()は整数の角度が結果として戻る。
 *  次の計測のために再度CompassStart() 呼ぶ。
 *
 */

  const static unsigned short RESET   = 0x0000;
  const static unsigned short MEASURE = 0x0008;
  const static unsigned short REPORT  = 0x000c;
  const static unsigned short READY   = 0x000c;

  static unsigned m_Port;
  static int m_DinDout; //pin DinDout
  static int m_Clk;     //pin CLK
  static int m_En;      //pin /EN

  static int m_x;       //x-axis value
  static int m_y;       //y-axis value
  
  static BYTE PCR=0;    // 36064のPCRが馬鹿だから

  /**
   * コンストラクタ
   *
   * DinDout:  双方向通信に用いるピン番号
   * Clk 　 :  シリアルクロック出力に用いるピン番号
   * En 　　:  チップイネーブルに用いる出力ピン番号
   */

void SetDefaultCompassHM55B() {  // デフォールト　コンストラクタ
    PCR = 0;
    m_Port = port2;
    m_DinDout = 3;
    m_Clk = 0;
    m_En = 4;
    setOutput(m_Port, m_Clk);
    setOutput(m_Port, m_En);
    writePin(m_Port, m_Clk, false); //initialize clock pin to low output
    writePin(m_Port, m_En, true);   //initialize enable pin to high output
    CompassReset();
  }

void SetCompassHM55B(unsigned port, int DinDout, int Clk, int En) {
    PCR = 0;                      // 全ビット入力になっていることを仮定する
    m_Port = port;
    m_DinDout = DinDout;
    m_Clk = Clk;
    m_En = En;
    setOutput(port, Clk);
    setOutput(port, En);
    writePin(port, m_Clk, false); //initialize clock pin to low output
    writePin(port, m_En, true);   //initialize enable pin to high output
    CompassReset();
  }

  /**
   * Ｘ方向の値を得る
   *
   * 戻り:　X方向の値
   */
  int getCompassX() {
    return m_x;
  }

  /**
   * Ｙ方向の値を得る
   *
   * 戻り:　Ｙ方向の値
   */
  int getCompassY() {
    return m_y;
  }

  /**
   * 方位の角度を得る
   *
   * 戻り: 角度(0-359).
   */
  int getCompassAngle() {
    return atan2(-m_y, m_x);
    // return interpolation((float)atan2(-m_y, m_x));
  }

  /**
   * コンパスをリセットする
   */
  void CompassReset() {
    writePin(m_Port, m_En, false);
    shiftOut(m_Port, m_DinDout, m_Port, m_Clk, 4, RESET);
    writePin(m_Port, m_En, true);
  }

  /**
   * コンパスの計測を開始させる
   */
  void CompassStart() {
    writePin(m_Port, m_En, false);
    shiftOut(m_Port, m_DinDout, m_Port, m_Clk, 4, MEASURE);
 }

  /**
   * 計測終了を調べる
   *
   * 戻り: 計測終了の場合trueそうでない時false
   */
bool CompassPoll() {
    bool result;
    int m_status;
    writePin(m_Port, m_En, true);
    writePin(m_Port, m_En, false);
    shiftOut(m_Port, m_DinDout, m_Port, m_Clk, 4, REPORT);
    m_status = shiftIn(m_Port, m_DinDout, m_Port, m_Clk, 4);
    result = (m_status == READY);
    if (result) {
      m_x = shiftIn(m_Port, m_DinDout, m_Port, m_Clk, 11);
      m_y = shiftIn(m_Port, m_DinDout, m_Port, m_Clk, 11);
      writePin(m_Port, m_En, true);
      if ((m_x & 0x0400) != 0) m_x |= (short)0xF800;
      if ((m_y & 0x0400) != 0) m_y |= (short)0xF800;
    }
    return result;
}
  
void writePin(unsigned int port, int bit, bool value)
{

  switch(bit) {
    case 0: (*(volatile struct ioBit *)port).B0   = value; break;
    case 1: (*(volatile struct ioBit *)port).B1   = value; break;
    case 2: (*(volatile struct ioBit *)port).B2   = value; break;
    case 3: (*(volatile struct ioBit *)port).B3   = value; break;
    case 4: (*(volatile struct ioBit *)port).B4   = value; break;
    case 5: (*(volatile struct ioBit *)port).B5   = value; break;
    case 6: (*(volatile struct ioBit *)port).B6   = value; break;
    case 7: (*(volatile struct ioBit *)port).B7   = value; break;
  }

}

void shiftOut(unsigned dataPort, int dPin, unsigned clockPort, int cPin, 
                   int bitCount, unsigned short data)
{
  int i;
  int bits;

  setOutput(dataPort, dPin); // 直前に入力したかもしれないから

  // start()が実行されているとする。
 
  if(bitCount<=0 || 16<bitCount) return;

  bits = 1 << (bitCount-1);
  for(i=0; i<bitCount; i++, data<<=1) {
    if(data & bits) writePin(dataPort, dPin, true);
    else writePin(dataPort, dPin, false);
    delay(1);
    writePin(clockPort, cPin, true);   // clock high
    delay(1);
    writePin(clockPort, cPin, false);
  }
  delay(1);

}


int shiftIn(unsigned dataPort, int dPin, unsigned clockPort, int cPin, int bitCount)
{
  int i;
  int data=0;

  setOutput(clockPort, cPin);
  setInput(dataPort, dPin);
  if(bitCount<=0 || 256<bitCount) return 0;
  delay(1);
 // POST_CLOCK_MSB
  writePin(clockPort, cPin, 0);
  delay(1);
  for(i=0; i<bitCount; i++) {
    writePin(clockPort, cPin, 1);   // clock high
    delay(1);
    data<<=1;
    if(readPin(dataPort, dPin)) data |= 1;
    writePin(clockPort, cPin, 0);
    delay(1);
  }
  return data & 0xffff;
}

// 単位が約5μsのつもり
void delay(unsigned wait)
{ 
  int i;
  int n = wait * 2;
  for(i=0; i<n; i++);
}
unsigned char readPin(unsigned int port, int bit)
{

  unsigned char val = *(volatile unsigned char *)port;
  unsigned char Bit = 1 << bit;
  return val & Bit; 
}

// 他の機能で使われていても、勝手に出力に設定する
void setOutput(unsigned int port, int bit)
{
  PCR |= 1 << bit;
  *(volatile unsigned char *)(port + 0x10) = PCR;
}

// ほかの機能で使われていても、勝手に入力に設定する
void setInput(unsigned int port, int bit)
{
  int Bit = 1<<bit;
  PCR &= ~Bit;
  *(volatile unsigned char *)(port + 0x10) = PCR;
}

/**
 *  線形補間を用いて値を補正する
 *
 */ 
int interpolation(const float x){
  float ar[17]={0,36,59,84,100,126,145,158,172,194,211,228,253,280,306,337,360};
  int i;
  float y=0, z;
  for(i=1;i<16;i++){
    y = 22.5*i;
    if(y>=x) break;
  }
  if(i==16) y=360;
  z = y-22.5;
  // y=yi + (yi+1-yi)(x-xi)/(xi+1-xi) を行い値を返す
  z = ar[i-1]+(((ar[i]-ar[i-1])*(x-z))/(y-z));
  return (int)(z+0.5);
}


上記のソースファイルではatan2(アークタンジェント)を計算することになっていますが、ライブラリを用いるとコードが大きくなったり、実行時間がかかります。（通常はテーラー展開などを用います）ここではある程度の誤差を許して（そもそもセンサー自体にかなりの誤差がある）近似値で整数型の角度を返す関数にしてみました。ファイル名はIntegerMath.c
IntegerMath.c 
// filename: IntegerMath.c
typedef unsigned char bool;
#define false 0
#define true  1

static int abs(int x) {
  if (x == -32768) x = 0;
  return (x < 0) ? -x : x;
}
  /* x: (0-359)
   * 戻り値 truncated int(100*sin(angle))
   * 範囲　 -100 to +100
   */
  static int sign(int x) {
    return (x == 0) ? 0 : (x<0) ? -1 : 1;
  }
  static int cos(int angle) {
    int qop = angle;
    int div = 101;

    /* perform quadrant conversion */
    /* (input is 0 to 359) */

    if (angle > 90) {
      qop = 180 - angle;
      if (angle < 270) div = -101;
    }
    if (angle > 180) qop = angle - 180;
    if (angle > 270) qop = 360 - angle;

    /* quadratic fit for quadrant */
    return ((10188 - qop * (qop + 23)) / div);
  }
  static int sin(int angle) {
    return cos((angle+270)%360);
  }
  /**
   * arctan(y/x)
   *
   * y: y-value
   * x: x-value (0であってはいけない)
   */
  /* オーバーフローを防ぐため引数は1000以下にする */
  int atan2(int y, int x) {
    int ax;
    int ay;
    int aq=0;
    bool flip = false;

    // 第一象限で計算する
    ax = abs(x);
    ay = abs(y);

    // ax >= ay の場合で近似する
    if (ax < ay) {
      aq = ax;
      ax = ay;
      ay = aq;
      flip = true;
    }

    // 0の値の対処をする */
    if (ay != 0) {
      aq = (30 * ax) / ay;
      if (aq == 0) {
        aq = 90;
      }
      else {
        if (aq >= 82) {
          aq = 1720 / aq;
        }
        else {
          aq = 1720 / (aq + 8);
        }
      }
    }
    else {
      // atan2(0,0) は駄目
      //if (ax == 0) //aq = aq / 0;
      aq = 0;
    }

    // ひっくり返したものを元に戻す
    if (flip) aq = 90 - aq;

    // 正しい象限に戻す
    if (x < 0) {
      if (y < 0) {
        // 第三象限
        aq = 180 + aq;
      }
      else {
        // 第二象限
        aq = 180 - aq;
      }
    }
    else {
      if (y < 0) aq = 360 - aq;
    }

    // 0～359の間にする
    return (aq % 360);
}


キャリブレーション

デジタルコンパスはそのままではかなりの誤差を持っています。これを出来るだけ正しい値にするために補正を行います。
  1. 下に示すような16に分けた方位を作るかプリントしたものを用意してください。
  2. 磁石を使ってそのプリントを北に合わせてください。
  3. コンパスのテストをする机上にプリントを貼ってください。
  4. 磁石はテストを続ける前に離れた場所においてください。また、磁性体が近くに無いようにしてください。
  5. ０度マークのところを通っている破線にプリント基板の端をそろえることによってコンパスモジュールを真っ直ぐに置いてください。
  6. 22.5度ずつ反時計回りに測定します。（多分逆さでないとやりにくいから逆周り）
  7. 計測した値をinterpolation関数にあるar配列に設定します。
  8. getCompassAngle関数で線形補完するようにコメント部分を変更する。
  9. 再度計測して、誤差が大きかった場合、はじめからやり直す。



おまけ



ここの部分は、動作には全く無関係なので各自で行うかどうか考えてから行ってください。

もう少し考えてみよう。プログラムがスマートになる。
CompassHM55B.cのグローバル変数にPCRという変数があります。

static BYTE PCR=0; // 36064のPCRが馬鹿だから

「36064のPCRが馬鹿だから」というコメントの意味は、PCRが書き込みのみで、読み込みが出来ないのと、ビットアクセスができない二重苦だからです。CompassHM55B.cではこの変数を読むことで入出力の状態を知り新しくそのビットの入出力方向を変更することができるようにしています。何故こんな面倒な事をしているかといえば、PCRがバイトアクセスしかできない設定になっているからです。従って、簡単にコントロールするにはビットアクセスできるようにiodefine.hの設定を変えてしまえば良いということが分かります。
　これはシステムの設定の変更なのでちょっと勇気が要ります。ここでは、PCR1からPCR7までを変更してみます。これに伴って、vs-wrc003.cの修正が必要になります。これは各自行ってください。

CompassHM55B.cに関しては、PCRに関する行を全て削除します。

また、setOutputとsetInputの関数を以下のように変更します。

// 他の機能で使われていても、勝手に出力に設定する
void setOutput(unsigned int port, int bit)
{
  port += 0x10;
  writePin(port, bit, 1);
}

// ほかの機能で使われていても、勝手に入力に設定する
void setInput(unsigned int port, int bit)
{
  port += 0x10;
  writePin(port, bit, 0);
}


iodefine.hの一部を変更する 
             union {                                    /* PCR1         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char B7:1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char   :1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR1;                   /*              */
             union {                                    /* PCR2         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char   :1;           /*    Bit 7     */
                          unsigned char   :1;           /*    Bit 6     */
                          unsigned char   :1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char B3:1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR2;                   /*              */
             union {                                    /* PCR3         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char B7:1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char B3:1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR3;                   /*              */
             char               wk6;                    /*              */
             union {                                    /* PCR5         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char B7:1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char B3:1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR5;                   /*              */
             union {                                    /* PCR6         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char B7:1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char B3:1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR6;                   /*              */
             union {                                    /* PCR7         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char   :1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char B4:1;           /*    Bit 4     */
                          unsigned char   :1;           /*    Bit 3     */
                          unsigned char B2:1;           /*    Bit 2     */
                          unsigned char B1:1;           /*    Bit 1     */
                          unsigned char B0:1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR7;                   /*              */
             union {                                    /* PCR8         */
                   unsigned char BYTE;                  /*  Byte Access */
                   struct {                             /*  Bit  Access */
                          unsigned char B7:1;           /*    Bit 7     */
                          unsigned char B6:1;           /*    Bit 6     */
                          unsigned char B5:1;           /*    Bit 5     */
                          unsigned char   :1;           /*    Bit 4     */
                          unsigned char   :1;           /*    Bit 3     */
                          unsigned char   :1;           /*    Bit 2     */
                          unsigned char   :1;           /*    Bit 1     */
                          unsigned char   :1;           /*    Bit 0     */
                          }      BIT;                   /*              */
                   }            PCR8;                   /*              */