Motion Detect with Gyro sensor - desktopmake’s blog

#define SV_Motor_x_PIN 32
#define SV_Motor_y_PIN 26
#define SV_Motor_z_PIN 14

#include<math.h>
#define Deg2rad 3.1415 /180
#include <MadgwickAHRS.h>

//MPU9250
#include <Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ;

//Actual unitization scale factor of acquired data
float SF_Acc = 16384; //acceleration scale factor   (digit/g) 
float SF_Gy  = 131; //Gyro scale factor (digit/dps)
float SF_Mg  = 500; //magnetic scale factor(500☛0.6かも。。)
float SF_Tmp = 333.87; //temperature scale factor
float g2mpss = 9.80665; //conversion from G to m/s2
float deg2rad = 3.14159265/180; //from degree to radian


Madgwick MadgwickFilter;

//servo motor define duty min and max
float duty_min = 25.6; // at 0 deg
float duty_max = 122.9; // at 180 deg
float duty_active = duty_max - duty_min;
float duty_mid = (duty_min + duty_max)/2; // at 90 deg

///タイマー割込み 参考https://55life555.blog.fc2.com/blog-entry-3194.html
volatile int timeCounter1;
hw_timer_t *timer1 = NULL; 
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void IRAM_ATTR onTimer1(){

  portENTER_CRITICAL_ISR(&timerMux);
  timeCounter1++;
  portEXIT_CRITICAL_ISR(&timerMux);
}

void setup() {

  //SVmotor
  pinMode(SV_Motor_x_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT
  pinMode(SV_Motor_y_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT
  pinMode(SV_Motor_z_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT
  ledcSetup(0, 50, 10);
  ledcSetup(1, 50, 10);
  ledcSetup(2, 50, 10);
  ledcAttachPin(SV_Motor_x_PIN,0); 
  ledcAttachPin(SV_Motor_y_PIN,1); 
  ledcAttachPin(SV_Motor_z_PIN,2); 

////////////////////////////Highly needed module
  Serial.begin(115200);
////////////////////////////

//MPU9250 set up
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  setupMPU9255();

  //MadgwickFilter
  MadgwickFilter.begin(10);//filtering frequency 100Hz

//タイマー割込み
  timer1 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer1, &onTimer1, true);
  timerAlarmWrite(timer1, 100000, true);
  timerAlarmEnable(timer1);

}

//value after sensor value scaling
float AcX_SF;
float AcY_SF;
float AcZ_SF;
float GyX_SF;
float GyY_SF;
float GyZ_SF;

//変数
struct strc{
  float AXIS_X;
  float AXIS_Y;
  float AXIS_Z;
};

float ROLL, PITCH, YAW;

void setupMPU9255() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

//MPU9250 setup
void readCompass() {
  Wire.beginTransmission(0x0C);
  Wire.write(0x02);
  Wire.endTransmission();
  Wire.requestFrom(0x0C, 1);

  uint8_t ST1 = Wire.read();
  if (ST1 & 0x01) {
    Wire.beginTransmission(0x0C);
    Wire.write(0x03);
    Wire.endTransmission();
    Wire.requestFrom(0x0C, 7);
    uint8_t i = 0;
    uint8_t buf[7];
    while (Wire.available()) {
      buf[i++] = Wire.read();
    }
    if (!(buf[6] & 0x08)) {
      MgX = ((int16_t)buf[1] << 8) | buf[0];
      MgY = ((int16_t)buf[3] << 8) | buf[2];
      MgZ = ((int16_t)buf[5] << 8) | buf[4];
    }
  }
}

void loop() 
{ 
  unsigned long time;//「time」をunsigned longで変数宣言, declared "time"as variable
  time = millis();//プログラム実行からの経過時間(ms)をtimeに返す,Returns from program execution lapsed time (ms) to "time" 

    if (timeCounter1 > 0) 
      {
      portENTER_CRITICAL(&timerMux);
      timeCounter1--;
      portEXIT_CRITICAL(&timerMux);
    
      ////MPU9250関連////
      //読み出し, read
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,16,true);  // request a total of 14 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
      readCompass();
    
      //Acc/g->m/s^2,Gy/dps->rad/s
      MadgwickFilter.update(GyX/SF_Gy, GyY/SF_Gy, GyZ/SF_Gy, AcX/SF_Acc, AcY/SF_Acc, AcZ/SF_Acc, MgX/SF_Mg, MgY/SF_Mg, MgZ/SF_Mg);
      ROLL = MadgwickFilter.getRoll();
      PITCH = MadgwickFilter.getPitch();
      YAW  = MadgwickFilter.getYaw();
    
      //for serial plot
      Serial.print(ROLL); Serial.print(",");
      Serial.print(PITCH); Serial.print(",");
      Serial.print(YAW);
      Serial.print("\n");

      //convert from angle of sensor for servo control
      ROLL = duty_mid + ROLL/180 * duty_active;
      ROLL = min(max(ROLL, duty_min), duty_max);
      
      PITCH = duty_mid + PITCH/180 * duty_active;
      PITCH = min(max(PITCH, duty_min), duty_max);
      
      YAW = duty_mid + YAW/180 * duty_active;
      YAW = min(max(YAW, duty_min), duty_max);
                   
      ledcWrite(0, ROLL);
      ledcWrite(1, PITCH);
      ledcWrite(2, YAW);
      
      delay(10);
    }