二つ前の記事でIMUとサーボモーターを連動させたので、構造部分を作成してM5stackの直立状態を保持できるような姿勢制御を作ってみました。
前回からの変更点はサーボ制御部分のみです。
#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
