Source Code of DIY Drone - ESP32 [Arduino] Source code

Drone side source code

//参考https://rikoubou.hatenablog.com/entry/2017/10/06/181805

#include<math.h>
#define Deg2rad 3.1415 /180
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

float quat[4] = {1.0, 0.0, 0.0 ,0.0};
float rate[3];

///タイマー割込み 参考https://55life555.blog.fc2.com/blog-entry-3194.html
volatile int timeCounter1;
hw_timer_t *timer1 = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

void IRAM_ATTR onTimer1(){

  portENTER_CRITICAL_ISR(&timerMux);
  timeCounter1++;
  portEXIT_CRITICAL_ISR(&timerMux);
}

////////MPU9250関連////////
//サンプリング周期
const int dt = 3; ///ms

//平均値処理のためのサンプリング回数
const int FIFO_order = 5;

//PIDにおける微分項に用いる時間の割り算に関する演算子
const int Deno = 1 /dt * FIFO_order;

#include <Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ;

////構造体定義////
//参考https://www.cc.kyoto-su.ac.jp/~yamada/programming/struct.html#declaration
typedef struct {
  float Axis_X;
  float Axis_Y;
  float Axis_Z;
} structure;

//センサー値格納//
structure Ac_Sens[FIFO_order];//加速度
structure Gy_Sens[FIFO_order];//角速度
structure Ahrs_Sens[FIFO_order];//姿勢角

//各軸の目標値
//姿勢角(deg)
structure Ahrs_Tgt_Front = {-2,0,100};//前方
structure Ahrs_Tgt_Back = {2,0,100};//後方
structure Ahrs_Tgt_Right = {0,2,100};//右
structure Ahrs_Tgt_Left = {0,-2,100};//左
structure Ahrs_Tgt_Keep = {0,0,100};//維持
structure Ahrs_Tgt;//↑受け渡し用

//角速度(deg/s)
structure Gy_Tgt;
//加速度(m/s^2)
structure Ac_Tgt_Up = {0,0,3};//上昇
structure Ac_Tgt_Down = {0,0,-5};//下降
structure Ac_Tgt_Keep = {0,0,0};//維持
structure Ac_Tgt;//↑受け渡し用

//初期速度 m/s
structure Vl_Pre = {0,0,0};//ドローンの初速度
structure Vl;//ドローン速度

//誤差
structure error_Vl;//速度(m/s)
structure error_Ac;//加速度(m/s^2)
structure error_Ac_Pre;//加速度(m/s^2)_1時刻前の値
structure error_Ahrs;//角度(deg)
structure error_Ahrs_Pre;//角速度(deg)_1時刻前の値
structure error_Gy;//角速度(deg)
structure error_Gy_Pre;//角速度(deg)_1時刻前の値

////PID制御
///定数
//速度(m/s)
structure Kp_Vl = {1, 1, 1};//Propoptional
structure Ki_Vl = {1, 1, 1};//Integral
//加速度(m/s^2)
structure Kp_Ac = {0, 0, 3};//Propoptional
structure Ki_Ac = {0, 0, 3};//Integral
structure Kd_Ac = {0, 0, 3};//differential
//姿勢角(deg)
structure Kp_Ahrs = {0.1, 0.1, 0.1};//Propoptional(参考値{3, 3, 4})
structure Ki_Ahrs = {0.0, 0.0, 0};//Integral(参考値{0, 0, 0})
structure Kd_Ahrs = {0.0, 0.0, 0};//differential
//角速度(deg/s)
structure Kp_Gy = {3, 3, 3};//Propoptional(参考値{100, 100, 1000})
structure Ki_Gy = {3, 3, 0};//Integral(参考値{100, 100, 0})
structure Kd_Gy = {0.6, 0.6, 0};//differential(参考値{10, 10, 0})

//ループ中更新値
//速度(m/s)
structure Pid_Integ_Vl;
structure Pid_Integ_Vl_Pre = {0,0,0};
//加速度(m/s^2)
structure Pid_Integ_Ac;
structure Pid_Integ_Ac_Pre = {0,0,0};
structure Pid_Derive_Ac;
structure Pid_Derive_Ac_Pre = {0,0,0};
//姿勢角(deg)
structure Pid_Integ_Ahrs;
structure Pid_Integ_Ahrs_Pre = {0,0,0};
structure Pid_Derive_Ahrs;
structure Pid_Derive_Ahrs_Pre = {0,0,0};
//角速度(deg/s)
structure Pid_Integ_Gy;
structure Pid_Integ_Gy_Pre = {0,0,0};
structure Pid_Derive_Gy;
structure Pid_Derive_Gy_Pre = {0,0,0};

//PID値
structure Pid_Vl;   //速度
structure Pid_Ac;   //加速度
structure Pid_Ahrs; //姿勢角
structure Pid_Gy;   //角速度


//追従偏差微分のノイズフィルタ定数
const float D_Filter_C_Ac = 0.025/(dt*0.001*2*Deg2rad*180);//(/1000*2*Deg2rad*180);
//const float D_Filter_C_Ahrs = 0.025/(dt*0.001*2*Deg2rad*180);
const float D_Filter_C_Gy = 0.025/(dt*0.001*2*Deg2rad*180);//(dt/1000*2*Deg2rad*180);
//積分値の上限値
structure Limit_Vl = {1,1,1};//速度
structure Pid_Integ_Limit_Vl = {2,2,2};//速度
structure Pid_Integ_Limit_Ac = {2,2,2};//加速度
structure Pid_Integ_Limit_Ahrs = {0.05,0.05,0.05};//default{2,2,2}
structure Pid_Integ_Limit_Gy = {2,2,2};//default{20,20,2}
//PID上限値
structure Pid_Limit_Ac = {0,0,30};//加速度
structure Pid_Limit_Ahrs = {2,2,2};//default記載見つからず
structure Pid_Limit_Gy = {20,20,20};//default記載見つからず
const int PWM_Motor_Limit = 120;

//コマンド分配則調整
float Motor_offset[4] = {-16.3, -8.1, 0, -8.2};//{-16.3, -8.1, 0, -8.2}

////PWM指令////
float PWM_Motor_1;
float PWM_Motor_2;
float PWM_Motor_3;
float PWM_Motor_4;
float Motor_th = 1;

//Motor_th調整
int i_up = 0;
int i_up_num_limit = 5;//機体上昇の際の重力計算の平均値算出におけるサンプル取得回数
float Ac_Hovering = - 3;//機体上昇の際のZ軸方向の加速度（重力は加味）
int Motor_th_limit = 85;

//スタート後の重力計算
structure g_average = {0,0,0};
int g_k = 0;
int g_total_num = 0;
int g_total_num_limit = 500;//センサーからの重力値取得までのカウント回数, dt×g_total_num_limit ms→取得時間
float g2mpss_sens;

//WiFi関連
#include <WiFi.h>
#include <WiFiUdp.h>
const char ssid[] = "ESP32_wifi"; // SSID
const char pass[] = "esp32pass";  // password
const int localPort = 10000;      // ポート番号(pythonのポート番号に合わせる)
const IPAddress ip(192 ,168, 2 ,14);       // IPアドレス(pythonのIPアドレスに合わせる)
const IPAddress subnet(255, 255, 255, 0); // サブネットマスク
WiFiUDP udp;
int val_2;

//電源OFF条件
const int time_PWMoff = 5000;//5,000msの間wifiudp入力がなかった場合に切断する
int k;// PWM->OFFまでのカウント回数
int k_PWMoff = round(time_PWMoff / dt);
int i_th_limit = 5;//offまでのMotor_thの段階

//ESP32からのPWM出力
//https://wak-tech.com/archives/742
const int Motor_1 = 32;
const int Motor_2 = 33;
const int Motor_3 = 25;
const int Motor_4 = 26;

//取得データの実単位化スケールファクター
float SF_Ac = 16384; //加速度のスケールファクタ   (digit/g) 
float SF_Gy  = 131; //#ジャイロのスケールファクタ  (digit/dps)
float SF_Mg  = 500; //磁気のスケールファクタ(500☛0.6かも。。)
float SF_Tmp = 333.87; //温度のスケールファクタ
float g2mpss = 9.80665; //Gをm/s2に変換
float deg2rad = 3.14159265/180; //ディグリーをラジアンに

//センサー値スケール化後数値
float AcX_SF;
float AcY_SF;
float AcZ_SF;
float GyX_SF;
float GyY_SF;
float GyZ_SF;



float ROLL, PITCH, YAW;

//サーボ初期値設定
int sv1 = 73;
int sv2 = 73;  
int sv3 = 73;

int delta_sv = 1;

void setupMPU9255() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

//MPU9250向け電力供給
void setup() {

  Serial.begin(115200);

//ピンNo5から電力供給 
  pinMode(5, OUTPUT);
  digitalWrite(5, HIGH);

//MPU9250セットアップ
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  setupMPU9255();

  
//UDP(PC-ESP間)通信関係
  //Serial.begin(115200);

  WiFi.softAP(ssid, pass);           // SSIDとパスの設定
  //delay(100);                        // 追記：このdelayを入れないと失敗する場合がある
  WiFi.softAPConfig(ip, ip, subnet); // IPアドレス、ゲートウェイ、サブネットマスクの設定

  Serial.print("AP IP address: ");
  IPAddress myIP = WiFi.softAPIP();
  Serial.println(myIP);

  Serial.println("Starting UDP");
  udp.begin(localPort);  // UDP通信の開始(引数はポート番号)

  Serial.print("Local port: ");
  Serial.println(localPort);

//MadgwickFilter
  MadgwickFilter.begin(1000/dt);//フィルタリング周波数100Hz

//タイマー割込み
  timer1 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer1, &onTimer1, true);
  timerAlarmWrite(timer1, dt*1000, true); //μs
  timerAlarmEnable(timer1);

//LEDC関係
  // 使用するタイマーのチャネルと周波数を設定
  ledcSetup(0, 12800, 8);
  ledcSetup(1, 12800, 8);
  ledcSetup(2, 12800, 8);
  ledcSetup(3, 12800, 8);
  // ledPinをチャネル0へ接続
  ledcAttachPin(Motor_1, 0);
  ledcAttachPin(Motor_2, 1);
  ledcAttachPin(Motor_3, 2);
  ledcAttachPin(Motor_4, 3);

  //コントローラ初期値、
  Ahrs_Tgt = Ahrs_Tgt_Keep;

}

//MPU9250セットアップ関連
void readCompass() {
  Wire.beginTransmission(0x0C);
  Wire.write(0x02);
  Wire.endTransmission();
  Wire.requestFrom(0x0C, 1);

  uint8_t ST1 = Wire.read();
  if (ST1 & 0x01) {
    Wire.beginTransmission(0x0C);
    Wire.write(0x03);
    Wire.endTransmission();
    Wire.requestFrom(0x0C, 7);
    uint8_t i = 0;
    uint8_t buf[7];
    while (Wire.available()) {
      buf[i++] = Wire.read();
    }
    if (!(buf[6] & 0x08)) {
      MgX = ((int16_t)buf[1] << 8) | buf[0];
      MgY = ((int16_t)buf[3] << 8) | buf[2];
      MgZ = ((int16_t)buf[5] << 8) | buf[4];
    }
  }
}


////////////
////Main////
////////////

void loop() {

//for(int j; j < 10000000; j++)
//{
//if (udp.parsePacket()) {
//char val = udp.read();
//val = int(val);

//Serial.print(val);
//if (val == 57){ 
//j == 100000000;
//}
//}
//else if(udp.parsePacket() == false){
//delay(10);
//Serial.println(j);
//}
//}
//}
//}

delay(20000);

//モーター起動
for(int i; i < 100; i++){
ledcWrite(0, 10);
ledcWrite(1, 10);  
ledcWrite(2, 10);
ledcWrite(3, 10);
delay(10);
ledcWrite(0, 0);
ledcWrite(1, 0);  
ledcWrite(2, 0);
ledcWrite(3, 0);
delay(10);
}

//モーターを一旦停止
ledcWrite(0, 0);
ledcWrite(1, 0);  
ledcWrite(2, 0);
ledcWrite(3, 0);

while(1) //ここからループ開始
{
unsigned long time;//「time」をunsigned longで変数宣言
time = millis();//プログラム実行からの経過時間(ms)をtimeに返す

//平均値格納//
structure Ac_Sens_Ave = {0,0,0};//加速度
structure Gy_Sens_Ave = {0,0,0};//角速度
structure Ahrs_Sens_Ave = {0,0,0};//姿勢角

for(int i = 0; i < FIFO_order; i++){
if (timeCounter1 > 0) {
  portENTER_CRITICAL(&timerMux);
  timeCounter1--;
  portEXIT_CRITICAL(&timerMux);

  ////MPU9250関連////
  //読み出し
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,16,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  readCompass();

  //実単位化(Acc/g->m/s^2,Gy/dps->rad/s)およびマドウィックフィルターによる角度推定
  MadgwickFilter.update(GyX/SF_Gy, GyY/SF_Gy, GyZ/SF_Gy, AcX/SF_Ac, AcY/SF_Ac, AcZ/SF_Ac, MgX/SF_Mg, MgY/SF_Mg, MgZ/SF_Mg);


  //書き出し
  //加速度について
  //Serial.print(AcX/SF_Ac*g2mpss);Serial.print(",");
  //Serial.print(AcY/SF_Ac*g2mpss);Serial.print(",");
  //Serial.print(AcZ/SF_Ac*g2mpss);Serial.print(","); 
  //姿勢角について
  //Serial.print(MadgwickFilter.getRoll()); Serial.print(",");
  //Serial.print(MadgwickFilter.getPitch()); Serial.print(",");
  //Serial.println(MadgwickFilter.getYaw());
  //Serial.print("\n");
  
  //センサー値を格納//
  //加速度(m/s^2)
  Ac_Sens[i].Axis_X = AcX/SF_Ac*g2mpss;
  Ac_Sens[i].Axis_Y = AcY/SF_Ac*g2mpss;
  if(g_total_num <= g_total_num_limit)
  {
    Ac_Sens[i].Axis_Z = AcZ/SF_Ac*g2mpss;//Z軸方向への加速度（重力を差し引く）- g2mpss
  }
  else if(g_total_num > g_total_num_limit)
  {
    Ac_Sens[i].Axis_Z = AcZ/SF_Ac*g2mpss - g2mpss_sens;//重力センサー値取得後
  }

  
  //角速度(deg/s)
  Gy_Sens[i].Axis_X = GyX/SF_Gy*deg2rad;
  Gy_Sens[i].Axis_Y = GyY/SF_Gy*deg2rad;
  Gy_Sens[i].Axis_Z = GyZ/SF_Gy*deg2rad;
  //姿勢角(deg)
  Ahrs_Sens[i].Axis_X = MadgwickFilter.getRoll();
  Ahrs_Sens[i].Axis_Y = MadgwickFilter.getPitch();
  Ahrs_Sens[i].Axis_Z = MadgwickFilter.getYaw();

  //平均値算出//
  //加速度(m/s^2)
  Ac_Sens_Ave.Axis_X += Ac_Sens[i].Axis_X / FIFO_order;
  Ac_Sens_Ave.Axis_Y += Ac_Sens[i].Axis_Y / FIFO_order;
  Ac_Sens_Ave.Axis_Z += Ac_Sens[i].Axis_Z / FIFO_order;  
  //角速度(deg/s)
  Gy_Sens_Ave.Axis_X += Gy_Sens[i].Axis_X / FIFO_order;
  Gy_Sens_Ave.Axis_Y += Gy_Sens[i].Axis_Y / FIFO_order;
  Gy_Sens_Ave.Axis_Z += Gy_Sens[i].Axis_Z / FIFO_order;
  //姿勢角(deg/s)
  Ahrs_Sens_Ave.Axis_X += Ahrs_Sens[i].Axis_X / FIFO_order;
  Ahrs_Sens_Ave.Axis_Y += Ahrs_Sens[i].Axis_Y / FIFO_order;
  Ahrs_Sens_Ave.Axis_Z += Ahrs_Sens[i].Axis_Z / FIFO_order;

//重力値取得
  if(g_k == 0)
  {
    if(g_total_num <= g_total_num_limit)
    {
      g_total_num++;
      g_average.Axis_X = (g_total_num * g_average.Axis_X + Ac_Sens[i].Axis_X) / (g_total_num + 1);
      g_average.Axis_Y = (g_total_num * g_average.Axis_Y + Ac_Sens[i].Axis_Y) / (g_total_num + 1);
      g_average.Axis_Z = (g_total_num * g_average.Axis_Z + Ac_Sens[i].Axis_Z) / (g_total_num + 1);
      g2mpss_sens = sqrt(g_average.Axis_X*g_average.Axis_X + g_average.Axis_Y*g_average.Axis_Y + g_average.Axis_Z*g_average.Axis_Z);
    }
    else if(g_total_num > g_total_num_limit)
    {
      g_k = 1;
    }
  }

//自動落下のタイムリミット設定
k ++;
//Serial.print(k);

if(k > k_PWMoff)
{
  for(int i = i_th_limit - 1; i >= 0 ; i-- )
  {
  if(i > 0)
  {
  ledcWrite(0, Motor_th * i / i_th_limit);
  ledcWrite(1, Motor_th * i / i_th_limit);  
  ledcWrite(2, Motor_th * i / i_th_limit);
  ledcWrite(3, Motor_th * i / i_th_limit);
  delay(1000);
  }
  else if(i == 0)
  {
  ledcWrite(0, Motor_th * i / i_th_limit);
  ledcWrite(1, Motor_th * i / i_th_limit);  
  ledcWrite(2, Motor_th * i / i_th_limit);
  ledcWrite(3, Motor_th * i / i_th_limit);
  delay(1000);
    //Serial.print("finish");
    delay(10000000);  
  }
    //Serial.print(i);
  }
}  

if (i = 4){
//推定値//
//速度(m/s)
Vl.Axis_X += Vl_Pre.Axis_X + Ac_Sens_Ave.Axis_X * dt * FIFO_order;
Vl.Axis_Y += Vl_Pre.Axis_Y + Ac_Sens_Ave.Axis_Y * dt * FIFO_order;
Vl.Axis_Z += Vl_Pre.Axis_Z + Ac_Sens_Ave.Axis_Z * dt * FIFO_order;
if(Vl.Axis_Z > Limit_Vl.Axis_Z)
{
  Vl.Axis_Z = Limit_Vl.Axis_Z;
}
else if(Vl.Axis_Z < -Limit_Vl.Axis_Z)
{
  Vl.Axis_Z = -Limit_Vl.Axis_Z;
}

//値の更新
Vl_Pre.Axis_X = Vl.Axis_X;
Vl_Pre.Axis_Y = Vl.Axis_Y;
Vl_Pre.Axis_Y = Vl.Axis_Z;

////方向指示////    
if (udp.parsePacket()) {
  char val = udp.read();
  val = int(val);
  
  if (val == 48){//維持
    Ahrs_Tgt = Ahrs_Tgt_Keep;//維持
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;
    Ac_Tgt = Ac_Tgt_Keep;//維持
  }

  else if (val == 49){//前
    Ahrs_Tgt = Ahrs_Tgt_Front;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;    
    Ac_Tgt.Axis_Z = Ac_Tgt_Keep.Axis_Z;//高さのみ維持
  }

  else if (val == 50){//後
    Ahrs_Tgt = Ahrs_Tgt_Back;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;    
    Ac_Tgt.Axis_Z = Ac_Tgt_Keep.Axis_Z;//高さのみ維持
  }

  else if (val == 51){//右
    Ahrs_Tgt = Ahrs_Tgt_Right;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;
    Ac_Tgt.Axis_Z = Ac_Tgt_Keep.Axis_Z;//高さのみ維持
  }

  else if (val == 52){//左
    Ahrs_Tgt = Ahrs_Tgt_Left;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;
    Ac_Tgt.Axis_Z = Ac_Tgt_Keep.Axis_Z;//高さのみ維持
  }

  else if (val == 53){//上昇
    Ahrs_Tgt = Ahrs_Tgt_Keep;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;
    Ac_Tgt = Ac_Tgt_Up;
  }

  else if (val == 54){//下降
    Ahrs_Tgt = Ahrs_Tgt_Keep;
    Ahrs_Tgt.Axis_Z = Ahrs_Sens_Ave.Axis_Z;
    Ac_Tgt = Ac_Tgt_Down;
  }

  else if (val == 57){//スイッチオフ
    ledcWrite(0, 30);
    ledcWrite(1, 30);  
    ledcWrite(2, 30);
    ledcWrite(3, 30);
    delay(1000);
    ledcWrite(0, 10);
    ledcWrite(1, 10);  
    ledcWrite(2, 10);
    ledcWrite(3, 10);
    delay(1000);
    ledcWrite(0, 0);
    ledcWrite(1, 0);  
    ledcWrite(2, 0);
    ledcWrite(3, 0);  
    delay(10000000);
  }
  
val_2 = val;

k = 0;//kのリセット

}

////誤差////
//姿勢角(deg)
error_Ahrs.Axis_X = Ahrs_Tgt.Axis_X - Ahrs_Sens_Ave.Axis_X; 
error_Ahrs.Axis_Y = Ahrs_Tgt.Axis_Y - Ahrs_Sens_Ave.Axis_Y;
error_Ahrs.Axis_Z = Ahrs_Tgt.Axis_Z - Ahrs_Sens_Ave.Axis_Z;

//加速度(m/s)
error_Ac.Axis_Z = Ac_Tgt.Axis_Z - Ac_Sens_Ave.Axis_Z;

//Serial.print(error_Vl.Axis_Z); Serial.print("  ");Serial.print(Vl_Tgt.Axis_Z); Serial.print("  ");Serial.print(Vl.Axis_Z); Serial.print("  ");

////PID制御////
///PID速度///
//Z Axis
//Pid_Integ_Vl.Axis_Z += error_Vl.Axis_Z * dt * FIFO_order;
//if (Pid_Integ_Vl.Axis_Z > Pid_Integ_Limit_Vl.Axis_Z){
//  Pid_Integ_Vl.Axis_Z = Pid_Integ_Limit_Vl.Axis_Z;
//  }
//else if (Pid_Integ_Vl.Axis_Z < -1 * Pid_Integ_Limit_Vl.Axis_Z){
//  Pid_Integ_Vl.Axis_Z = -1 * Pid_Integ_Limit_Vl.Axis_Z;
//  }
//Pid_Vl.Axis_Z = error_Vl.Axis_Z * Kp_Vl.Axis_Z + Pid_Integ_Vl.Axis_Z * Ki_Vl.Axis_Z;

///PID加速度///
//Z Axis
Pid_Integ_Ac.Axis_Z += error_Ac.Axis_Z * dt * FIFO_order;
if (Pid_Integ_Ac.Axis_Z > Pid_Integ_Limit_Ac.Axis_Z){
  Pid_Integ_Ac.Axis_Z = Pid_Integ_Limit_Ac.Axis_Z;
  }
else if (Pid_Integ_Ac.Axis_Z < -1 * Pid_Integ_Limit_Ac.Axis_Z){
  Pid_Integ_Ac.Axis_Z = -1 * Pid_Integ_Limit_Ac.Axis_Z;
  }
Pid_Derive_Ac.Axis_Z = (error_Ac.Axis_Z - error_Ac_Pre.Axis_Z) * Deno;
error_Ac_Pre.Axis_Z = error_Ac.Axis_Z;
Pid_Derive_Ac.Axis_Z = Pid_Derive_Ac_Pre.Axis_Z + (Pid_Derive_Ac.Axis_Z - Pid_Derive_Ac_Pre.Axis_Z) * D_Filter_C_Gy;
Pid_Derive_Ac_Pre.Axis_Z = Pid_Derive_Ac.Axis_Z;

Pid_Ac.Axis_Z = Kp_Ac.Axis_Z * error_Ac.Axis_Z + Ki_Ac.Axis_Z * Pid_Integ_Ac.Axis_Z + Kd_Ac.Axis_Z * Pid_Derive_Ac.Axis_Z;//PID全体計算
if (Pid_Ac.Axis_Z > Pid_Limit_Ac.Axis_Z){
  Pid_Ac.Axis_Z = Pid_Limit_Ac.Axis_Z;
  }
else if (Pid_Ac.Axis_Z < -Pid_Limit_Ac.Axis_Z){
  Pid_Ac.Axis_Z = -Pid_Limit_Ac.Axis_Z;
  }


//Motor_th調整
if(i_up == 0)
{
  if(val_2 == 53)
  {
  Motor_th += 1;
  if(Motor_th >= Motor_th_limit)
    {
      i_up = 1;
      if(Ac_Sens_Ave.Axis_Z - g2mpss_sens > Ac_Hovering)
      {
        for(int i_up_num = 0; i_up_num < i_up_num_limit ; i_up_num ++)
        {
        if(i_up_num >= i_up_num_limit)
          {
            i_up = 1;
          }
        }
      }
    }
  }
}

//Motor_th = Pid_Ac.Axis_Z;//「トリム値」もともとMotor_th = Pid_Ac.Axis_Z;

//Serial.print(Ac_Sens_Ave.Axis_Z);Serial.print("   ");
//Serial.print(error_Ac.Axis_Z);Serial.print("   ");

///PID姿勢角//
//X Axis
Pid_Integ_Ahrs.Axis_X += error_Ahrs.Axis_X * dt * FIFO_order;
if (Pid_Integ_Ahrs.Axis_X > Pid_Integ_Limit_Ahrs.Axis_X){
  Pid_Integ_Ahrs.Axis_X = Pid_Integ_Limit_Ahrs.Axis_X;
  }
else if (Pid_Integ_Ahrs.Axis_X < -1 * Pid_Integ_Limit_Ahrs.Axis_X){
  Pid_Integ_Ahrs.Axis_X = -1 * Pid_Integ_Limit_Ahrs.Axis_X;
  }
Pid_Derive_Ahrs.Axis_X = (error_Ahrs.Axis_X - error_Ahrs_Pre.Axis_X) * Deno;
error_Ahrs_Pre.Axis_X = error_Ahrs.Axis_X;
Pid_Derive_Ahrs.Axis_X = Pid_Derive_Ahrs_Pre.Axis_X + (Pid_Derive_Ahrs.Axis_X - Pid_Derive_Ahrs_Pre.Axis_X) * D_Filter_C_Gy;
Pid_Derive_Ahrs_Pre.Axis_X = Pid_Derive_Ahrs.Axis_X;

Pid_Ahrs.Axis_X = Kp_Ahrs.Axis_X * error_Ahrs.Axis_X + Ki_Ahrs.Axis_X * Pid_Integ_Ahrs.Axis_X + Kd_Ahrs.Axis_X * Pid_Derive_Gy.Axis_X;

if (Pid_Ahrs.Axis_X > Pid_Limit_Ahrs.Axis_X){
  Pid_Ahrs.Axis_X = Pid_Limit_Ahrs.Axis_X;
  }
else if (Pid_Ahrs.Axis_X < -Pid_Limit_Ahrs.Axis_X){
  Pid_Ahrs.Axis_X = -Pid_Limit_Ahrs.Axis_X;
  }

//Y axis
Pid_Integ_Ahrs.Axis_Y += error_Ahrs.Axis_Y * dt * FIFO_order;
if (Pid_Integ_Ahrs.Axis_Y > Pid_Integ_Limit_Ahrs.Axis_Y){
  Pid_Integ_Ahrs.Axis_Y = Pid_Integ_Limit_Ahrs.Axis_Y;
  }
else if (Pid_Integ_Ahrs.Axis_Y < -1 * Pid_Integ_Limit_Ahrs.Axis_Y){
  Pid_Integ_Ahrs.Axis_Y = -1 * Pid_Integ_Limit_Ahrs.Axis_Y;
  }
Pid_Derive_Ahrs.Axis_Y = (error_Ahrs.Axis_Y - error_Ahrs_Pre.Axis_Y) * Deno;
error_Ahrs_Pre.Axis_Y = error_Ahrs.Axis_Y;
Pid_Derive_Ahrs.Axis_Y = Pid_Derive_Ahrs_Pre.Axis_Y + (Pid_Derive_Ahrs.Axis_Y - Pid_Derive_Ahrs_Pre.Axis_Y) * D_Filter_C_Gy;
Pid_Derive_Ahrs_Pre.Axis_Y = Pid_Derive_Ahrs.Axis_Y;

Pid_Ahrs.Axis_Y = Kp_Ahrs.Axis_Y * error_Ahrs.Axis_Y + Ki_Ahrs.Axis_Y * Pid_Integ_Ahrs.Axis_Y + Kd_Ahrs.Axis_Y * Pid_Derive_Ahrs.Axis_Y;

if (Pid_Ahrs.Axis_Y > Pid_Limit_Ahrs.Axis_Y){
  Pid_Ahrs.Axis_Y = Pid_Limit_Ahrs.Axis_Y;
  }
else if (Pid_Ahrs.Axis_Y < -Pid_Limit_Ahrs.Axis_Y){
  Pid_Ahrs.Axis_Y = -Pid_Limit_Ahrs.Axis_Y;
  }

//Z axis
Pid_Integ_Ahrs.Axis_Z += error_Ahrs.Axis_Z * dt * FIFO_order;
if (Pid_Integ_Ahrs.Axis_Z > Pid_Integ_Limit_Ahrs.Axis_Z){
  Pid_Integ_Ahrs.Axis_Z = Pid_Integ_Limit_Ahrs.Axis_Z;
  }
else if (Pid_Integ_Ahrs.Axis_Z < -1 * Pid_Integ_Limit_Ahrs.Axis_Z){
  Pid_Integ_Ahrs.Axis_Z = -1 * Pid_Integ_Limit_Ahrs.Axis_Z;
  }
Pid_Derive_Ahrs.Axis_Z = (error_Ahrs.Axis_Z - error_Ahrs_Pre.Axis_Z) * Deno;
error_Ahrs_Pre.Axis_Z = error_Ahrs.Axis_Z;
Pid_Derive_Ahrs.Axis_Z = Pid_Derive_Ahrs_Pre.Axis_Z + (Pid_Derive_Ahrs.Axis_Z - Pid_Derive_Ahrs_Pre.Axis_Z) * D_Filter_C_Gy;
Pid_Derive_Ahrs_Pre.Axis_Z = Pid_Derive_Ahrs.Axis_Z;

Pid_Ahrs.Axis_Z = Kp_Ahrs.Axis_Z * error_Ahrs.Axis_Z + Ki_Ahrs.Axis_Z * Pid_Integ_Ahrs.Axis_Z + Kd_Ahrs.Axis_Z * Pid_Derive_Ahrs.Axis_Z;

if (Pid_Ahrs.Axis_Z > Pid_Limit_Ahrs.Axis_Z){
  Pid_Ahrs.Axis_Z = Pid_Limit_Ahrs.Axis_Z;
  }
else if (Pid_Ahrs.Axis_Z < -Pid_Limit_Ahrs.Axis_Z){
  Pid_Ahrs.Axis_Z = -Pid_Limit_Ahrs.Axis_Z;
  }


///PID角速度//
//X Axis
error_Gy.Axis_X = Pid_Ahrs.Axis_X - Gy_Sens_Ave.Axis_X;
Pid_Integ_Gy.Axis_X += error_Gy.Axis_X * dt * FIFO_order;
if (Pid_Integ_Gy.Axis_X > Pid_Integ_Limit_Gy.Axis_X){
  Pid_Integ_Gy.Axis_X = Pid_Integ_Limit_Gy.Axis_X;
  }
else if (Pid_Integ_Gy.Axis_X < -1 * Pid_Integ_Limit_Gy.Axis_X){
  Pid_Integ_Gy.Axis_X = -1 * Pid_Integ_Limit_Gy.Axis_X;
  }
Pid_Derive_Gy.Axis_X = (error_Gy.Axis_X - error_Gy_Pre.Axis_X) * Deno;
error_Gy_Pre.Axis_X = error_Gy.Axis_X;
Pid_Derive_Gy.Axis_X = Pid_Derive_Gy_Pre.Axis_X + (Pid_Derive_Gy.Axis_X - Pid_Derive_Gy_Pre.Axis_X) * D_Filter_C_Gy;
Pid_Derive_Gy_Pre.Axis_X = Pid_Derive_Gy.Axis_X;

Pid_Gy.Axis_X = Kp_Gy.Axis_X * error_Gy.Axis_X + Ki_Gy.Axis_X * Pid_Integ_Gy.Axis_X + Kd_Gy.Axis_X * Pid_Derive_Gy.Axis_X;//PID全体計算
if (Pid_Gy.Axis_X > Pid_Limit_Gy.Axis_X){
  Pid_Gy.Axis_X = Pid_Limit_Gy.Axis_X;
  }
else if (Pid_Gy.Axis_X < -Pid_Limit_Gy.Axis_X){
  Pid_Gy.Axis_X = -Pid_Limit_Gy.Axis_X;
  }


//Y Axis
error_Gy.Axis_Y = Pid_Ahrs.Axis_Y - Gy_Sens_Ave.Axis_Y;
Pid_Integ_Gy.Axis_Y += error_Gy.Axis_Y * dt * FIFO_order;
if (Pid_Integ_Gy.Axis_Y > Pid_Integ_Limit_Gy.Axis_Y){
  Pid_Integ_Gy.Axis_Y = Pid_Integ_Limit_Gy.Axis_Y;
  }
else if (Pid_Integ_Gy.Axis_Y < -1 * Pid_Integ_Limit_Gy.Axis_Y){
  Pid_Integ_Gy.Axis_Y = -1 * Pid_Integ_Limit_Gy.Axis_Y;
  }
Pid_Derive_Gy.Axis_Y = (error_Gy.Axis_Y - error_Gy_Pre.Axis_Y) * Deno;
error_Gy_Pre.Axis_Y = error_Gy.Axis_Y;
Pid_Derive_Gy.Axis_Y = Pid_Derive_Gy_Pre.Axis_Y + (Pid_Derive_Gy.Axis_Y - Pid_Derive_Gy_Pre.Axis_Y) * D_Filter_C_Gy;
Pid_Derive_Gy_Pre.Axis_Y = Pid_Derive_Gy.Axis_Y;

Pid_Gy.Axis_Y = Kp_Gy.Axis_Y * error_Gy.Axis_Y + Ki_Gy.Axis_Y * Pid_Integ_Gy.Axis_Y + Kd_Gy.Axis_Y * Pid_Derive_Gy.Axis_Y;//PID全体計算
if (Pid_Gy.Axis_Y > Pid_Limit_Gy.Axis_Y){
  Pid_Gy.Axis_Y = Pid_Limit_Gy.Axis_Y;
  }
else if (Pid_Gy.Axis_Y < -Pid_Limit_Gy.Axis_Y){
  Pid_Gy.Axis_Y = -Pid_Limit_Gy.Axis_Y;
  }

//Z Axis
error_Gy.Axis_Z = Pid_Ahrs.Axis_Z - Gy_Sens_Ave.Axis_Z;
Pid_Integ_Gy.Axis_Z += error_Gy.Axis_Z * dt * FIFO_order;
if (Pid_Integ_Gy.Axis_Z > Pid_Integ_Limit_Gy.Axis_Z){
  Pid_Integ_Gy.Axis_Z = Pid_Integ_Limit_Gy.Axis_Z;
  }
else if (Pid_Integ_Gy.Axis_Z < -1 * Pid_Integ_Limit_Gy.Axis_Z){
  Pid_Integ_Gy.Axis_Z = -1 * Pid_Integ_Limit_Gy.Axis_Z;
  }
Pid_Derive_Gy.Axis_Z = (error_Gy.Axis_Z - error_Gy_Pre.Axis_Z) * Deno;
error_Gy_Pre.Axis_Z = error_Gy.Axis_Z;
Pid_Derive_Gy.Axis_Z = Pid_Derive_Gy_Pre.Axis_Z + (Pid_Derive_Gy.Axis_Z - Pid_Derive_Gy_Pre.Axis_Z) * D_Filter_C_Gy;
Pid_Derive_Gy_Pre.Axis_Z = Pid_Derive_Gy.Axis_Z;

Pid_Gy.Axis_Z = Kp_Gy.Axis_Z * error_Gy.Axis_Z + Ki_Gy.Axis_Z * Pid_Integ_Gy.Axis_Z + Kd_Gy.Axis_Z * Pid_Derive_Gy.Axis_Z;//PID全体計算
if (Pid_Gy.Axis_Z > Pid_Limit_Gy.Axis_Z){
  Pid_Gy.Axis_Z = Pid_Limit_Gy.Axis_Z;
  }
else if (Pid_Gy.Axis_Z < -Pid_Limit_Gy.Axis_Z){
  Pid_Gy.Axis_Z = -Pid_Limit_Gy.Axis_Z;
  }

PWM_Motor_1 = (Motor_th + Pid_Ac.Axis_Z) - Pid_Gy.Axis_X - Pid_Gy.Axis_Y + Pid_Gy.Axis_Z + Motor_offset[0];
  if(PWM_Motor_1 < 0){
    PWM_Motor_1 = 0;
  }
  else if(PWM_Motor_1 > PWM_Motor_Limit){
    PWM_Motor_1 = PWM_Motor_Limit;
  }
  
PWM_Motor_2 = (Motor_th + Pid_Ac.Axis_Z) + Pid_Gy.Axis_X - Pid_Gy.Axis_Y - Pid_Gy.Axis_Z + Motor_offset[1];
  if(PWM_Motor_2 < 0){
    PWM_Motor_2 = 0;
  }
  else if(PWM_Motor_2 > PWM_Motor_Limit){
    PWM_Motor_2 = PWM_Motor_Limit;
  }

PWM_Motor_3 = (Motor_th + Pid_Ac.Axis_Z) + Pid_Gy.Axis_X + Pid_Gy.Axis_Y + Pid_Gy.Axis_Z + Motor_offset[2];
  if(PWM_Motor_3 < 0){
    PWM_Motor_3 = 0;
  }
  else if(PWM_Motor_3 > PWM_Motor_Limit){
    PWM_Motor_3 = PWM_Motor_Limit;
  }
  
PWM_Motor_4 = (Motor_th + Pid_Ac.Axis_Z) - Pid_Gy.Axis_X + Pid_Gy.Axis_Y - Pid_Gy.Axis_Z + Motor_offset[3];
  if(PWM_Motor_4 < 0){
    PWM_Motor_4 = 0;
  }
  else if(PWM_Motor_4 > PWM_Motor_Limit){
    PWM_Motor_4 = PWM_Motor_Limit;
  }

//Serial.print(Motor_th);Serial.print(", ");

Serial.print(PWM_Motor_1);Serial.print(", ");
Serial.print(PWM_Motor_2);Serial.print(", ");
Serial.print(PWM_Motor_3);Serial.print(", ");
Serial.print(PWM_Motor_4);Serial.println(", ");
//Serial.println(val_2);

//モーターへの出力
ledcWrite(0, PWM_Motor_1);
ledcWrite(1, PWM_Motor_2);  
ledcWrite(2, PWM_Motor_3);
ledcWrite(3, PWM_Motor_4);
}
}
}
}
}

PC and Playstation3 controller side source code

#参考https://rikoubou.hatenablog.com/entry/2017/10/06/181805
#参考https://greenhornprofessional.hatenablog.com/entry/2020/09/13/223155

from __future__ import print_function
import socket
import time
from time import sleep
from contextlib import closing
import pygame
from pygame.locals import *

###ジョイスティック側
# ジョイスティックの初期化
pygame.joystick.init()
try:
   # ジョイスティックインスタンスの生成
   joystick = pygame.joystick.Joystick(0)
   joystick.init()
   print('ジョイスティックの名前:', joystick.get_name())
   print('ボタン数 :', joystick.get_numbuttons())
except pygame.error:
   print('ジョイスティックが接続されていません')

# pygameの初期化
pygame.init()

# 画面の生成
SCREEN_SIZE = (640, 480)  # 画面サイズ (横/縦)
STEP = 10       #キーボード/ハットスイッチの反応の良さ
X_CENTER = int(SCREEN_SIZE[0]/2)
Y_CENTER = int(SCREEN_SIZE[1]/2)

screen = pygame.display.set_mode(SCREEN_SIZE) 
screen2 = pygame.display.set_mode(SCREEN_SIZE) 

screen.fill((0,0,0))
screen2.fill((0,0,0))

#通信部分
def main():
  host = '192.168.4.1' # IPアドレス
  port = 10000 # ポート番号
  sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #AFINETは、V4を使うという意味、DGRAMはUDP指定の意味
  with closing(sock):
    while True:
        
   # イベントの取得
       for e in pygame.event.get():
       # 終了ボタン
           if e.type == QUIT:
               active = False
    
           # ジョイスティックのボタンの入力
           if e.type == pygame.locals.JOYAXISMOTION:
               print(joystick.get_axis(0), -1*joystick.get_axis(1), joystick.get_axis(2), -1*joystick.get_axis(3))

               command0 = joystick.get_axis(0)+1
               command0 = str(round(command0))
               print(command0)
               print(type(command0))

               command1 = -joystick.get_axis(1)+1
               command1 = str(round(command1))
               print(command1)
               print(type(command1))
               
               command2 = joystick.get_axis(2)+1
               command2 = str(round(command2))
               print(command2)
               print(type(command2))

               command3 = -joystick.get_axis(3)+1
               command3 = str(round(command3))
               print(command3)
               print(type(command3))
               
               command = command0 + command1 + command2 + command3
               
               if command == "1211": #上
                       command = "5"

               elif command == "1011": #下
                       command = "6"
                
               elif command == "2111": #
                       command = "0"                       

               elif command == "0111": #
                       command = "0"

               elif command == "1112": #前
                       command = "1"                

               elif command == "1110": #後
                       command = "2" 

               elif command == "1121": #右
                       command = "3"

               elif command == "1101": #左
                       command = "4"

               elif command == "2101": #スイッチオン（内側コマンド）
                       command = "9"

                    
               else: #継続
                       command = "0"

               print(command)
               command = str(command).encode("UTF-8")
               print(command)
                
               
                  
               sock.sendto(command,(host, port))
               
               time.sleep(0.01)
        
               #sock.sendto(bytes(command, encoding='ascii'), (host, port))



    return

"""
      message = 'A'.encode('utf-8') # 送る文字列(ここでは「A」という文字列を送っている)
      print(message)
      sock.sendto(message, (host, port))
      time.sleep(1)
"""
    #return

if __name__ == '__main__':
  main()