【ドローン製作記録】PID制御用プログラムの作成【Arduino】

スポンサーリンク
ドローン製作記録
スポンサーリンク

はじめに

大学が始まってしまい、中々作業や更新ができなくなっています。

今回は暫定で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制御を行えば制御については完成すると思います。

ただ、スケッチサイズ的にそろそろいっぱいなので先に無線通信について触れる予定です。

コメント

タイトルとURLをコピーしました