はじめに
大学が始まってしまい、中々作業や更新ができなくなっています。
今回は暫定でPID制御するためのプログラムを作成したので、ソースコードの紹介をしたいと思います。
ソースコード
今回はPID制御用に新しく簡単な自作ライブラリを作りました。
特に他の人が利用できるようなものではないのですが、参考になれば幸いです。
.hファイル
#ifndef QUADPID_KTORA_H
#define QUADPID_KTORA_H
//ヘッダ
#include <Arduino.h>
#include <Servo.h>
//デフォルト出力ピン
#define def_Fr 10 //Front_right
#define def_Fl 9 //Front_left
#define def_Rr 5 //Rear_right
#define def_Rl 6 //Rear_left
//デフォルトPID係数
#define def_Kp 1 //Kp
#define def_Ki 0 //Ki
#define def_Kd 0 //Kd
#define def_Dt 20 //Dt
//クラス宣言
class QUADPID_ktora{
//private
private:
//各モータのServoオブジェクト
Servo Front_r; //Front_right
Servo Front_l; //Front_left
Servo Rear_r; //Rear_right
Servo Rear_l; //Rear_left
//PID環境変数
//ゲイン
uint8_t Kp;
uint8_t Ki;
uint8_t Kd;
//周期
uint8_t Dt;
//偏差積分保存
float itrl_errPitch;
float itrl_errRoll;
float itrl_errYaw;
//偏差保存
float last_errPitch;
float last_errRoll;
float last_errYaw;
//目標姿勢角
float target_Pitch;
float target_Roll;
float target_Yaw;
//PWM信号の設定
void set_pwm_Quadpid(int16_t Fr_pwm, int16_t Fl_pwm, int16_t Rr_pwm, int16_t Rl_pwm);
//PID計算
float calc_Quadpid(float error, float *i_error, float *last_error);
//public
public:
//コンストラクタ 初期化
QUADPID_ktora():
Kp(0),
Ki (0),
Kd(0),
Dt(0),
itrl_errPitch(0),
itrl_errRoll(0),
itrl_errYaw(0),
last_errPitch(0),
last_errRoll(0),
last_errYaw(0),
target_Pitch(0),
target_Roll(0),
target_Yaw(0) {};
//setup
void setup_Quadpid(uint8_t set_Dt = def_Dt, uint8_t set_Kp = def_Kp, uint8_t set_Ki = def_Ki, uint8_t set_Kd = def_Kd, uint8_t Fr_pin = def_Fr, uint8_t Fl_pin = def_Fl, uint8_t Rr_pin = def_Rr, uint8_t Rl_pin = def_Rl);
//目標姿勢角の更新
void target_Quadpid(float get_pitch, float get_roll, float get_yaw);
//姿勢角の更新,PID制御
void update_Quadpid(float get_pitch, float get_roll, float get_yaw, float get_throttle);
};
#endif
.cppファイル
#include "QUADPID_ktora.h"
//出力の最大最小の設定（固定値）
#define Pwm_Max 1800 //最大パルス幅
#define Pwm_Min 1000 //最小パルス幅
//PID制御
#define inte_Num 300 //積分偏差の大小の絶対値
#define pid_Num 1000 //制御幅
//PWM信号の設定
void QUADPID_ktora::set_pwm_Quadpid(int16_t Fr_pwm, int16_t Fl_pwm, int16_t Rr_pwm, int16_t Rl_pwm){
//出力の補正
if(Fr_pwm < Pwm_Min){
Fr_pwm = Pwm_Min;
}else if(Fr_pwm > Pwm_Max){
Fr_pwm = Pwm_Max;
}
if(Fl_pwm < Pwm_Min){
Fl_pwm = Pwm_Min;
}else if(Fl_pwm > Pwm_Max){
Fl_pwm = Pwm_Max;
}
if(Rr_pwm < Pwm_Min){
Rr_pwm = Pwm_Min;
}else if(Rr_pwm > Pwm_Max){
Rr_pwm = Pwm_Max;
}
if(Rl_pwm < Pwm_Min){
Rl_pwm = Pwm_Min;
}else if(Rl_pwm > Pwm_Max){
Rl_pwm = Pwm_Max;
}
//シリアル出力
Serial.print("FR=");
Serial.print(Fr_pwm);
Serial.print(" FL=");
Serial.print(Fl_pwm);
Serial.print(" RR=");
Serial.print(Rr_pwm);
Serial.print(" RL=");
Serial.print(Rl_pwm);
Serial.println("");
Front_r.writeMicroseconds(Fr_pwm);
Front_l.writeMicroseconds(Fl_pwm);
Rear_r.writeMicroseconds(Rr_pwm);
Rear_l.writeMicroseconds(Rl_pwm);
delay(10);
}
//PID計算
float QUADPID_ktora::calc_Quadpid(float error, float *i_error, float *last_error){
//偏差積分の加算
*i_error += error * Dt / 10000.0;
//偏差積分補正
if(*i_error > inte_Num){
*i_error = inte_Num;
}else if(*i_error < -inte_Num){
*i_error = -inte_Num;
}
//PID各数値の計算
float PID = Kp / 10.0 * error + Ki / 10.0 * (*i_error) + Kd / 10.0 * (error - (*last_error)) / Dt;
//PID補正
if(PID < -pid_Num){
PID = -pid_Num;
}else if(PID > pid_Num){
PID = pid_Num;
}
//偏差の更新
*last_error = error;
return PID;
}
//setup
void QUADPID_ktora::setup_Quadpid(uint8_t set_Dt, uint8_t set_Kp, uint8_t set_Ki, uint8_t set_Kd, uint8_t Fr_pin, uint8_t Fl_pin, uint8_t Rr_pin, uint8_t Rl_pin){
//PID環境変数の設定
Dt = set_Dt;
Kp = set_Kp;
Ki = set_Ki;
Kd = set_Kd;
//モータの設定
Front_r.attach(Fr_pin); //Front_right
Front_l.attach(Fl_pin); //Front_left
Rear_r.attach(Rr_pin); //Rear_right
Rear_l.attach(Rl_pin); //Rear_left
//各ESCに最小PWM幅を送信
set_pwm_Quadpid(Pwm_Min,Pwm_Min,Pwm_Min,Pwm_Min); //PWM制御
delay(1000); //待ち
}
//目標姿勢角の更新
void QUADPID_ktora::target_Quadpid(float get_pitch, float get_roll, float get_yaw){
target_Pitch = get_pitch;
target_Roll = get_roll;
target_Yaw = get_yaw;
}
//姿勢角の更新,PID制御
void QUADPID_ktora::update_Quadpid(float get_pitch, float get_roll, float get_yaw, float get_throttle){
//偏差の計算
float errPitch = target_Pitch - get_pitch;
float errRoll = target_Roll - get_roll;
float errYaw = target_Yaw - get_yaw;
float pwm_Pitch = calc_Quadpid(errPitch, &itrl_errPitch, &last_errPitch);
float pwm_Roll = calc_Quadpid(errRoll, &itrl_errRoll, &last_errRoll);
float pwm_Yaw = calc_Quadpid(errYaw, &itrl_errYaw, &last_errYaw);
//PWM信号の計算
int16_t Fr_pwm = get_throttle - pwm_Pitch + pwm_Roll - pwm_Yaw;
int16_t Fl_pwm = get_throttle + pwm_Pitch + pwm_Roll + pwm_Yaw;
int16_t Rr_pwm = get_throttle - pwm_Pitch - pwm_Roll + pwm_Yaw;
int16_t Rl_pwm = get_throttle + pwm_Pitch - pwm_Roll - pwm_Yaw;
set_pwm_Quadpid(Fr_pwm,Fl_pwm,Rr_pwm,Rl_pwm); //PWM制御
}
PID制御プログラム
今回作ったライブラリとは別に、前回の記事で作った自作ライブラリとMadgwickFilterのライブラリを用いています。
このプログラムでは一定の出力で同じ体勢を維持しようとします。
後は実際に動かしながらゲイン調整をしてやれば姿勢についてのPID制御が完成します。
#include <MPU9250_ktora.h>
#include <QUADPID_ktora.h>
#include <MadgwickAHRS.h>
MPU9250_ktora mpu9250; //MPU9250
QUADPID_ktora quadpid; //QUADPID
Madgwick MadgwickFilter; //Madgwick
float pitch = 0.0;
float roll = 0.0;
float yaw = 0.0;
//setup
void setup(){
Serial.begin(9600);
Wire.begin(); //I2C通信を開始する
mpu9250.set_Wire(&Wire); //Wireのコピー
mpu9250.setup_Mpu9250(); //mpu9250の設定
mpu9250.set_Offset(0,0,0,0,0,0,-28.10,-11.27,-6.04); //オフセットの設定
mpu9250.calc_Offset(); //オフセットの計算
MadgwickFilter.begin(20); //20hz
delay(3000); //3秒待機
quadpid.setup_Quadpid(); //escの設定
quadpid.target_Quadpid(0,0,0); //目標姿勢角の設定
}
//loop
void loop(){
mpu9250.update_Data_Mpu9250(); //データの更新
float AccX = mpu9250.get_AccX();
float AccY = mpu9250.get_AccY();
float AccZ = mpu9250.get_AccZ();
float GyrX = mpu9250.get_GyrX();
float GyrY = mpu9250.get_GyrY();
float GyrZ = mpu9250.get_GyrZ();
float MagX = mpu9250.get_MagX();
float MagY = mpu9250.get_MagY();
float MagZ = mpu9250.get_MagZ();
//計算処理
MadgwickFilter.update(GyrX,GyrY,GyrZ,AccX,AccY,AccZ,MagX,MagY,MagZ);
pitch = MadgwickFilter.getPitch();
roll = MadgwickFilter.getRoll();
yaw = MadgwickFilter.getYaw() - 180; //(-180~180)に調整
quadpid.update_Quadpid(pitch,0,0,1200);
delay(5);
}
最後に
ゲイン調整をして、BME280の気圧データを用いて上下のPID制御を行えば制御については完成すると思います。
ただ、スケッチサイズ的にそろそろいっぱいなので先に無線通信について触れる予定です。
コメント