C-StyleでP制御（比例制御）　その２

工作

先日のプログラムを（Wikipedia「PID制御」よりP制御）の式に当てはめると、

  • 操作量 u(t) → 10行目の変数A（モーター値）
  • 出力値 y(t) → 6行目の変数Z（ループ内での今の向き）
  • 目標値 r(t) → 3行目の変数Y（スタート時の向き）
  • 比例ゲイン Kp → 9,10行目の変数Aの係数の合算値（ = -0.5）
  • 偏差 e(t) = y(t) - r(t) → 8行目の「Z-Y」

となる。


出力値の変数Zと目標値の変数Yはともにコンパスセンサの値が入るので、どちらも0〜359度までの値となるが、359度と0度をまたいで向きが変わった場合に、偏差「Z-Y」が期待する値にならない。

だだし、一周回って向きそのものは正しい（-340度+360度＝20度、340度-360度=-20度）。

また、単純な引き算だけでは偏差「Z-Y」が180度を超える場合もある。

ここまでの規則性から偏差「Z-Y」が

  • 180度を超えたら、360を引く
  • -179度未満なら、360を足す

ことで、偏差「Z-Y」が-179〜180の値になるよう調整したプログラムがこちら。


実際の動きはこちら。

スタート時の向きから離れているほど素早く動いているが、もっと早く動いてほしいところ。


比例制御は比例ゲイン(Kp)を変えることで操作量を変えることができる。
2倍の速度で動かすなら、比例ゲインが2倍(-0.5 → -1.0)になるようプログラムを直せばよい。

しかし、偏差「Z-Y」は-179〜180の値を取るので、そのまま操作量として使うと、-100〜100で指定するモーター値の下限・上限を超えてしまうので、超えた場合はそれぞれ下限値・上限値を操作量とする門番を入れ、誤動作を防ぐ。


もっと早く動いてほしいかな。


比例ゲインを更に2倍(-2.0)


比例ゲインを更に2倍(-4.0)

（続く）