/**
 * アナログ気圧計
 * 待機時はシステムをスタンバイさせることで消費電流を低減。
 * 電池駆動が可能。
 * 
 * MPL115A2の制御はgarretlabさん作成の関数を使用しています。
 *
 * system_sleep()とsetup_watchdog()関数は下記から頂きました
 * KHM 2008 / Lab3/  Martin Nawrath nawrath@khm.de
 * Kunsthochschule fuer Medien Koeln
 * Academy of Media Arts Cologne
 *
 * by ラジオペンチ http://radiopench.blog96.fc2.com/
 * 2013/12/21 初版作成
 * コンパイル後のスケッチサイズ:8322バイト
 */

#include <Wire.h>
#include <avr/sleep.h>
#include <avr/wdt.h>

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

volatile int wdt_cycle = 0;                    // WDT割り込みサイクルカウンタ

// 気圧計関係の変数
const int addresPsensor = 0x60;             // 気圧計のI2Cアドレス
float a0, b1, b2, c12, c11, c22;
float Pha;
unsigned int Padc;
unsigned int Tadc;
long lastPressValue;

boolean flag = false;               // モータ励磁方向フラグ
unsigned int cwPulse = 17;          // 順回転パルス幅 標準は17mS
unsigned int ccwPulse1 = 3500;      // 逆回転パルス幅1　単位はus
unsigned int ccwPulse2 = 12000;     // 逆回転パルス幅2　単位はus,値の上限は16383
int intervalX8 = 75;                // 表示インターバル（単位は8秒で指定）
int sleep_t = 9;                    // スリープ時間指定 9=8秒

void setup(){                 
  pinMode(7, INPUT);
  digitalWrite(7, HIGH);       // プルアップ
  pinMode(8, INPUT);           // 動作モード指定スイッチ
  digitalWrite(8, HIGH);       // プルアップ
  pinMode(9, OUTPUT);          // conect coil thrugh 220Ω
  pinMode(10, OUTPUT);         // coil return
  pinMode(13, OUTPUT);         // 動作表示LED

  if(digitalRead(7) == LOW){   // Pin7がGNDなら
    for(;;){                   // 無限ループで
      clockDemo();             // 時計の抵抗調整プログラム（リセットで脱出）
    } 
  }

  if(digitalRead(8) == LOW){     // 起動時にPin8のスイッチが押されていたら
    intervalX8 = 1;              // 動作確認用に、ループは1回
    sleep_t = 7;                 // 2秒の待ち時間で運転
  }
  digitalWrite(8, LOW);          // 消費電流低減のためプルアップ解除 

  Wire.begin();
  Serial.begin(9600);
  Serial.println("Balometer start");

  delay(1000);                         // 気圧計が立ち上がるまで待つ
  readCalConst();                      // 校正係数読み出し
  lastPressValue=Psens();              // 初期気圧測定

  Serial.print("initial value = ");
  Serial.println(lastPressValue);
  Serial.println("");
  delay(100);                          // シリアル出力完了まで待つ

  setup_watchdog(sleep_t);             // WDTを割り込みモードで起動（通常は8秒周期）
}    // setup

void loop(){                          // メインループ
  if(wdt_cycle == intervalX8){         // 設定時間になったら処理を行う
    wdt_cycle=0;
    long x = Psens();                  // 気圧測定　単位：0.01ｈPa
    moveTo(x);                         // 針を気圧の位置に動かす
  }
  system_sleep();                    // スリープに入りWDT割り込みで復帰  
}   // Loop

void readCalConst(){                          // 気圧計校正係数読み出し
  Wire.beginTransmission(addresPsensor);
  Wire.write(0x04);                           // Read coefficient data
  Wire.endTransmission();
  Wire.requestFrom(addresPsensor, 12);        // Request 12 bytes
  if (Wire.available()) {
    a0 = read_coefficients(16, 3, 0);
    b1 = read_coefficients(16, 13, 0);
    b2 = read_coefficients(16, 14, 0);
    c12 = read_coefficients(14, 13, 9);
    c11 = read_coefficients(11, 10, 11);
    c22 = read_coefficients(11, 10, 15);
  }
}  // readCalConst

long Psens(){                          // 気圧読み出し
  float sumP;
  long outP;          
  int n;
  sumP = 0;
  for(n=0; n<100; n++){               // バラツキが大きいので1000回アベレージング
    sensData();                       // 気圧計のデータを読む（本番）
    sumP = sumP + Pha;
    wdt_reset();                      // WDTをリセット
  }
  outP = sumP;                        // 0.01hPa単位の整数に変換
  return outP;
}   // Psens

void sensData () {                     // センサーから値を読み出す
  Wire.beginTransmission(addresPsensor);
  Wire.write(0x12);                    // Start both conversions(Pressure and Temperature)
  Wire.write(0x01);
  Wire.endTransmission();
  delay(3);

  Wire.beginTransmission(addresPsensor);
  Wire.write((uint8_t)0x00);           // Read pressure and temperature
  Wire.endTransmission();

  Wire.requestFrom(addresPsensor, 4);  // Request 4 bytes
  if(Wire.available()) {
    Padc = read_adc();
    Tadc = read_adc();
    int signedTadc = Tadc;
    float Pcomp = a0 + (b1 + c12 * Tadc) * Padc + (b2 * Tadc);   // c11 とc22はゼロなので計算省略（84バイト節約）
    Pha = Pcomp * 650 / 1023 + 500;             // 気圧の値をヘクトパスカルに換算
  }
  delay(1);
}  // sensData

float read_coefficients(int total_bits, int fractional_bits, int zero_pad) {
  unsigned char msb, lsb;
  msb = Wire.read();
  lsb = Wire.read();
  Serial.print("mbs = "); 
  Serial.print(msb); 
  Serial.print(", lsb = "); 
  Serial.println(lsb);
  return ((float) ((msb << 8) + lsb) / ((long)1 << (16 - total_bits + fractional_bits + zero_pad)));
}  // read_coefficient

unsigned int read_adc() {
  unsigned char msb, lsb;
  msb = Wire.read();
  lsb = Wire.read();
  return (((unsigned int)msb << 8) + lsb) >> 6;
}  // read_adc

void moveTo(long x){                   // 指定した気圧の位置に針を動かす
  static int lastMod;
  long y,p;
  y = x - lastPressValue;              // 気圧変化量　単位：0.01hPa
  p = (y * 6 + lastMod) / 10;          // ドライブパルス量は yの0.6倍
  lastMod = ( y * 6 + lastMod) % 10;   // 余りは次回の計算のために保存

  Serial.print(x);
  Serial.print(", ");
  Serial.print(y);
  Serial.print(", ");
  Serial.print(p);
  Serial.print(", ");
  Serial.println(lastMod);

  clockDrive(p);                       // 針を動かす 
  delay(20);                           // シリアルの送信が終わるまで待つ
  lastPressValue = x;                  // 気圧の値を保存
}  // moveTo

void clockDrive(int x){               // 引数の値だけ針を動かす（引数は正負）
  if( x != 0){
    if( x > 0){
      cw(x); 
    }
    else{
      x *= -1;
      ccw(x);
    }
  } 
}  // clockDrive

void cw(int n){                       // 指定パルスだけ順回転
  if(n !=0){
    for(int x = 1; x <=n; x++){
      cwP();
      delay(35);                      // 順回転速度設定(MAXは25程度）
    }
  }
}  //cw

void ccw(int n){                      // 指定パルスだけ逆回転
  if(n !=0){
    for(int x = 1; x <=n; x++){
      ccwP();
      delay(60);                      // 逆回転速度設定（MAXは50程度）
    }
  }
}  //ccw

void cwP(){                           // 順回転パルス発生
  digitalWrite(13,HIGH);              // LED flash
  wdt_reset();                        // ウォッチドッグタイマーリセット
  flag = ! flag;
  if (flag == true) {
    digitalWrite(9, HIGH);            // coil drive foward
    delay(cwPulse);                   // wait 
    digitalWrite(9, LOW);             // coil drive end
  }
  else {
    digitalWrite(10, HIGH);           // coil drive revers
    delay(cwPulse);                   // wait
    digitalWrite(10, LOW);            // coil drive end
  }
  digitalWrite(13,LOW);               // LED flash end
}  //cwP

void ccwP(){                          // 逆回転パルス発生
  digitalWrite(13,HIGH);              // LED flash
  wdt_reset();                        // ウォッチドッグタイマーリセット
  flag = ! flag;
  if (flag == true) {
    digitalWrite(9, HIGH);            // coil drive foward
    delayMicroseconds(ccwPulse1);     // wait 
    digitalWrite(9, LOW);             // coil drive end

    digitalWrite(10, HIGH);           // coil drive revers
    delayMicroseconds(ccwPulse2);     // wait 
    digitalWrite(10, LOW);            // coil drive end
  }
  else {
    digitalWrite(10, HIGH);           // coil drive revers
    delayMicroseconds(ccwPulse1);     // wait 
    digitalWrite(10, LOW);            // coil drive end

    digitalWrite(9, HIGH);            // coil drive foward
    delayMicroseconds(ccwPulse2);     // wait 
    digitalWrite(9, LOW);             // coil drive end
  }
  digitalWrite(13,LOW);               // LED flash end
}  //ccwP

void system_sleep() {                   // システム停止 
  cbi(ADCSRA,ADEN);                     // ADC 回路電源をOFF (ADC使って無くても120μA消費するため）
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);  // パワーダウンモード
  sleep_enable();
  sleep_mode();                         // ここでスリープに入る
  sleep_disable();                      // WDTがタイムアップしたらここから動作再開 
  sbi(ADCSRA,ADEN);                     // ADC ON
}  //system_sleep

// 割り込みモードでウォッチドッグタイマーを設定。引数による動作は時間は下記
// 0=16ms, 1=32ms,2=64ms,3=128ms,4=250ms,5=500ms
// 6=1sec,7=2sec, 8=4sec, 9=8sec
void setup_watchdog(int ii) {           // 
  byte bb;
  int ww;
  if (ii > 9 ) ii=9;
  bb=ii & 7;
  if (ii > 7) bb|= (1<<5);
  bb|= (1<<WDCE);
  ww=bb;
  MCUSR &= ~(1<<WDRF);
  // start timed sequence
  WDTCSR |= (1<<WDCE) | (1<<WDE);
  // set new watchdog timeout value
  WDTCSR = bb;
  WDTCSR |= _BV(WDIE);
}  //setup_watchdog

void setPowDownMode(){        // CPUをパワーダウンモードに設定
  // CPU Sleep Modes          参考にしたプログラムに入っていたがこのルーチンは使用しない
  // SM2 SM1 SM0 Sleep Mode
  // 0    0  0 Idle
  // 0    0  1 ADC Noise Reduction
  // 0    1  0 Power-down
  // 0    1  1 Power-save
  // 1    0  0 Reserved
  // 1    0  1 Reserved
  // 1    1  0 Standby(1)

  // パワーダウンモードへ設定
  cbi( SMCR,SE );      // sleep enable, power down mode
  cbi( SMCR,SM0 );     // power down mode
  sbi( SMCR,SM1 );     // power down mode
  cbi( SMCR,SM2 );     // power down mode
}  //  setPowDownMode

ISR(WDT_vect) {                     // WDTがタイムアップした時に実行される処理
  wdt_cycle++;                      // タイムアップ検出カウンタをインクリメント
}  

void clockDemo(){            // 時計の動作確認プログラム
 clockDrive(10);             // 正負に10パルス振らす
  delay(300);
  clockDrive(-10);
  delay(600);
  clockDrive(-10);
  delay(300);
 clockDrive(10); 
  delay(600);
}