概要
こちらのamazonで売っているものを自作してみたいと思います．
完成したものyoutube
解決すべき問題
- ホールセンサでどのように，浮遊体の姿勢を計測するか．姿勢角とホールセンサ出力の関係
- 電磁石OFF時にホールセンサ計測を行おうと考えているが，OFF時に鉄心が磁化されている影響がどの程度発生するか．回路的に完全OFFがすぐにできているか
- 電磁石のパワーは足りているのか
- ホールセンサ計測のため，電磁石OFFを一定間隔でしても問題なく制御可能かどうか
基礎実験
磁石長
下の磁石が長いほうが，姿勢を崩しにくい．
原因
たくさん積んだ方が，磁力が強まり（または，長いほど距離に対して磁界が弱まりにくくなる），高い位置で自重と釣り合うため，不安定姿勢に入りにくくなると考えられる．
磁石間隔
浮遊体直径より大きいほど，不安定状態に入りやすくなる．
浮遊体直径より必ず小さく磁石を配置する必要があるだろう．
浮遊体外径59.5mmに対し，
磁石中心間隔35mm（磁石径19mm） ー＞水平方向のスライドメイン
磁石中心間隔45mm（磁石径19mm） ー＞水平方向のスライド・本体の回転 中間ぐらい両方でる
磁石中心間隔55mm（磁石径19mm） ー＞本体の回転メイン
ここでいう，本体回転メインとは
こんな感じで，本体が回転して安定状態になろうとする状態です．
磁石中心間隔35mm（磁石径19mm）以下になると
もはや1本の磁石の場合と同じになると考えられます．
磁石中心間隔55mm（磁石径19mm） ー＞本体の回転メイン以上では
こんな感じで，中心にリングの片側が下がる形になりがちです．
制御するうえでは，おそらく，水平方向のシフトのほうが対処しやすそうなので，こちらメインになるよう，35mm間隔で配置されるよう並べます．
装置の外観
コイルを右上・右下．左上・左下に4つ配置し，ホールセンサも４つ上・下・左・右につけています（黄色い四角形のやつ）．売っているものは，ホールセンサを中心にとりつけていますが，位置計測の方法がわからなかったため，この取付位置になってます．
下には，重力を打ち消すための永久磁石（百均20個入りのもの）を設置しています．163gでした．
コイルの線の太さはエナメル線0.55mm-8mのものと0.4mm-10mのものを使っています（ホーマックで買いました，売ってないホーマックもあります）．
ホールセンサの応答
オペアンプで増幅回路（10倍）を組んだため，かなり変化の大きいデータを取得できています（arduinoのanalogReadのため0~1023で変化）．増幅回路を使わないと30程度しか動かないので，確実に増幅回路は必要です．
電源ラインは，7805にヒートシンクをつけていますが，どの程度安定化しているかは微妙です．増幅回路を外すと4.67->4.81Vに増加したので，電圧降下は確実にあります．
オペアンプの飽和の関係で180~720までしかデータは取れないようです．
コイルON，OFFに対する応答
コイルに2A程度流すと微妙に変化がでています．
黄色：PWM指令値+500
青，赤，緑，黄色：ホールセンサ出力
＋側とー側で均等に出てくるわけではないようです．最大で50程度の誤差になっています．
とりあえず，誤差ぐらいの認識とします．
地磁気の影響
赤線は飽和していますが，装置本体を動かすと，地磁気の影響で微妙に動きます．
装置は固定する必要があります．
追記：まったく影響なし，前回のは電源ライン系によると予想
ホールセンサより，浮遊磁石位置計測方法
浮遊磁石を上下に動かすと次のような反応を示します．
左右に動かすと
青線は1の場所のホールセンサ,赤線は3の場所のホールセンサ，緑線は4の場所のホールセンサ，黄色は2の場所のホールセンサ
の反応を示しています．
上下は1-3,左右は2-4のみが反応しているため，ホールセンサの値より位置が計測できると考えられます．
そこで，横方向の変位（x），縦方向の変位(y)を次の式で計算します．ただし，変位と磁気変化は非線形と考えられるため，そのまま変位に変換できるわけではないですが，今回は変位と読み取ることとします．
x = hole4 - hole2\\
y = hole1 - hole3\\
変動の中心
また，上の式でもう一つ考えることがあります．
変動の中心を事前に与える必要があるということです．
緑線のように，変動の中心はセンサーによって異なるため事前に与える必要があります．
制御方法
コイルと，ホールセンサの取り付け位置には45度ずれがあるため，x-yの数値を45度回転しx'-y'として
x'=(x+y)/ \sqrt{2} \\
y'=(y-x)/ \sqrt{2}
とし，コイルの電圧をPWMで出力を調整します．PWMの数値はコイルの番号を次のようにつけるとすると，
\begin{eqnarray}
coil4_{power} = k*x'+p\frac{dx'}{dt}\\
coil2_{power} = -1*(k*x'+p\frac{dx'}{dt})\\
coil1_{power} = k*y'+p\frac{dy'}{dt}\\
coil3_{power} = -1*(k*y'+p\frac{dy'}{dt})\\
\end{eqnarray}
のように，PD制御でとりあえず，1秒ほどの浮遊に成功しました！
youtube
Arduinoのプログラム
#include <TimerOne.h>
//Timer3 Timer4 Timer5 で pwm を生成する．
//3:2,3,5 4:6,7,8 5:44,45 ピンで行う
#define LU1 2
#define LU2 3
#define LD1 5
#define LD2 6
#define RU1 7
#define RU2 8
#define RD1 44
#define RD2 45
volatile int phase = 0;
volatile boolean flag = 0;
volatile int count = 0;
volatile int hole1 = 0;
volatile int hole2 = 0;
volatile int hole3 = 0;
volatile int hole4 = 0;
volatile float mag_x[4] = {0, 0, 0, 0};
volatile float mag_y[4] = {0, 0, 0, 0};
volatile float dif_mag_x[4] = {0, 0, 0, 0};
volatile float dif_mag_y[4] = {0, 0, 0, 0};
void printer() {
flag = true;
count++;
if (count >= 5) {
count = 0;
}
}
void coil_LU(int power) { //Nを正とする
//digitalWrite(LU1, LOW); //N:内側
//digitalWrite(LU2, HIGH);
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(LU1, 0);
analogWrite(LU2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(LU1, power * -1);
analogWrite(LU2, 0);
}
}
void coil_LD(int power) { //Nを正とする
//digitalWrite(LD1, LOW); //N：内側
//digitalWrite(LD2, HIGH);
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(LD1, 0);
analogWrite(LD2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(LD1, power * -1);
analogWrite(LD2, 0);
}
}
void coil_RU(int power) { //Nを正とする
//digitalWrite(RU1, HIGH); //N
//digitalWrite(RU2, LOW);
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(RU1, power);
analogWrite(RU2, 0);
} else {
if (power < -255) {
power = -255;
}
analogWrite(RU1, 0);
analogWrite(RU2, power * -1);
}
}
void coil_RD(int power) { //Nを正とする
//digitalWrite(RD1, LOW); //N
//digitalWrite(RD2, HIGH);
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(RD1, 0);
analogWrite(RD2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(RD1, power * -1);
analogWrite(RD2, 0);
}
}
void setup() {
Serial.begin(250000);
pinMode(LU1, OUTPUT);
pinMode(LU2, OUTPUT);
pinMode(LD1, OUTPUT);
pinMode(LD2, OUTPUT);
pinMode(RU1, OUTPUT);
pinMode(RU2, OUTPUT);
pinMode(RD1, OUTPUT);
pinMode(RD2, OUTPUT);
analogWrite(2, 100);
analogWrite(3, 50);
analogWrite(5, 20);
analogWrite(6, 10);
analogWrite(7, 5);
analogWrite(8, 2);
analogWrite(44, 1);
analogWrite(45, 0);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
Timer1.initialize(2000); //10ms
Timer1.attachInterrupt(printer);
coil_LU(0);
coil_LD(0);
coil_RU(0);
coil_RD(0);
}
void loop() {
if (flag == true) {
flag = false;
if (count <= 3) {
hole1 += analogRead(A0);
hole2 += analogRead(A1);
hole3 += analogRead(A2);
hole4 += analogRead(A3);
} else {
float hole_up = hole1 / 4 - 329;
float hole_down = hole2 / 4 - 529;
float hole_right = hole3 / 4 - 239;
float hole_left = hole4 / 4 - 259;
/*
Serial.print(hole_up );
Serial.print(" ");
Serial.print(hole_down );
Serial.print(" ");
Serial.print(hole_right);
Serial.print(" ");
Serial.println(hole_left );
*/
hole1 = 0;
hole2 = 0;
hole3 = 0;
hole4 = 0;
if (hole_up < 200) {//制御開始
float hole_y = hole_down - hole_up;
float hole_x = hole_left - hole_right;
//45度回転ccw 1/sqrt(2) は無視
mag_x[0] = (hole_x + hole_y) ;
mag_y[0] = (hole_y - hole_x) ;
//微分器の導入 PD制御する．
//Cat of Frequency 9.689219 Hz, 0.608792 rad , 60.879159 rad/s 20
//dif_mag_x[0] = 12.792877 * mag_x[0] + 0.000000 * mag_x[1] + -12.792877 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (-1.168261 * dif_mag_x[1] + 0.424118 * dif_mag_x[2]);
//Cat of Frequency 31.954646 Hz, 2.007770 rad , 200.776964 rad/s 100
//dif_mag_x[0] = 86.745276 * mag_x[0] + 0.000000 * mag_x[1] + -86.745276 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (0.515887 * dif_mag_x[1] + 0.219018 * dif_mag_x[2]);
//Cat of Frequency 21.192237 Hz, 1.331548 rad , 133.154750 rad/s 50
//dif_mag_x[0] = 45.230740 * mag_x[0] + 0.000000 * mag_x[1] + -45.230740 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (-0.280946 * dif_mag_x[1] + 0.185561 * dif_mag_x[2]);
//Cat of Frequency 26.508241 Hz, 1.665562 rad , 166.556193 rad/s 70
//dif_mag_x[0] = 64.240973 * mag_x[0] + 0.000000 * mag_x[1] + -64.240973 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (0.111065 * dif_mag_x[1] + 0.173754 * dif_mag_x[2]);
//Cat of Frequency 17.856615 Hz, 1.121964 rad , 112.196423 rad/s 40
dif_mag_x[0] = 34.579220 * mag_x[0] + 0.000000 * mag_x[1] + -34.579220 * mag_x[2] ;
dif_mag_x[0] = dif_mag_x[0] - (-0.530110 * dif_mag_x[1] + 0.221694 * dif_mag_x[2]);
for (int i = 2; i >= 1; i--) {
dif_mag_x[i] = dif_mag_x[i - 1];
mag_x[i] = mag_x[i - 1];
}
//Cat of Frequency 9.689219 Hz, 0.608792 rad , 60.879159 rad/s
//dif_mag_y[0] = 12.792877 * mag_y[0] + 0.000000 * mag_y[1] + -12.792877 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (-1.168261 * dif_mag_y[1] + 0.424118 * dif_mag_y[2]);
//Cat of Frequency 31.954646 Hz, 2.007770 rad , 200.776964 rad/s
//dif_mag_y[0] = 86.745276 * mag_y[0] + 0.000000 * mag_y[1] + -86.745276 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (0.515887 * dif_mag_y[1] + 0.219018 * dif_mag_y[2]);
//Cat of Frequency 21.192237 Hz, 1.331548 rad , 133.154750 rad/s
//dif_mag_y[0] = 45.230740 * mag_y[0] + 0.000000 * mag_y[1] + -45.230740 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (-0.280946 * dif_mag_y[1] + 0.185561 * dif_mag_y[2]);
//Cat of Frequency 26.508241 Hz, 1.665562 rad , 166.556193 rad/s
//dif_mag_y[0] = 64.240973 * mag_y[0] + 0.000000 * mag_y[1] + -64.240973 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (0.111065 * dif_mag_y[1] + 0.173754 * dif_mag_y[2]);
//Cat of Frequency 17.856615 Hz, 1.121964 rad , 112.196423 rad/s
dif_mag_y[0] = 34.579220 * mag_y[0] + 0.000000 * mag_y[1] + -34.579220 * mag_y[2] ;
dif_mag_y[0] = dif_mag_y[0] - (-0.530110 * dif_mag_y[1] + 0.221694 * dif_mag_y[2]);
for (int i = 3; i >= 1; i--) {
dif_mag_y[i] = dif_mag_y[i - 1];
mag_y[i] = mag_y[i - 1];
}
float k = 0.7; //比例ゲイン
float p = 0.015;
float power_x = mag_x[0] * k + dif_mag_x[0] * p;
float power_y = mag_y[0] * k + dif_mag_y[0] * p;
coil_LU(power_y);
coil_RD(-1 * power_y);
coil_RU(power_x);
coil_LD(-1 * power_x);
Serial.print(mag_x[0]*k);
Serial.print(" ");
Serial.println(dif_mag_x[0]*p );
/*
Serial.print(dif_mag_x[0]);
Serial.print(" ");
Serial.println(dif_mag_y[0] );
*/
/*
coil_LU(10);
coil_RD(1);
coil_RU(4);
coil_LD(13);
*/
/*
Serial.print(mag_x );
Serial.print(" ");
Serial.println(mag_y);
*/
} else {
coil_LU(0);
coil_RD(0);
coil_RU(0);
coil_LD(0);
}
}
}
}
さらなる安定性の向上に向けて：入力積分方式
比例ゲイン・微分ゲインを調整しても落としきれない「ゆらゆらー＞落下」という現象があります．
青線が比例入力成分 mag_x[0]×k ，赤線が微分入力成分dif_mag_x[0]×pです．
横軸はサンプリング回数（時刻）で，時刻200ぐらいに浮遊体を乗せ，時刻750ぐらいから安定が崩れ，落下しています．わかることは
- 比例入力成分に一定値が入っている
- 入力は255で飽和のため，片側は飽和ギリギリになってしまっている．
- 最終的にギリギリ側(負側)に発散して，落下している
ということが読み取れます．
仮に，浮遊体を中心に持ってこれた場合，制御入力は必要なく浮遊します（実際は外乱等で中心から少しぶれるため，入力がいる）．
今の状態は
このように，中心から少しずれた場所に浮遊しようとしているため，常時一定の入力が必要となっているのです．
なぜ，中心から少しずれた場所に浮遊しようとしているかというと，センサーで検出されるx-yは中心を(0,0)としているわけではなく，事前になんとなくここかなという場所を(0,0)と指定しているため，ずれが発生します．このずれにより，「中心から少しずれた場所に浮遊しようとしている」状態になります．
シミュレーションします．次のシステム，状態量$x$,センサー出力$y$
\begin{eqnarray}
\ddot{x}=-kx-c\dot{x}+u\\
y=x+a\\
\end{eqnarray}
センサー出力には，状態量に$a$という偏差が与えられています．ここで，
u=k1*y+k2*\dot{y}
というフィードバック制御をすると
このグラフ（状態量）の青線のように0には収束せず，別の一定値に収束します．
このグラフ（制御入力）のように，制御入力も一定値偏差が残って安定化します．
この問題への対応は，制御入力の積分を行うのが効果的です．
u=k1*(y-k_{int}int_u)+k2*\dot{y}
このように，入力uの積分$int_u$が正に大きいほど，偏差aは正に大きいということなので，センサー出力から$int_u$を引いてやればよいと考えられます．あまり，この成分が支配的になると全然安定しないので，ゲイン$k_{int}$で効果を調整します．
↑入力積分なし，↓入力積分あり
入力積分項を制御入力の計算に導入することで0に収束できます．
詳しくはこちらに書いています．
実際に制御に取り入れると
青線が比例入力成分 mag_x[0]×k ，赤線が微分入力成分dif_mag_x[0]×pです．
先ほどと違い，比例成分の中心がほぼ0に移動しました．
しかし，振動が収まりませんでした．
可能性を挙げていく
- 電流による磁界依存のホールセンサ出力の変動
- 浮遊体の姿勢角依存の振動
今後，解決次第更新します．
現状のセンサー配置では，姿勢角を計測することができないため，センサーを追加する必要がある．
制御入力にノッチフィルタを追加する
制御系が振動してしまっているので，制御帯域入力をカットします．磁石自体が回転する固有周波数ですが，これは磁石自体の物性値で決まる（$ \sqrt{k/m}$)ではないと考えるので（ここでいうばね定数が制御系の入力），制御入力をカットするのは有効でしょう．自励振動のデータを取ります．2.3Hz付近で発振していることがわかりました．
2.3Hzのノッチフィルタを生成します．
https://qiita.com/yknk0104/items/9519fa02dfd37ea4b228
いろいろ試しましたが，だめっぽいです．
センサーを追加する
中心に3方向のセンサーを追加します．
計測データ
制御中に計測，青：左右方向，赤：上下方向，緑；奥・手前方向
手で，左右，奥手前に少し動かすと，変動は小さい
以上より，本体の傾きが計測できているのではないか，青：左右，緑：奥手前
この2つの信号より，フィードバックする．
成功！
youtube
#include <TimerOne.h>
//Timer3 Timer4 Timer5 で pwm を生成する．
//3:2,3,5 4:6,7,8 5:44,45 ピンで行う
#define LU1 2
#define LU2 3
#define LD1 5
#define LD2 6
#define RU1 7
#define RU2 8
#define RD1 44
#define RD2 45
#define coil_k_LU 1.3
#define coil_k_LD 1.3
#define coil_k_RU 1.0
#define coil_k_RD 1.0
volatile int phase = 0;
volatile boolean flag = 0;
volatile int count = 0;
volatile int hole1 = 0;
volatile int hole2 = 0;
volatile int hole3 = 0;
volatile int hole4 = 0;
volatile int hole5 = 0;
volatile int hole6 = 0;
volatile int hole7 = 0;
volatile float mag_x[4] = {0, 0, 0, 0};
volatile float mag_y[4] = {0, 0, 0, 0};
volatile float dif_mag_x[4] = {0, 0, 0, 0};
volatile float dif_mag_y[4] = {0, 0, 0, 0};
volatile float int_u_x = 0;
volatile float int_u_y = 0;
volatile float inc_x[4] = {0, 0, 0, 0};
volatile float inc_y[4] = {0, 0, 0, 0};
volatile float inc_dx[4] = {0, 0, 0, 0};
volatile float inc_dy[4] = {0, 0, 0, 0};
void printer() {
flag = true;
count++;
if (count >= 5) {
count = 0;
}
}
void coil_LU(int power) { //Nを正とする
//digitalWrite(LU1, LOW); //N:内側
//digitalWrite(LU2, HIGH);
power = coil_k_LU * power;
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(LU1, 0);
analogWrite(LU2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(LU1, power * -1);
analogWrite(LU2, 0);
}
}
void coil_LD(int power) { //Nを正とする
//digitalWrite(LD1, LOW); //N：内側
//digitalWrite(LD2, HIGH);
power = coil_k_LD * power;
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(LD1, 0);
analogWrite(LD2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(LD1, power * -1);
analogWrite(LD2, 0);
}
}
void coil_RU(int power) { //Nを正とする
//digitalWrite(RU1, HIGH); //N
//digitalWrite(RU2, LOW);
power = coil_k_RU * power;
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(RU1, power);
analogWrite(RU2, 0);
} else {
if (power < -255) {
power = -255;
}
analogWrite(RU1, 0);
analogWrite(RU2, power * -1);
}
}
void coil_RD(int power) { //Nを正とする
//digitalWrite(RD1, LOW); //N
//digitalWrite(RD2, HIGH);
power = coil_k_RD * power;
if (power >= 0) {
if (power > 255) {
power = 255;
}
analogWrite(RD1, 0);
analogWrite(RD2, power);
} else {
if (power < -255) {
power = -255;
}
analogWrite(RD1, power * -1);
analogWrite(RD2, 0);
}
}
void setup() {
Serial.begin(250000);
pinMode(LU1, OUTPUT);
pinMode(LU2, OUTPUT);
pinMode(LD1, OUTPUT);
pinMode(LD2, OUTPUT);
pinMode(RU1, OUTPUT);
pinMode(RU2, OUTPUT);
pinMode(RD1, OUTPUT);
pinMode(RD2, OUTPUT);
analogWrite(2, 100);
analogWrite(3, 50);
analogWrite(5, 20);
analogWrite(6, 10);
analogWrite(7, 5);
analogWrite(8, 2);
analogWrite(44, 1);
analogWrite(45, 0);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
Timer1.initialize(2000); //10ms
Timer1.attachInterrupt(printer);
coil_LU(0);
coil_LD(0);
coil_RU(0);
coil_RD(0);
}
void loop() {
if (flag == true) {
flag = false;
if (count <= 3) {
hole1 += analogRead(A0);
hole2 += analogRead(A1);
hole3 += analogRead(A2);
hole4 += analogRead(A3);
hole5 += analogRead(A5);
hole6 += analogRead(A6);
hole7 += analogRead(A7);
} else {
float hole_up = hole1 / 4 - 329;
float hole_down = hole2 / 4 - 529;
float hole_right = hole3 / 4 - 239;
float hole_left = hole4 / 4 - 259;
float hole_inc_right = hole5 / 4 - 320 - 50;
float hole_inc_up = hole7 / 4 - 541 - 50;
/*
Serial.print(hole_up );
Serial.print(" ");
Serial.print(hole_down );
Serial.print(" ");
Serial.print(hole_right);
Serial.print(" ");
Serial.println(hole_left );
*/
hole1 = 0;
hole2 = 0;
hole3 = 0;
hole4 = 0;
hole5 = 0;
hole6 = 0;
hole7 = 0;
if (hole_up < 200) {//制御開始
float hole_y = hole_down - hole_up;
float hole_x = hole_left - hole_right;
//45度回転ccw 1/sqrt(2) は無視
mag_x[0] = (hole_x + hole_y) ;
mag_y[0] = (hole_y - hole_x) ;
//微分器の導入 PD制御する．
//Cat of Frequency 9.689219 Hz, 0.608792 rad , 60.879159 rad/s 20
//dif_mag_x[0] = 12.792877 * mag_x[0] + 0.000000 * mag_x[1] + -12.792877 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (-1.168261 * dif_mag_x[1] + 0.424118 * dif_mag_x[2]);
//Cat of Frequency 31.954646 Hz, 2.007770 rad , 200.776964 rad/s 100
//dif_mag_x[0] = 86.745276 * mag_x[0] + 0.000000 * mag_x[1] + -86.745276 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (0.515887 * dif_mag_x[1] + 0.219018 * dif_mag_x[2]);
//Cat of Frequency 21.192237 Hz, 1.331548 rad , 133.154750 rad/s 50
//dif_mag_x[0] = 45.230740 * mag_x[0] + 0.000000 * mag_x[1] + -45.230740 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (-0.280946 * dif_mag_x[1] + 0.185561 * dif_mag_x[2]);
//Cat of Frequency 26.508241 Hz, 1.665562 rad , 166.556193 rad/s 70
dif_mag_x[0] = 64.240973 * mag_x[0] + 0.000000 * mag_x[1] + -64.240973 * mag_x[2] ;
dif_mag_x[0] = dif_mag_x[0] - (0.111065 * dif_mag_x[1] + 0.173754 * dif_mag_x[2]);
//Cat of Frequency 17.856615 Hz, 1.121964 rad , 112.196423 rad/s 40
//dif_mag_x[0] = 34.579220 * mag_x[0] + 0.000000 * mag_x[1] + -34.579220 * mag_x[2] ;
//dif_mag_x[0] = dif_mag_x[0] - (-0.530110 * dif_mag_x[1] + 0.221694 * dif_mag_x[2]);
for (int i = 2; i >= 1; i--) {
dif_mag_x[i] = dif_mag_x[i - 1];
mag_x[i] = mag_x[i - 1];
}
//Cat of Frequency 9.689219 Hz, 0.608792 rad , 60.879159 rad/s
//dif_mag_y[0] = 12.792877 * mag_y[0] + 0.000000 * mag_y[1] + -12.792877 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (-1.168261 * dif_mag_y[1] + 0.424118 * dif_mag_y[2]);
//Cat of Frequency 31.954646 Hz, 2.007770 rad , 200.776964 rad/s
//dif_mag_y[0] = 86.745276 * mag_y[0] + 0.000000 * mag_y[1] + -86.745276 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (0.515887 * dif_mag_y[1] + 0.219018 * dif_mag_y[2]);
//Cat of Frequency 21.192237 Hz, 1.331548 rad , 133.154750 rad/s
//dif_mag_y[0] = 45.230740 * mag_y[0] + 0.000000 * mag_y[1] + -45.230740 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (-0.280946 * dif_mag_y[1] + 0.185561 * dif_mag_y[2]);
//Cat of Frequency 26.508241 Hz, 1.665562 rad , 166.556193 rad/s
dif_mag_y[0] = 64.240973 * mag_y[0] + 0.000000 * mag_y[1] + -64.240973 * mag_y[2] ;
dif_mag_y[0] = dif_mag_y[0] - (0.111065 * dif_mag_y[1] + 0.173754 * dif_mag_y[2]);
//Cat of Frequency 17.856615 Hz, 1.121964 rad , 112.196423 rad/s
//dif_mag_y[0] = 34.579220 * mag_y[0] + 0.000000 * mag_y[1] + -34.579220 * mag_y[2] ;
//dif_mag_y[0] = dif_mag_y[0] - (-0.530110 * dif_mag_y[1] + 0.221694 * dif_mag_y[2]);
for (int i = 3; i >= 1; i--) {
dif_mag_y[i] = dif_mag_y[i - 1];
mag_y[i] = mag_y[i - 1];
}
float k = 0.7; //比例ゲイン
float p = 0.02;
float iu = 0.001;
float power_x = mag_x[0] * k + dif_mag_x[0] * p + int_u_x * iu;
float power_y = mag_y[0] * k + dif_mag_y[0] * p + int_u_y * iu;
if (power_x > 256) {
power_x = 255;
} else if (power_x < -256) {
power_x = -255;
}
if (power_y > 256) {
power_y = 255;
} else if (power_y < -256) {
power_y = -255;
}
//入力積分
int_u_x += power_x;
int_u_y += power_y;
//傾斜に対する入力
float k_inc = 0.1;
inc_x[0] = -1 * (hole_inc_right + hole_inc_up); //電磁石のx-y平面での方向に変換する
inc_y[0] = hole_inc_right - hole_inc_up;
//微分して，d制御した方がよさそう
float d_inc = 0.01;
//Cat of Frequency 4.739314 Hz, 0.297780 rad , 29.777990 rad/s 30
inc_dx[0] = 3.644811 * inc_x[0] + -0.000000 * inc_x[1] + -3.644811 * inc_x[2] ;
inc_dx[0] = inc_dx[0] - (-1.583468 * inc_dx[1] + 0.656364 * inc_dx[2]);
//Cat of Frequency 4.739314 Hz, 0.297780 rad , 29.777990 rad/s
inc_dy[0] = 3.644811 * inc_y[0] + -0.000000 * inc_y[1] + -3.644811 * inc_y[2] ;
inc_dy[0] = inc_dy[0] - (-1.583468 * inc_dy[1] + 0.656364 * inc_dy[2]);
for (int i = 3; i >= 1; i--) {
inc_dx[i] = inc_dx[i - 1];
inc_x[i] = inc_x[i - 1];
inc_dy[i] = inc_dy[i - 1];
inc_y[i] = inc_y[i - 1];
}
/*
power_x += d_inc * inc_dx[0];
power_y += d_inc * inc_dy[0];
*/
power_x += k_inc * inc_x[0]+d_inc * inc_dx[0];
power_y += k_inc * inc_y[0]+d_inc * inc_dy[0];
coil_LU(power_y);
coil_RD(-1 * power_y);
coil_RU(power_x);
coil_LD(-1 * power_x);
/*
Serial.print(mag_y[0]*k);
Serial.print(" ");
Serial.print(dif_mag_y[0]*p );
Serial.print(" ");
Serial.println( int_u_y * iu);
*/
/*
Serial.print(mag_y[0]*k+int_u_y * iu);
Serial.print(" ");
Serial.println(dif_mag_y[0]*p );
*/
/*
Serial.print(dif_mag_x[0]);
Serial.print(" ");
Serial.println(dif_mag_y[0] );
*/
/*
coil_LU(10);
coil_RD(1);
coil_RU(4);
coil_LD(13);
*/
/*
Serial.print(mag_x[0] );
Serial.print(" ");
Serial.println(mag_y[0]);
*/
/*
Serial.print(hole_inc_right );
Serial.print(" ");
Serial.println(hole_inc_up);
*/
/*
Serial.print(inc_x);
Serial.print(" ");
Serial.println(inc_y);
*/
Serial.print(inc_dx[0]);
Serial.print(" ");
Serial.println(inc_dy[0]);
} else {
coil_LU(0);
coil_RD(0);
coil_RU(0);
coil_LD(0);
}
}
}
}
終わりに
この情報だけでは，わからないことが多いと思うので，何か質問があればください．