M5stack IMUとサーボモーターの連動（二軸）

#include <imu.h>

int servopinx=2;    // dio port definition
int servopiny=5;
int pulsewidth;     // pwm on time
int repeat=1;       // number of pwm write cycle repetition
int ax;             // angle for servo motors
int ay;
int az;
float comp = 1.1;  // compensate not to show minus value
float adj = 30;    // adjuct the angle with map() function not to exceed the range upper limit 180　& eliminate the noise
float p_accX = 0.5;
float p_accY = 0.5;
float p_accZ = 0.5;
boolean first = true;   // check if it is first cycle or not.
float smooth = 0.6;     // smoothing the angle value. it causes a delay time to the angle change.
//
void servo(int myangle, int motor)      // servo motor pwm contorol. motor 0 : servoponx , motor 1 : servopiny
{
 for (int i = 0; i < repeat; i ++)      // i is used to keep a moving time, since this source does not use the library.
 {
    int port;
    if (motor == 0){
        port = servopinx;
         }
        else{
        port = servopiny;
    }
    pulsewidth=map(myangle,0,180,500,2500);
    digitalWrite(port,HIGH);
    delayMicroseconds(pulsewidth);
    digitalWrite(port,LOW);
    delay(20-pulsewidth/1000);
 }
}
void setup()
{
 setup_imu();
 pinMode(servopinx,OUTPUT);
 pinMode(servopiny,OUTPUT);
}
void loop()
{
 loop_imu();
 if (first == true){        // smoothing
     p_accX = accX;
     p_accY = accY;
     p_accZ = accZ;
     first = false;
    }
    else{
        p_accX = p_accX*smooth + accX*(1-smooth);       
        p_accY = p_accY*smooth + accY*(1-smooth);
        p_accZ = p_accZ*smooth + accZ*(1-smooth);
    }
 ax = map((int)((p_accX+comp)*adj), 0, 66, 0, 180);     // translate acc values to the range 0 to 180
 ay = map((int)((p_accY+comp)*adj), 0, 66, 0, 180);
 az = map((int)((p_accZ+comp)*adj), 0, 66, 0, 180);
 M5.Lcd.setCursor(0, 114);
 M5.Lcd.printf("%3d  %3d  %3d ", ax, ay, az);
 servo(ax , 0);             // drive the servo motor
 servo(ay , 1);
 delay(5);
}

/*
*******************************************************************************
* Copyright (c) 2021 by M5Stack
*                  Equipped with M5Core sample source code
* Visit the website for more information：https://docs.m5stack.com/en/core/gray
*
* describe：MPU6886 example. 
* date：2021/7/21
*******************************************************************************
*/
#define M5STACK_MPU6886
#include <m5stack.h>

float accX = 0.0F;  // Define variables for storing inertial sensor data
float accY = 0.0F;
float accZ = 0.0F;

void setup_imu(){
  M5.begin(); //Init M5Core.  
  M5.Power.begin(); //Init Power module.  

  M5.IMU.Init();  //Init IMU sensor.  

  M5.Lcd.fillScreen(BLACK); //Set the screen background color to black. 
  M5.Lcd.setTextColor(GREEN , BLACK); //Sets the foreground color and background color of the displayed text.  
  M5.Lcd.setTextSize(2);  //Set the font size.  
}

void loop_imu() {
  M5.IMU.getAccelData(&accX,&accY,&accZ); //Stores the triaxial accelerometer. 

  // Accelerometer output is related
  M5.Lcd.setCursor(0, 70);
  M5.Lcd.printf("accX,   accY,  accZ");
  M5.Lcd.setCursor(0, 92);
  M5.Lcd.printf("%5.2f  %5.2f  %5.2f G", accX, accY, accZ);
}