18bai-Kaiohken/M5stickで倒立振子.cpp

## M5stickで倒立振子.cpp
//ライブラリのインポート
#include <Arduino.h>
#include <M5Unified.h>//6軸センサの値取得時に使う
#include <MadgwickAHRS.h>//姿勢推定に用いる
#include <WiFi.h>
#include <ESPAsyncWebServer.h>


// WiFi設定---------------------------------------------
const char* ssid = "aterm-23841f-g";
const char* password = "14b3384e2cc60";
// Webサーバーのインスタンスを作成
AsyncWebServer server(80);


// HTMLページの定義（プルダウンメニュー付き）
const char index_html[] = R"rawliteral(
  <!DOCTYPE html>
  <html>
  <head>
      <title>PID setup</title>
  </head>
  <body>
      <h2></h2>

      <label for="parameter">parameter:</label>
      <select id="parameter">
          <option value="Kp">Kp</option>
          <option value="Ki">Ki</option>
          <option value="Kd">Kd</option>
          <option value="V_MIN">V_MIN</option>
          <option value="target">target</option>
      </select><br>

      <label for="value">value:</label>
      <input type="number" id="value"><br>

      <button onclick="sendData()">send</button>

      <script>
          function sendData() {
              let param = document.getElementById("parameter").value;
              let value = document.getElementById("value").value;
              fetch(`/send?param=${param}&value=${value}`);
          }
      </script>
  </body>
  </html>
  )rawliteral";

char control[80];
//wifi設定ここまで----------------------------------


//ピン設定-----------------
int motorPIN1 = 25;
int motorPIN2 = 26;
//------------------------


//姿勢推定用---------------
Madgwick MadgwickFilter;

float ax,ay,az;
float gx,gy,gz;
float roll,pitch,yaw;
//------------------------


//PID用-----------------------
float P,I,D,preP;
float Kp = 5;
float Ki = 70;
float Kd = 0.1;

float target = -0.75;        // 目標値。モジュールが横置きなら0前後、縦置きなら90前後
float iLimit = 255;
float dt, preTime;
//---------------------------


//モーター駆動用--------------
int PID;
int power1;
int power2;
int pwm;
int V_MIN = 70;
int V_MAX = 255;
float frontGain = 1;
//---------------------------


//PWM関連---------------------
const double PWM_Hz = 2000;   // PWM周波数
const uint8_t PWM_level = 8; // PWM分解能 16bit(1～256)
//----------------------------


//状態切り替え------
bool wifi = false;
bool move = false;
//-----------------


void setup(){
  delay(1000);
  auto cfg = M5.config();   // M5Stack の初期化
  M5.begin(cfg);
  Serial.begin(115200);

  //Madgwickフィルタ初期化
  MadgwickFilter.begin(100);//フィルタのサンプリングレートを設定(今回は10Hz)

  //出力ピン設定
  pinMode(motorPIN1,OUTPUT);
  pinMode(motorPIN2,OUTPUT);

  // チャンネルと周波数の分解能を設定
  ledcSetup(1, PWM_Hz, PWM_level);
  ledcSetup(2, PWM_Hz, PWM_level);

  // モータのピンとチャンネルの設定
  ledcAttachPin(25, 1);
  ledcAttachPin(26, 2);


  delay(1000);

}


void loop(){
  M5.update();
  if (M5.BtnA.wasPressed()){
    if (wifi == false ){
      wifi = true;
      move = false;
      PID = 0;
      pwm = 0;
      I = 0;
      ledcWrite(1,0);
      ledcWrite(2,0);
      WiFi.begin(ssid, password);  //WiFi接続開始の呼び出しを追加
      while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.println("接続中...");
      }
      Serial.println("WiFi接続完了");
      Serial.print("IPアドレス: ");
      Serial.println(WiFi.localIP());

      // ここでルートパス "/" のハンドラーを追加
      server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
        request->send_P(200, "text/html", index_html);
      });

      // パラメーターと値を受け取る処理
      server.on("/send", HTTP_GET, [](AsyncWebServerRequest *request){
      if (request->hasParam("param") && request->hasParam("value")) {
          String param = request->getParam("param")->value();
          float value = request->getParam("value")->value().toFloat();

          // 受信した値を適切な変数に格納
          if (param == "Kp") {
              Kp = value;
          } else if (param == "Ki") {
              Ki = value;
          } else if (param == "Kd") {
              Kd = value;
          } else if (param == "V_MIN") {
              V_MIN = value;
          } else if (param == "target") {
            target = value;
          }
          // シリアルモニタに出力
          Serial.println("現在のPIDパラメータ:");
          sprintf(control, "Kp:%f, Ki:%f, Kd:%f,V_MIN:%d", Kp,Ki,Kd,V_MIN);
          Serial.println(control);
      }
      request->send(200, "text/plain", "OK");
  });
  server.begin();
    }else{
      wifi = false;
      Serial.println("サーバーを停止します");
        server.end();
        WiFi.disconnect();
        Serial.println("WiFi接続を終了しました");
      move = true;
    }
  }
  if (move == true){
    //姿勢推定--------------------------------------------------
    M5.Imu.getGyro(&gx, &gy, &gz);//ジャイロセンサの値を取得
    M5.Imu.getAccel(&ax, &ay, &az);//加速度センサの値を取得

    MadgwickFilter.updateIMU(gx,gy,gz,ax,ay,az); //センサの値をMadgwickフィルタに与える

    //ピッチ角を計算
    pitch = MadgwickFilter.getPitch();
    //---------------------------------------------------------

    //PID制御--------------------------------------------------
    dt = (micros() - preTime) * 0.000001;  // 処理時間を求める
    preTime = micros();                   // 処理時間を記録

    P = (pitch - target) * Kp;
    I += P * dt;                    // 偏差を積分する
    D  = (P - preP) / dt;           // 偏差を微分する
    preP = P;                       // 偏差を記録する

    // 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
    if (iLimit/Ki < abs(I)){
      I = iLimit/Ki;
    }
    //---------------------------------------------------------


    //モーター制御----------------------------------------------
    PID = (int)(Kp * P + Ki * I + Kd * D );
    pwm = (int)(constrain((int)(abs(PID)+V_MIN), V_MIN, V_MAX)); //255に制限　飽和する
    if (PID >= 0){
      ledcWrite(1,pwm*frontGain);
      ledcWrite(2,0);
    }else{
      ledcWrite(1,0);
      ledcWrite(2,pwm);
    }
    //---------------------------------------------------------
  delay(10);

}
}
	//ライブラリのインポート
	#include <Arduino.h>
	#include <M5Unified.h>//6軸センサの値取得時に使う
	#include <MadgwickAHRS.h>//姿勢推定に用いる
	#include <WiFi.h>
	#include <ESPAsyncWebServer.h>


	// WiFi設定---------------------------------------------
	const char* ssid = "aterm-23841f-g";
	const char* password = "14b3384e2cc60";
	// Webサーバーのインスタンスを作成
	AsyncWebServer server(80);


	// HTMLページの定義（プルダウンメニュー付き）
	const char index_html[] = R"rawliteral(
	<!DOCTYPE html>
	<html>
	<head>
	<title>PID setup</title>
	</head>
	<body>
	<h2></h2>

	<label for="parameter">parameter:</label>
	<select id="parameter">
	<option value="Kp">Kp</option>
	<option value="Ki">Ki</option>
	<option value="Kd">Kd</option>
	<option value="V_MIN">V_MIN</option>
	<option value="target">target</option>
	</select><br>

	<label for="value">value:</label>
	<input type="number" id="value"><br>

	<button onclick="sendData()">send</button>

	<script>
	function sendData() {
	let param = document.getElementById("parameter").value;
	let value = document.getElementById("value").value;
	fetch(`/send?param=${param}&value=${value}`);
	}
	</script>
	</body>
	</html>
	)rawliteral";

	char control[80];
	//wifi設定ここまで----------------------------------


	//ピン設定-----------------
	int motorPIN1 = 25;
	int motorPIN2 = 26;
	//------------------------


	//姿勢推定用---------------
	Madgwick MadgwickFilter;

	float ax,ay,az;
	float gx,gy,gz;
	float roll,pitch,yaw;
	//------------------------


	//PID用-----------------------
	float P,I,D,preP;
	float Kp = 5;
	float Ki = 70;
	float Kd = 0.1;

	float target = -0.75; // 目標値。モジュールが横置きなら0前後、縦置きなら90前後
	float iLimit = 255;
	float dt, preTime;
	//---------------------------


	//モーター駆動用--------------
	int PID;
	int power1;
	int power2;
	int pwm;
	int V_MIN = 70;
	int V_MAX = 255;
	float frontGain = 1;
	//---------------------------


	//PWM関連---------------------
	const double PWM_Hz = 2000; // PWM周波数
	const uint8_t PWM_level = 8; // PWM分解能 16bit(1～256)
	//----------------------------


	//状態切り替え------
	bool wifi = false;
	bool move = false;
	//-----------------


	void setup(){
	delay(1000);
	auto cfg = M5.config(); // M5Stack の初期化
	M5.begin(cfg);
	Serial.begin(115200);

	//Madgwickフィルタ初期化
	MadgwickFilter.begin(100);//フィルタのサンプリングレートを設定(今回は10Hz)

	//出力ピン設定
	pinMode(motorPIN1,OUTPUT);
	pinMode(motorPIN2,OUTPUT);

	// チャンネルと周波数の分解能を設定
	ledcSetup(1, PWM_Hz, PWM_level);
	ledcSetup(2, PWM_Hz, PWM_level);

	// モータのピンとチャンネルの設定
	ledcAttachPin(25, 1);
	ledcAttachPin(26, 2);


	delay(1000);

	}


	void loop(){
	M5.update();
	if (M5.BtnA.wasPressed()){
	if (wifi == false ){
	wifi = true;
	move = false;
	PID = 0;
	pwm = 0;
	I = 0;
	ledcWrite(1,0);
	ledcWrite(2,0);
	WiFi.begin(ssid, password); //WiFi接続開始の呼び出しを追加
	while (WiFi.status() != WL_CONNECTED) {
	delay(1000);
	Serial.println("接続中...");
	}
	Serial.println("WiFi接続完了");
	Serial.print("IPアドレス: ");
	Serial.println(WiFi.localIP());

	// ここでルートパス "/" のハンドラーを追加
	server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
	request->send_P(200, "text/html", index_html);
	});

	// パラメーターと値を受け取る処理
	server.on("/send", HTTP_GET, [](AsyncWebServerRequest *request){
	if (request->hasParam("param") && request->hasParam("value")) {
	String param = request->getParam("param")->value();
	float value = request->getParam("value")->value().toFloat();

	// 受信した値を適切な変数に格納
	if (param == "Kp") {
	Kp = value;
	} else if (param == "Ki") {
	Ki = value;
	} else if (param == "Kd") {
	Kd = value;
	} else if (param == "V_MIN") {
	V_MIN = value;
	} else if (param == "target") {
	target = value;
	}
	// シリアルモニタに出力
	Serial.println("現在のPIDパラメータ:");
	sprintf(control, "Kp:%f, Ki:%f, Kd:%f,V_MIN:%d", Kp,Ki,Kd,V_MIN);
	Serial.println(control);
	}
	request->send(200, "text/plain", "OK");
	});
	server.begin();
	}else{
	wifi = false;
	Serial.println("サーバーを停止します");
	server.end();
	WiFi.disconnect();
	Serial.println("WiFi接続を終了しました");
	move = true;
	}
	}
	if (move == true){
	//姿勢推定--------------------------------------------------
	M5.Imu.getGyro(&gx, &gy, &gz);//ジャイロセンサの値を取得
	M5.Imu.getAccel(&ax, &ay, &az);//加速度センサの値を取得

	MadgwickFilter.updateIMU(gx,gy,gz,ax,ay,az); //センサの値をMadgwickフィルタに与える

	//ピッチ角を計算
	pitch = MadgwickFilter.getPitch();
	//---------------------------------------------------------

	//PID制御--------------------------------------------------
	dt = (micros() - preTime) * 0.000001; // 処理時間を求める
	preTime = micros(); // 処理時間を記録

	P = (pitch - target) * Kp;
	I += P * dt; // 偏差を積分する
	D = (P - preP) / dt; // 偏差を微分する
	preP = P; // 偏差を記録する

	// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
	if (iLimit/Ki < abs(I)){
	I = iLimit/Ki;
	}
	//---------------------------------------------------------


	//モーター制御----------------------------------------------
	PID = (int)(Kp * P + Ki * I + Kd * D );
	pwm = (int)(constrain((int)(abs(PID)+V_MIN), V_MIN, V_MAX)); //255に制限　飽和する
	if (PID >= 0){
	ledcWrite(1,pwm*frontGain);
	ledcWrite(2,0);
	}else{
	ledcWrite(1,0);
	ledcWrite(2,pwm);
	}
	//---------------------------------------------------------
	delay(10);

	}
	}
No results found