姿勢制御 – Blowing in the wind

2023年1月4日

倒立振子（その後）

モーターの駆動にはNiH単三４本使っていますが、電池電圧が低下するとトルクも変わってくるのでPIDパラメータもその変化に対応して変えないと安定状態を保てなくなるという当然の結果。

現実的な対応としては、

① 電池電圧を安定化する

少なくともDD-CON必要だし、電圧ドロップ考えると電池電圧足りないだろうからハード作り直しレベル

② 電池電圧の変化に応じてモーターへの最低印加電圧を可変する

float MOTOR_POWER_MIN = 100;
float MOTOR_POWER_MAX = 250;

ここでMOTOR_POWER_MINを変えてやる、今はモーターが回り始めるぐらいの電圧になるように調整していますが、当然電圧低下するとこの値を変えてやらないとダメなのは自明。

理屈ではM5StackのADコンバーター使って可変することはできそうだね、やるやらないは別にして。

admin

2022年12月28日2023年1月12日

倒立振子（P制御のみ有効化）

チューニングで問題だったのは、M5Stackの画面にログ用に表示させている数値の表示がやたらに遅いこと。ジャイロ・加速度・pitch/role/yawを画面表示させているとloop()の処理時間がおよそ50msも掛かっていました。これではフィードバックが間に合わないので、ログ表示を削除すると10ms以下に収まってます。

P値も大きすぎるとリンギングは盛大になるので、適当なところで止めてみたのが以下の動画です。まあ、振動はしてるし、外乱にも対応できない状態ですが。

admin

2022年12月26日

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

倒立振子の製作を始めてますが、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