M5stackの加速度計（重力加速度）とサーボモーターを組み合わせることで、M5stackの姿勢をフォローするものを作ります。
M5stackの加速度測定数値は結構揺らぎがあるので、平滑化と有効ビットの削減をおこなっています。
<servo.cpp>
#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);
}本来的なヘッダーファイルの使い方とは多少異なりますが、M5stack IMUの加速度数値を読み取るルーチンです。
<imu.h>
/*
*******************************************************************************
* 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);
}
やりたいことにはもう一工夫がいるので、それを追加して３Dプリンタで構造を作れば完成型になります。
P.S.
傾き(θ)に対して重力加速度はリニアではなく、重力加速度からarcsin()で傾きを計算しないといけないわけですが、大まかな振る舞いを見るには直線性は無視しても構わないと思います。
P.S.2
Switch Science記載のサーボモーターのピン番号は間違っている（PowとSig入れ違い）ので、正しいピン番号を記載しておきます。
admin
