LoginSignup
0
0
@etoolsLab369

M5Stack Core2 simpleWalker 歩数計

Last updated at Posted at 2024-05-28

Simple Walker　加速度センサーで歩数計

IMG_4744.JPG
加速度センサーの３次元ベクトルの大きさで、カウントしています。腕の振りに同期して、カウントします。

IMG_4745.JPG
3Dプリンターで台座をつくり、電線をまとめるケーブルバンドを短くして腕に巻き付けるようにしました。

腕の振りに同期させて、一振りで一カウントさせています。

  1. 前方に振り上げた腕の位置が、スタート位置。
  2. 後方に振り下げた限界点の位置
  3. 前方のスタート位置にもどる。ここで、歩数をカウント。

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

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

時計歩数計SimpleWalkerコード

加速度センサークラス 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
加速度センサー 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);
}
タイマー割り込み(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_
タイマー割り込み(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;
    }
}
シリアル通信クラス 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
シリアル通信クラス 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);
  } 
}
時計付き歩数計クラス　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
時計付き歩数計クラス　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;
}
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);
}
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0