倒立振子パラメータ調整の準備まで完了

倒立振子の製作を始めてますが、PID制御のパラメータ設定を簡単に行えるような準備も整ったのであとはコード全体を書いて調整に取り掛かるだけ。

PIDパラメータは調整のおおまかな方針は決まっているようですが、最後の微調整はカットアンドトライになるので、いちいちソースコンパイル、ダウンロードでは効率が悪いので、ダイナミック(on the fly)でパラメータ書き換えの手段が必須になります。

やり方はM5Stack側はUDP受信で、送信側はProcessingを使って更新します。

<Processingのコード>

/*
set PID parametor to the m5stack using UDP

*/
import hypermedia.net.*;
import controlP5.*;

UDP udp;
ControlP5 cp5;

final String IP = "192.168.1.15";
final int PORT = 3002;
String msg = "10, 3, 1";  // P, I, D

void setup() {
  size(200, 200);

  cp5 = new ControlP5(this);
  udp = new UDP( this, 3002 );

  ControlFont cf = new ControlFont(createFont("Serif",20));

  cp5.addButton("UDP_Msg")
    .setFont(cf)
    .setLabel("send")
    .setPosition(50,50)
    .setSize(100,100);
}

void draw() {
  background(200);
}

void UDP_Msg(){
  udp.send(msg, IP, PORT);
}

SENDボタンでUDP送信、パラメータの書き換えはソースコードで行いますがコンパイル時間は見えないので、実質瞬時に対応できます。

<M5StackのUDP受信処理抜き出し>

M5StackのRegExp処理はいまいちよく分からなかったので、受信テキストは文字列分割で対応。カンマ区切りの３個のパラメータだけなので文字分割でも対応できますが、その分コードは美しくない。このコードをloop()処理中に記述して、ダイナミックにPIDパラメータ変更を実現します。

UDP通信とパラメータ格納の該当部分だけ抜き出したコードです。


#define N 1024

// PID variables
float P_val = 50;
float I_val = 2;
float D_val = 10;

WiFiUDP udp;

	  char packetBuffer[N];
	  int packetSize = udp.parsePacket();

	  if (packetSize){
		  int len = udp.read(packetBuffer, packetSize);

      String pid_data = String(packetBuffer);
      //Serial.println(pid_data);
      int index = pid_data.indexOf(",");
      P_val = (pid_data.substring(0, index)).toFloat();     // P
      pid_data = pid_data.substring(index + 1, pid_data.length());
      index = pid_data.indexOf(",");
      I_val = (pid_data.substring(0, index)).toFloat();     // I
      pid_data = pid_data.substring(index + 1, pid_data.length());
      D_val = pid_data.toFloat();                           // D

admin