SimpleFOCの速度PIDゲイン調整してみた。

 とりあえずBluetoothラジコンとしてPS3コンで動くようになったのは良いんだけども、モータがカクカクするのでPIDのゲインを調整してみた。負荷がかかるとハンチングしてる感じ。

SimpleFOCのPIDゲイン調整をしたときにAngleでFOCのゲインを調整をしていたのにもかかわらず、そもそものAngleとVelocityのゲインは未調整だった…

  // velocity loop PID
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20.0;
  motor.PID_velocity.D = 0.0;
  motor.PID_velocity.output_ramp = 1000.0;
  motor.PID_velocity.limit = 8.0;
  // Low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;
  // angle loop PID
  motor.P_angle.P = 20.0;
  motor.P_angle.I = 0.0;
  motor.P_angle.D = 0.0;
  motor.P_angle.output_ramp = 0.0;
  motor.P_angle.limit = 40.0;
  // Low pass filtering time constant
  motor.LPF_angle.Tf = 0.0;

この前のスケッチを見るとデフォルト値？でこんな感じになっていた。

コネクタがこの通り抜き差ししにくいので、とりあえずモータを取り外してシリアルでPCに繋いでSimpleFOC Studioで追加で調整してみた。まずは速度ゲインの調整を。Angleはとりあえずは使っていないので。
コネクタの向きはモータドライバステーを改良すれば行けそうな気がするな…

IとDをゼロにしてPを上げて行くとすぐハンチングしてしまう。LPFを0.05にしてみたらPが1.5ぐらいまで行けそうだったのでとりあえずはPを1.5にしてIを上げていってみた。Iを結構上げても大丈夫そうなんだけど負荷のかかり具合によってまたカクカクしそうなのでとりあえず10ぐらいにしておいた。

  // velocity loop PID
  motor.PID_velocity.P = 1.5;
  motor.PID_velocity.I = 10.0;
  motor.PID_velocity.D = 0.0;
  motor.PID_velocity.output_ramp = 1000.0;
  motor.PID_velocity.limit = 8.0;
  // Low pass filtering time constant
  motor.LPF_velocity.Tf = 0.05;

まずはこんな感じにセットしてみた。

左右のモータドライバに書き込んで動かしてみるとカクカクが治っていい感じ。もう少し調整できそうな気はするけども…

ということでSimpleFOC Studioを使わなくてもCANから変更できるようにCANの部分を少し改良。オリジナルのOdriveでもそういうコマンドが合ったのでとりあえずPとIを変更できるようにしてみた。RTRを送ると現在の値が見れる。

        case 0x01B:  //Set Vel Gains
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.PID_velocity.P = input.value[0];
            motor.PID_velocity.I = input.value[1];
          }else{
            FLOAT_BYTE_UNION output;
            output.value[0] = motor.PID_velocity.P;
            output.value[1] = motor.PID_velocity.I;
            memcpy(CAN_TX_msg.data, output.uint, 8);
            CAN_TX_msg.len = 8;
            CANSend(&CAN_TX_msg);
          }
          break;

こんな感じで0x01Bに対応した部分をswitch case分の中に追加しただけ。とりあえず簡単なツールをPythonで作成して走行しながら調整してみよう。ESP32の方にもリアルタイムで調整できるようにWebServerでも追加しようかな。

うまく動くようになったのでスピードも少し上げてみた。もうちょっと早くできそう。
最大スピードもリアルタイムで変更できるようにしたいな。スピード上げると直線走行時にまたハンチングが少し出てきているような気が…
あとキャスターが左右にゆらゆらするので、そのせいで車体もゆらゆらしてしまうなぁ
ボールキャスターも試してみようかな。ちなみにモータが静かすぎてキャスターの回る音のほうが大きいのでキャスターを変えたらもっと静かになりそう。メカナムラジコンのときのギアの音とは大違いだ…