File content as of revision 0:b9cab5bc1074:

#include "mbed.h"

AnalogIn gyro1(A4);
AnalogIn gyro2(A5);

/* 小数点以下4桁目を四捨五入 */
double round4(double x) {
  double y;
  y = double(int((x + 0.0005)*1000))/1000;
  return(y);
}

int main() {
  float g1_offset;              // オフセット[V]
  float g1_sgm = 0;                 // 積分値[deg]
  float g1_deg_s;
  float g1_deg;
  float g1_wk0 = 0, g1_wk1 = 0;
  float g1_avg = 0, g1_sum;
  int i,j;
  
  /* オフセットを計算 */
  g1_offset = 0;
  for(i=0; i<300; i++) {
    g1_offset = g1_offset + round4(gyro1.read()*3.3);
    wait(0.001);
  }
  g1_offset = g1_offset/300; // オフセット電圧[V]
  
  while(1) {
    /* 平均値を計算 */
    g1_sum = 0;
    j = 0;
    for(i=0; i<100; i++) {
      g1_sum = g1_sum + round4(gyro1.read()*3.3);
      wait(0.001);
    }
    g1_avg = g1_sum/100; // 平均電圧[V]
    
    /* 回転速度を計算 */
    g1_deg_s = (g1_avg - g1_offset)*1000/0.67;
    if(g1_deg_s > 90) {
      g1_deg_s = 90;
    }
    if(g1_deg_s < -90) {
      g1_deg_s = -90;
    }
    
    /* 積分値を計算 */
    g1_wk1 = g1_deg_s;
    if(g1_wk1 > 0) {
      g1_wk1 = g1_wk1*1.0; // 補正
    }
    if(g1_wk1 < 0) {
      g1_wk1 = g1_wk1*1.0; // 補正
    }
    g1_sgm = g1_sgm + (g1_wk0 + g1_wk1)*0.1/2; // 回転角度
    g1_wk0 = g1_wk1;
    
    if(g1_sgm > 180) {
      g1_sgm = 180;
    }
    else if(g1_sgm < -180) {
      g1_sgm = -180;
    }
    
    printf("%5.1f[deg/s]\t", g1_deg_s);
    printf("%5.1f[deg]\n", g1_sgm);
  }
}