/** loop前後にstep計算式を追加 **/

float accel, interval, preInterval;
int stepcount,state,laststate=0;
float total,threshold,hysteresis=0;

void loop() {
  byte rc;
  float acc[3];
  rc = KX126.get_val(acc);

  for ( int i=1;i<=100;i++ ) {
   if (rc == 0) {
    // XYZ 軸の合成
    accel = sqrt( sq(acc[0]) + sq(acc[1]) + sq(acc[2]) );
    // 歩数カウントスタート
    if (i != 100) {
      total += accel;
    } else {
      threshold = total/i;
      hysteresis = threshold / 10;
      total = 0;
    }
    // 歩数カウントしきい値判定
    if ( accel > (threshold + hysteresis) ) {
      state = true;
    } else if ( accel < (threshold - hysteresis) ) {
      state = false;
    }
    // 歩数カウント
    if (laststate == false && state == true) {
      stepcount++;
      laststate = state;
    } else if (laststate == true && state == false) {
      laststate = state;
    }
   }
  }
  Serial.print("STEP: ");
  Serial.println(stepcount);
  Serial.println();
  delay(50);
}