投稿者: admin_wp

#include <imu.h>

int servopinx=2;    // dio port definition for X axis
int servopiny=5;    // dio port definition for Y axis
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 p_accX;       // to hold IMU read data
float p_accZ;
int p_angX;         // to hold servo motor angles
int p_angZ;
boolean first = true;   // check if it is first cycle or not.
//
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 init_sm()
{
    servo(90, 0);
    servo(90, 1);
    p_angX = 90;
    p_angZ = 90;
    delay(600);
}
void setup()
{
 setup_imu();
 pinMode(servopinx,OUTPUT);
 pinMode(servopiny,OUTPUT);
}
void loop()
{
 loop_imu();
 if (first == true){
    init_sm();                  // initialize to 90 degree
    first = false;
    }
// the angle change is needed? & the change is reflected? & check the angle limit
 if (accX < -0.05 && abs(p_accX - accX) > 0.02 && p_angX < 180){ p_angX ++; } else{ if (accX > 0.05 && abs(p_accX - accX) > 0.02 && p_angX > 0){
             p_angX --;
         }
     }
 if (accZ < -0.05 && abs(p_accZ - accZ) > 0.02 && p_angZ < 180){ p_angZ ++; } else{ if (accZ > 0.05 && abs(p_accZ - accZ) > 0.02 && p_angZ > 0){
             p_angZ --;
         }
     }
 p_accX = accX;                 // set to the previous values
 p_accZ = accZ;
 servo(p_angZ , 0);             // drive the servo motor
 servo(p_angX , 1);
 delay(1);
 M5.Lcd.setCursor(0, 114);
 M5.Lcd.printf("%3d  %3d ", p_angX, p_angZ);
}

imu.hには変更ありません。

今回はM5stackを垂直で使うので、二次元を水平に保つのはX軸とZ軸になるので、IMUからの読み出しデータはそれらを使います。

M5stackが移動するときには重力加速度以外の加速度も加算されますが、このアプリではそれを勘案する必要はないでしょう。多少動きがオーバーシュートするのはそのせいかもしれませんが。

サーボーモーターの電源入れない状態でM5stackを動かしているので、画面のアングル表示は変な値になっています。初期値がどうであっても、最後は収束しますが。

コードと3dプリンタ用のstlファイルは、

https://github.com/chateight/servo_

に置いてあります。M5stack追従のロジックは改善の余地があるように思います。

M5stack本体との接続は、DIO2,5とGNDの三本だけ。

admin

#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);
}

/*******舵机测试程序*******
 * Arduino型号：Arduino UNO
 **************************/
#include <m5stack.h>    // M5stack needs this module
int servopin=2;
int pulsewidth;
void servo(int myangle)
{
 for (int i = 0; i < 20; i ++)  // i is used to make moving time, since this source does not use library.
 {
pulsewidth=map(myangle,0,180,500,2500);
digitalWrite(servopin,HIGH);
delayMicroseconds(pulsewidth);
digitalWrite(servopin,LOW);
delay(20-pulsewidth/1000);
 }
}
void setup()
{
 pinMode(servopin,OUTPUT);
 M5.begin();
 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()
{
 M5.Lcd.setCursor(0, 70);
 M5.Lcd.printf("servo_motor drive");
 servo(0);
 delay(500);
 servo(180);
 delay(500);
}