M5Stack Core2 simpleWalker 歩数計

# Simple Walker　加速度センサーで歩数計
![IMG_4744.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/2060e286-23ab-f3c4-f558-ba56abc2f8cb.jpeg)
加速度センサーの３次元ベクトルの大きさで、カウントしています。腕の振りに同期して、カウントします。

https://youtube.com/shorts/AJiYlEkUYNM?si=2UQM_LZsn0B5ynYg


![IMG_4745.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/864b2a46-8117-f8e7-2c84-10f6b0b88db8.jpeg)
3Dプリンターで台座をつくり、電線をまとめるケーブルバンドを短くして腕に巻き付けるようにしました。

:::note info
腕の振りに同期させて、一振りで一カウントさせています。
1. 前方に振り上げた腕の位置が、スタート位置。
2. 後方に振り下げた限界点の位置
3. 前方のスタート位置にもどる。ここで、歩数をカウント。

１から３への状態変異をカウンタで捉えています。
:::

※歩数カウンタは、メインスレッドと、Ticker割込みで使用しているので、セマフォのミューテックスで、保護しています。

# 時計歩数計SimpleWalkerコード
```CPP:加速度センサークラス myMPU6886.hpp
#ifndef __MYMPU6886_HPP_
#define __MYMPU6886_HPP_
#include <M5Core2.h>

//加速度、角速度　ADC値
typedef struct{
    int16_t x;
    int16_t y;
    int16_t z;
}_acc_gyro_ADCdata;

typedef struct
{
    float x;
    float y;
    float z;
}_xyzfloat;

//加速度、角速度
typedef struct{
    float x;
    float y;
    float z;
    _xyzfloat offset;
    _xyzfloat angle;
}_acc_gyro_type;

//センサー姿勢(degree)
typedef struct{
    float pitch;
    float roll;
    float yaw;
}_senserAttitude;

class myMPU6886
{
    public:
        _acc_gyro_ADCdata accADC;
        _acc_gyro_ADCdata gyroADC;
        _acc_gyro_type acc;
        _acc_gyro_type gyro;
        _senserAttitude attitude;
        _xyzfloat angle;
        float accVectorSize;
        uint8_t read_buffer[30];

    public:
        void calibration();
        void GetAccGyroADC();
        void sendPlotter();
        void sendUnity();
        void GetVectorScalarSize();
};

extern myMPU6886 mpu;

#endif
```
```CPP:加速度センサー myMPU6886.cpp
#include "myMPU6886.h"

myMPU6886 mpu;


/// @brief MPU6886キャリブレーション
void myMPU6886::calibration()
{
    int i;
    
    mpu.acc.offset.x=0;
    mpu.acc.offset.y=0;
    mpu.acc.offset.z=0;

    mpu.gyro.offset.x=0;
    mpu.gyro.offset.y=0;
    mpu.gyro.offset.z=0;

    for(i=0; i<1000; i++)
    {
        M5.IMU.getAccelData(&mpu.acc.x, &mpu.acc.y, &mpu.acc.z);
        mpu.acc.offset.x+=mpu.acc.x;
        mpu.acc.offset.y+=mpu.acc.y;
        mpu.acc.offset.z+=mpu.acc.z;

        M5.IMU.getGyroData(&mpu.gyro.x, &mpu.gyro.y, &mpu.gyro.z);
        mpu.gyro.offset.x+=mpu.gyro.x;
        mpu.gyro.offset.y+=mpu.gyro.y;
        mpu.gyro.offset.z+=mpu.gyro.z;

        if(i%100==0)
        {
            M5.Lcd.print(".");
        }
    }
    M5.Lcd.println("");

    mpu.acc.offset.x/=1000;
    mpu.acc.offset.y/=1000;
    mpu.acc.offset.z/=1000;

    mpu.gyro.offset.x/=1000;
    mpu.gyro.offset.y/=1000;
    mpu.gyro.offset.z/=1000;

    M5.Lcd.printf("acc :%.2f %.2f %.2f\n\r", mpu.acc.offset.x, mpu.acc.offset.y, mpu.acc.offset.z);
    M5.Lcd.printf("gyro:%.2f %.2f %.2f\n\r", mpu.gyro.offset.x, mpu.gyro.offset.y, mpu.gyro.offset.z);
}

/// @brief MPU6886 生データ16bits取得
void myMPU6886::GetAccGyroADC()
{
    uint8_t i=0;
     //加速度センサーADC値---------------------------------------------
    //I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_XOUT_H, 6, buf);
    Wire1.beginTransmission(MPU6886_ADDRESS);
    Wire1.write(MPU6886_ACCEL_XOUT_H);
    Wire1.endTransmission(false);
    Wire1.requestFrom(MPU6886_ADDRESS, 6);
    //! Put read results in the Rx buffer
    while (Wire1.available()) {
        mpu.read_buffer[i++] = Wire1.read();
    } 

    //ジャイロセンサーADC値---------------------------------------------
    //I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_XOUT_H, 6, buf);
    Wire1.beginTransmission(MPU6886_ADDRESS);
    Wire1.write(MPU6886_GYRO_XOUT_H);
    Wire1.endTransmission(false);
    Wire1.requestFrom(MPU6886_ADDRESS, 6);
    //! Put read results in the Rx buffer
    while (Wire1.available()) {
        mpu.read_buffer[i++] = Wire1.read();
    } 
}

/// @brief 加速度センサーのベクトルの大きさ取得
void myMPU6886::GetVectorScalarSize(void)
{
    accVectorSize = mpu.acc.x*mpu.acc.x + mpu.acc.y*mpu.acc.y + mpu.acc.z*mpu.acc.z;
    accVectorSize = sqrt(accVectorSize);
}

```
```CPP:タイマー割り込み(Ticker)クラス myInterrupt.hpp
#ifndef _myINTERRUPT_H_
#define _myINTERRUPT_H_

#include <M5Core2.h>
#include <Ticker.h>

extern SemaphoreHandle_t   semaphore;

/// @brief timer割り込み構造体
typedef struct{
        Ticker ticker;  //ESP32Tickerクラス
        bool flag;      //time up flag
        int Count;      //counter
}_myTickerInterrupt;

/// @brief タイマー割り込み構造体
extern _myTickerInterrupt Ticker1,Ticker2, Ticker3;

/// @brief 割り込み処理クラス
class myInterrupt
{
    public:
        myInterrupt();  //コンストラクタ
};

/// @brief Ticker2割り込み処理関数
void Ticker1ISR();
void Ticker2ISR();


#endif //_myINTERRUPT_H_
```
```CPP:タイマー割り込み(Ticker)クラス myInterrupt.cpp
myInterrupt::myInterrupt()
{
    Ticker1.flag=false;
    Ticker1.Count=0;
    Ticker2.flag=false;
    Ticker2.Count=0;
    //Ticker初期化
    Ticker1.ticker.attach_ms( 10,Ticker1ISR);
    Ticker2.ticker.attach_ms(50,Ticker2ISR);
}

/// @brief Ticker1割り込み関数
void Ticker1ISR()
{
    //Ticker1.flag=true;
    M5.IMU.getAccelData(&mpu.acc.x, &mpu.acc.y, &mpu.acc.z);
    mpu.GetVectorScalarSize();
    swk.rangeChecker(mpu.accVectorSize);
}


/// @brief Ticker2割り込み処理関数
void Ticker2ISR()
{
    Ticker2.Count++;
    if(Ticker2.Count==4)//50ms*x
    {
        Ticker2.Count=0;
        
        Ticker2.flag=true;
    }
}
```
```CPP:シリアル通信クラス mySerial.hpp
#ifndef __MYSERIALTASK_HPP_
#define __MYSERIALTASK_HPP_

#include <M5Core2.h>


extern TaskHandle_t taskHandle[2];
extern void mTask1(void *pvParameters);

class mySerialRXTask
{
    public:
        uint8_t  Buffer[100];
        uint8_t  length;
        bool     Completed;
    public:
        void start();
};

extern mySerialRXTask rxTask;

#endif
```
```CPP:シリアル通信クラス mySerial.cpp
#include <M5Core2.h>
#include "mySerialRXTask.hpp"

TaskHandle_t taskHandle[2];
mySerialRXTask rxTask;

void mySerialRXTask::start()
{
  BaseType_t ret =xTaskCreateUniversal(mTask1, "mTask1", 2000, NULL, 7, &taskHandle[0], APP_CPU_NUM);
  Serial.printf("task start Core1:%d\r",ret);
}

//mTask1()----------------------------------------------------------------------
void mTask1(void *pvParameters){
  uint8_t ch;
  while(1)
  {
    if(Serial.available()>0){
      ch=Serial.read();
      Serial.printf("%c",ch);
      rxTask.Buffer[rxTask.length]=ch;
      rxTask.length++;
      if(rxTask.length>100){
        rxTask.length=0;
      }
      //Serial.printf("%s ",strBuf);
      if(ch == '\n'){
        //Serial.printf("\n");
        ch=' ';
        rxTask.Buffer[rxTask.length]='\0';
        rxTask.Completed=true;
      }
    }  
    vTaskDelay(5);
  } 
}
```
```CPP:時計付き歩数計クラス　simpleWaler.hpp
#ifndef __SIMPLEWALKER1_
#define __SIMPLEWALKER1_

#include <M5Core2.h>
#include <mutex>

// Create a mutex:
extern SemaphoreHandle_t mutex;

class simpleWalker
{
    public:
        int count;
        float diff;
        float old,now;
         
    public:
        simpleWalker();//コンストラクタ
        void rangeChecker(float _value);
};

extern simpleWalker swk; 

#endif
```

```CPP:時計付き歩数計クラス　simpleWaler.cpp
#include "simpleWalker.hpp"
#include <Arduino.h>

simpleWalker swk;
SemaphoreHandle_t mutex = xSemaphoreCreateMutex();

simpleWalker::simpleWalker()
{
    swk.count = 0;
    old=0;
    now=0;
    mutex = xSemaphoreCreateMutex();
    assert(mutex);
    
}

///@briefこの関数で、歩数をカウント
///float _value:加速度ベクトルの大きさ
void simpleWalker::rangeChecker(float _value)
{
    static int temp[2];
    static float ftemp;
    now = _value;
    diff = now - old;
    ftemp = abs(diff);
    
    if(temp[0]==0 && ftemp>0.3)//加速度に大きな変化が生じたら、計測開始。
    {
        temp[0]++;
    }else
    {
        if(temp[0]==1 && ftemp<0.1)//加速度の変化率が低下を検知
        {
            temp[0]=0;    //もう一度大きな加速度変化をとらえるため、クリア
            temp[1]++;    //安定動作をカウント。
            if(temp[1]==3)
            {//3回の大きな変化と、3回の安定動作をカウントしたら、歩数をインクリメント。
                temp[1]=0;
                xSemaphoreTake(mutex, portMAX_DELAY); // enter critical section
                count++;
                xSemaphoreGive(mutex); // exit critical section
                delay(150);
            }
        }
    }
    old = now;
}
```

```CPP:main.c
#include <M5Core2.h>
#include "simpleWalker.hpp"
#include "mySerialRXTask.hpp"
#include "myMPU6886.h"
#include "myInterrupt.hpp"


#define LCDWIDE 320
#define LCDHEIGHT 240

TFT_eSprite canvas = TFT_eSprite(&M5.Lcd);
TFT_eSprite canvas1 = TFT_eSprite(&M5.Lcd);

RTC_DateTypeDef RTC_Date; // Data
RTC_DateTypeDef StartDate;
RTC_DateTypeDef EndDate;
RTC_TimeTypeDef RTC_Time; // Time
RTC_TimeTypeDef StartTime;
RTC_TimeTypeDef EndTime;
RTC_DateTypeDef ElapseDate;
RTC_TimeTypeDef ElapseTime;

void Page0Paint(void);
void Page1Paint(void);

void setup()
{
   M5.begin();
   canvas.fillScreen(WHITE);
   canvas.createSprite(320, 110);  //Create a 320x240 canvas.
   canvas.setColorDepth(8);        // Set color depth.  
   canvas1.createSprite(320, 50);  //Create a 320x240 canvas.
   canvas1.setColorDepth(8);        // Set color depth.     
   //Task for Serial receiving
   rxTask.start();
   //加速度センサー初期化**************************************
   M5.IMU.Init();
   M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);    //range:+-4G
   M5.IMU.SetGyroFsr(M5.IMU.GFS_2000DPS);//range:2000 degree/s
   //mpu.calibration();
   //割り込み処理-------------------------------------------------------
   myInterrupt();//割り込み初期化
   //
   
   M5.Lcd.fillScreen(WHITE);
   M5.Lcd.setFreeFont(&FreeSans12pt7b);
   M5.Lcd.setCursor(5,25);
   M5.Lcd.setTextColor(BLUE);
   M5.Lcd.printf("Simple Walker ver1.0");
   Page0Paint();
   Serial.printf("M5Core2 START\n");
}


void loop()
{
   
   M5.update();
   
   if(M5.BtnA.pressedFor(5000))
   {
      swk.count=0;
   }

   //USBシリアル受信-------------------------------------------
   if(rxTask.Completed)
   {
      Serial.printf("%d echo->%s",rxTask.length,rxTask.Buffer);
      if(rxTask.Buffer[0]=='t' && rxTask.length==15)
      {
         RTC_Date.Year = String(&rxTask.Buffer[1],(unsigned int)2).toInt()+2000;
         RTC_Date.Month = String(&rxTask.Buffer[3],(unsigned int)2).toInt();
         RTC_Date.Date = String(&rxTask.Buffer[5],(unsigned int)2).toInt();
         //RTC_Date.WeekDay = 2;//Sun:0
         RTC_Time.Hours = String(&rxTask.Buffer[7],(unsigned int)2).toInt();
         RTC_Time.Minutes = String(&rxTask.Buffer[9],(unsigned int)2).toInt();
         RTC_Time.Seconds = String(&rxTask.Buffer[11],(unsigned int)2).toInt();
         if(!M5.Rtc.SetDate(&RTC_Date)) Serial.println("Wrong Date set");
         if(!M5.Rtc.SetTime(&RTC_Time)) Serial.println("Wrong time set");
      }
      if(rxTask.Buffer[0]=='w' && rxTask.Buffer[1]=='d' && rxTask.length==5)
      {
         RTC_Date.WeekDay = rxTask.Buffer[2]-0x30;
         if(!M5.Rtc.SetDate(&RTC_Date)) Serial.println("Wrong Date set");
         printf("week day %d OK.\n", RTC_Date.WeekDay);
      }
      rxTask.length=0;
      rxTask.Completed=false;
   }
   
   
   //表示
   /*if(Ticker1.flag)
   {
      Ticker1.flag=false;
      //加速度、ジャイロ取得
      M5.IMU.getAccelData(&mpu.acc.x, &mpu.acc.y, &mpu.acc.z);
      mpu.GetVectorScalarSize();
      swk.rangeChecker(mpu.accVectorSize);
   }*/
   if(Ticker2.flag)
   {
      Ticker2.flag=false;
      Page0Paint();
      Page1Paint();
   }
   delay(1);
   
}

void Page0Paint(void)
{
   const char *wd[7] = {"Sun","Mon","Tue","Wed","Thr","Fri","Sat"};
   const char position=10;
   M5.Rtc.GetDate(&RTC_Date);
   M5.Rtc.GetTime(&RTC_Time);

   canvas.fillScreen(WHITE);
   //*display Current Date----------------*//
   canvas.fillRect(0,0,320,50,BLUE);
   //canvas.fillRect(1,51,318,48,BLUE);
   canvas.setTextColor(WHITE);
   canvas.setCursor(5,40);
   canvas.setFreeFont(&FreeSans18pt7b);
   canvas.setTextSize(1);
   canvas.printf("%04d/%02d/%02d<%s>", RTC_Date.Year, RTC_Date.Month, RTC_Date.Date,wd[RTC_Date.WeekDay]);
   //*display current time----------------*//
   canvas.fillRect(0,60,320,50,BLUE);
   //canvas.fillRect(1,111,318,48,BLUE);
   canvas.setCursor(5,77); 
   canvas.setTextColor(WHITE);
   canvas.setFreeFont(&FreeSans9pt7b);
   canvas.printf("Current time");
   canvas.setCursor(120,105);
   canvas.setFreeFont(&FreeSans24pt7b);
   canvas.printf("%02d:%02d:%02d", RTC_Time.Hours, RTC_Time.Minutes, RTC_Time.Seconds);
   canvas.pushSprite(0,50);
   
}

void Page1Paint(void)
{
   //加速度値表示
   canvas1.fillScreen(WHITE);
   canvas1.drawRect(0,0,320,50,ORANGE);
   canvas1.setCursor(10,5+15);
   canvas1.setFreeFont(&FreeSans12pt7b);
   canvas1.setTextSize(1);
   canvas1.setTextColor(RED);  
   canvas1.printf("%5.1f %5.1f %5.1f",mpu.acc.x, mpu.acc.y, mpu.acc.z);

   canvas1.setCursor(65,5+35);
   canvas1.setTextColor(DARKGREEN);
   canvas1.printf("%5.1f",mpu.accVectorSize);
   
   canvas1.setCursor(190,5+30);
   canvas1.setTextColor(BLUE);
   canvas1.setFreeFont(&FreeSans24pt7b);
   xSemaphoreTake(mutex, portMAX_DELAY); // enter critical section
   canvas1.printf("%05d",swk.count);
    xSemaphoreGive(mutex); // exit critical section
   canvas1.pushSprite(0,190);
}

```