*************************************************************************
//自律走行車制御スケッチ
//作成者　並木精司
//作成日　2013年4月4日


#include <Wire.h>//I2C通信ライブラリを取り込む
int compassAddress = 0x42 >> 1; // ディジタル・コンパス・モジュールのアドレス設定
const int MOTOR_LF = 5; // MOTOR_LEFT_FORWARDの駆動ポート5を定義
const int MOTOR_RF = 6; // MOTOR_RIGHT_FORWARDの駆動ポート6を定義
const int MOTOR_LB = 9; // MOTOR_LEFT_BACKの駆動ポート9を定義
const int MOTOR_RB = 10; // MOTOR_RIGHT_BACKの駆動ポート10を定義
const int DISTANCE = 0; // 距離センサからの入力アナログ・ポートをAN0に定義

 int reading = 0; // ディジタル・コンパスの読み値変数定義
 int dif = 0; // 初期値から見た偏差値変数定義
 int target = 0; // 方位初期値変数定義
 int turn = 0; // 方向転換フラグ変数定義
 int back = 0; // 前進後進フラグ変数定義　0=前進　1=後進

//初期設定
void setup() {
  pinMode(MOTOR_LF,OUTPUT); // ポート5を出力に設定
  pinMode(MOTOR_RF,OUTPUT); // ポート6を出力に設定
  pinMode(MOTOR_LB,OUTPUT); // ポート9を出力に設定
  pinMode(MOTOR_RB,OUTPUT); // ポート10を出力に設定
  pinMode(DISTANCE,INPUT); // ポートAN0を入力に設定
  
  Wire.begin();//I2C通信開始
  Serial.begin(9600); // シリアル通信開始通信スピード
  Wire.beginTransmission(compassAddress); // ディジタル・コンパスのスレーブ・アドレス転送
  Wire.write('G'); // RAM書き込み用コマンド
  Wire.write(0x74); // MODE CONTアドレス指定
  Wire.write(0x72); // 20Hz Continuous  Modeに設定
  Wire.endTransmission(); // I2Cnotameni no通信終了
  delay(300); // 処理時間300msec待つ
  target = compas_read(); // 方向初期値取得のために関数compas_readを呼び出し値をtargetに返す
  //Serial.println(target); // デバッグ用パラメータ出力
}

void loop() { // メイン・ループ
 int i; // 距離センサ戻り値
 int j; // 距離に応じたスピード設定値
 int k; // 方向偏差値
 int n; // 左モータPWM設定値
 int o; // 右モータPWM設定値
  i = dist_sens(); // 関数dist_sensを呼び出してその帰り値を変数iに設定
  //Serial.print(i); // デバッグ用パラメータ出力
  //Serial.print(","); // 区切りカンマを出力
  i = i - 25; // 対象との距離が約50cm以下の出力は無視するためにバイアスをセット
  if (i > 60 && turn == 0){  // 距離センサ出力が1.0V以下で直進中なら
    target = target + 45; // targetの設定値を45°増やす
    if (target > 360) target = target -360; // 角度が360°越えないようにする
    turn = 1; // 方向転換フラグを立てる
  }
  if (i > 100) back = 1; // 障害物あり後進に設定
  if (i < 55) back = 0; // 障害物から離れたら前進に設定
  k = direction_dif(); // 関数direction_difを呼び出してその戻り値を変数kに設定
  if (abs(k) < 10) turn = 0; // ターゲットの方向とカーレントの方向が 10°以内ならRESET
  if (i < 0) i = 0; // 値がマイナスにならないようにする
  j = 255 - i; // 障害までの距離に応じたPWM値を設定
  if (k > 0) { // 右に偏ったら
    n = j - k * 2; // MOTOR_LEFTのPWM値を偏り角度分マイナス設定
    o = j; // MOTOR_RIGHTはそのままのPWM値設定
  }
  else { // 左に偏ったら
  n = j; // MOTOR_LEFTのPWM値はそのままを設定
  o = j + k * 2; // MOTOR_RIGHTのPWM値を偏り値分マイナス設定
  }
  if (n <= 0) n = 0; // PWM設定値が零または零より小さいとき設定値を零に固定
  if (n >= 255) n = 255; // PWM設定値が255または255より大きいとき設定値を255に固定
  if (o <= 0) o = 0; // PWM設定値が零または零より小さいとき設定値を零に固定
  if (o >= 255) o = 255; // PWM設定値が255または255より大きいとき設定値を255に固定
  if (back == 1){ // 後進設定だったら
    analogWrite(MOTOR_LF,0); // ポート5にLOW出力
    analogWrite(MOTOR_RF,0); // ポート6にLOW出力
    analogWrite(MOTOR_LB,o); // ポー9にPWM出力
    analogWrite(MOTOR_RB,n); // ポート10にPWM出力
  } else {                   // 前進設定だったら
    analogWrite(MOTOR_LF,n); // ポート5にPWM出力
    analogWrite(MOTOR_RF,o); // ポート6にPWM出力
    analogWrite(MOTOR_LB,0); // ポー9にLOW出力
    analogWrite(MOTOR_RB,0); // ポート10にLOW出力
  }
  // この部分はデバッグ用にパラメータをターミナルに送ってモニタするため
  // Serial.print(reading); // 方向DATAをシリアル通信で出力
  // Serial.print(","); // 区切りカンマを出力
  // Serial.print(y);   // 方向の偏差DATAをシリアル通信で出力
  // Serial.print(","); // 区切りのカンマを出力
  // Serial.print(n);  // MOTOR_LIGHTのPWM設定値をシリアル通信で出力
  // Serial.print(","); // 区切りカンマを出力
  // Serial.print(o); // MOTOR_LEFTPWM設定値をシリアル通信で出力
  // Serial.print(","); // 区切りカンマを出力
  // Serial.print(target);
  // Serial.print(","); // 区切りカンマを出力
  // Serial.println(turn);
  delay(1); // 1msの遅れ設定
}


int dist_sens() { // 距離センサ出力のアナログ値を読み出す関数
  int val = 0; // 変数valを整数型宣言
  int dval = 0; // 変数dvalを整数型宣言
  int z = 0; // ループ変数
   for (z = 0; z < 5; z++){ // 5回読み込み
  val =val +  analogRead(DISTANCE); // AN0ポートから距離DATAを読み込み・・・・
  }
  dval = val / 5; // 5個のデータの平均値を求める
  dval = dval / 4; // アナログ値を10ビットから8ビットに変換
  return dval;
}


int compas_read(){ // ディジタル・コンパスの方位DATAを取得する関数  
  Wire.requestFrom(compassAddress, 2); // デバイスに2バイト分のデータを要求する
  if(Wire.available()>1){ // 要求したデータが2バイト分来たら
    reading = Wire.read(); // 1バイト分のデータの読み込み 
    reading = reading << 8; // 読み込んだデータを8ビット左シフトし
    reading += Wire.read(); // 次の1バイト分のデータを読み込み一つ目のデータと合成する
    reading /= 10; // 2バイト分のデータを10で割って0-359の方位データにする
    return (reading);
  }
}

int direction_dif(){ // ターゲットの方向と現在の方向の偏差を計算する関数
  int x;
  int w;
  reading = compas_read(); // ディジタル・コンパスから現在の方向DATAを読み込む
    // 360度から0度への不連続を補正して方位の偏差を計算
    if (target > reading){
      w = target -reading;
    } else {
      w = reading - target;
    }
    if (target >180){
      if (w > 180){ // 初期値から180度以上の場合
        x = reading + 360 - target; // に初期値に360度をプラスして偏差を計算
      } else {
        x = reading - target; // 180度以下の場合はそのまま計算
      }
    }else{
       if (w > 180){ // 初期値から180度以上の場合
        x = reading - 360 - target; // に初期値に360度をマイナスして偏差を計算
       } else {
         x = reading - target; // 180度以下の場合はそのまま計算 
       }
    }
   return(x);
}
**********************************************************************