磁気浮上装置の開発

# 概要
こちらの[amazon](https://www.amazon.co.jp/Benkeg-DIY%E7%A3%81%E6%B0%97%E6%B5%AE%E4%B8%8A%E3%82%AD%E3%83%83%E3%83%88%E3%80%81DIY%E7%A3%81%E6%B0%97%E6%B5%AE%E4%B8%8A%E3%82%AD%E3%83%83%E3%83%88%E7%A3%81%E6%B0%97%E3%82%B5%E3%82%B9%E3%83%9A%E3%83%B3%E3%82%B7%E3%83%A7%E3%83%B3DIY%E3%83%A2%E3%82%B8%E3%83%A5%E3%83%BC%E3%83%AB%E3%82%AD%E3%83%83%E3%83%88%E7%A3%81%E6%B0%97%E3%83%95%E3%83%AD%E3%83%BC%E3%83%86%E3%82%A3%E3%83%B3%E3%82%B0DIY%E3%82%AD%E3%83%83%E3%83%88%E6%B5%AE%E9%81%8A%E3%81%8A%E3%82%82%E3%81%A1%E3%82%83/dp/B08BBYLSP5)で売っているものを自作してみたいと思います．

[原理を解説してくださっているサイト](https://ja.blog.shiftall.net/archives/2216/)

完成したもの[youtube](https://www.youtube.com/watch?v=y4biVBVRUHo)
![P_20201209_102810.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/66268f08-9806-5e60-fc3c-4b656089a6c0.jpeg)

# 解決すべき問題

* ホールセンサでどのように，浮遊体の姿勢を計測するか．姿勢角とホールセンサ出力の関係
* 電磁石OFF時にホールセンサ計測を行おうと考えているが，OFF時に鉄心が磁化されている影響がどの程度発生するか．回路的に完全OFFがすぐにできているか
* 電磁石のパワーは足りているのか
* ホールセンサ計測のため，電磁石OFFを一定間隔でしても問題なく制御可能かどうか

# 基礎実験

### 磁石長
下の磁石が長いほうが，姿勢を崩しにくい．

#### 原因
たくさん積んだ方が，磁力が強まり（または，長いほど距離に対して磁界が弱まりにくくなる），高い位置で自重と釣り合うため，不安定姿勢に入りにくくなると考えられる．

### 磁石間隔
浮遊体直径より大きいほど，不安定状態に入りやすくなる．
浮遊体直径より必ず小さく磁石を配置する必要があるだろう．

浮遊体外径59.5mmに対し，
磁石中心間隔35mm（磁石径19mm）　ー＞水平方向のスライドメイン
磁石中心間隔45mm（磁石径19mm）　ー＞水平方向のスライド・本体の回転　中間ぐらい両方でる
磁石中心間隔55mm（磁石径19mm）　ー＞本体の回転メイン

ここでいう，本体回転メインとは
![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/544a7c07-5f08-28cc-5389-60ecf7c6662a.png)
こんな感じで，本体が回転して安定状態になろうとする状態です．

磁石中心間隔35mm（磁石径19mm）以下になると
![3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/80741760-d557-594d-349d-5813c50a9677.png)
もはや1本の磁石の場合と同じになると考えられます．

磁石中心間隔55mm（磁石径19mm）　ー＞本体の回転メイン以上では
![4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/9a1a41ba-6558-edc8-50c7-928bc9175ccb.png)

こんな感じで，中心にリングの片側が下がる形になりがちです．

制御するうえでは，おそらく，水平方向のシフトのほうが対処しやすそうなので，こちらメインになるよう，35mm間隔で配置されるよう並べます．

[リング磁石の磁界シミュレーションをしました](https://qiita.com/hiRina/items/52db489820dc1a4dff6c)


# 装置の外観
![P_20201209_102810.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/66268f08-9806-5e60-fc3c-4b656089a6c0.jpeg)
コイルを右上・右下．左上・左下に4つ配置し，ホールセンサも４つ上・下・左・右につけています（黄色い四角形のやつ）．売っているものは，ホールセンサを中心にとりつけていますが，位置計測の方法がわからなかったため，この取付位置になってます．
![P_20201209_102917.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/2902db70-caab-8573-9989-531aec52299f.jpeg)
下には，重力を打ち消すための永久磁石（百均20個入りのもの）を設置しています．163gでした．
![P_20201209_150413.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/de41bc7c-a32f-143b-f0c6-4f6301da7330.jpeg)

コイルの線の太さはエナメル線0.55mm-8mのものと0.4mm-10mのものを使っています（ホーマックで買いました，売ってないホーマックもあります）．


# ホールセンサの応答
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/3a075e25-6b05-b45c-3575-0aeca34bd3fb.png)
オペアンプで増幅回路（10倍）を組んだため，かなり変化の大きいデータを取得できています（arduinoのanalogReadのため0~1023で変化）．増幅回路を使わないと30程度しか動かないので，確実に増幅回路は必要です．
電源ラインは，7805にヒートシンクをつけていますが，どの程度安定化しているかは微妙です．増幅回路を外すと4.67->4.81Vに増加したので，電圧降下は確実にあります．
オペアンプの飽和の関係で180~720までしかデータは取れないようです．

# コイルON，OFFに対する応答
コイルに2A程度流すと微妙に変化がでています．
![dennryuu.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/6e373b2e-9fa1-3148-ec69-3dce62565edf.png)
黄色：PWM指令値+500
青，赤，緑，黄色：ホールセンサ出力
＋側とー側で均等に出てくるわけではないようです．最大で50程度の誤差になっています．
とりあえず，誤差ぐらいの認識とします．


# 地磁気の影響
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/0529720c-1304-5a61-9ae8-099a186299ff.png)
赤線は飽和していますが，装置本体を動かすと，地磁気の影響で微妙に動きます．
装置は固定する必要があります．
追記：まったく影響なし，前回のは電源ライン系によると予想



# ホールセンサより，浮遊磁石位置計測方法
浮遊磁石を上下に動かすと次のような反応を示します．
![jyouge.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/4e3ef29a-8f9a-bc2a-880f-5bf1451ef0b8.png)

左右に動かすと
![sayuu.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/4b230072-3a77-7179-e021-0d7aabce4310.png)

青線は1の場所のホールセンサ,赤線は3の場所のホールセンサ，緑線は4の場所のホールセンサ，黄色は2の場所のホールセンサ
の反応を示しています．
![hole.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/27b643d0-e65a-04c9-cd12-129a363e52c3.jpeg)
上下は1-3,左右は2-4のみが反応しているため，ホールセンサの値より位置が計測できると考えられます．

そこで，横方向の変位（x），縦方向の変位(y)を次の式で計算します．ただし，変位と磁気変化は非線形と考えられるため，そのまま変位に変換できるわけではないですが，今回は変位と読み取ることとします．

```math
x = hole4 - hole2\\
y = hole1 - hole3\\
```

### 変動の中心
また，上の式でもう一つ考えることがあります．
変動の中心を事前に与える必要があるということです．![jyouge2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/360ec624-21da-7565-d366-4e7ed75cf5f8.png)
緑線のように，変動の中心はセンサーによって異なるため事前に与える必要があります．

# 制御方法
![hole2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/24488e8a-1b75-832f-65c9-74bddf51b49b.jpeg)
コイルと，ホールセンサの取り付け位置には45度ずれがあるため，x-yの数値を45度回転しx'-y'として

```math
x'=(x+y)/ \sqrt{2} \\
y'=(y-x)/ \sqrt{2}
```
とし，コイルの電圧をPWMで出力を調整します．PWMの数値はコイルの番号を次のようにつけるとすると，
![hole_cjpg.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/1fc18940-a8a4-f09e-2240-f97cec15d4b9.jpeg)

```math
\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](https://www.youtube.com/watch?v=fCnBDZ7NIyI&feature=youtu.be)

Arduinoのプログラム

```C
#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);
      }
    }
  }

}
```

# さらなる安定性の向上に向けて：入力積分方式
比例ゲイン・微分ゲインを調整しても落としきれない「ゆらゆらー＞落下」という現象があります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/ccc6f790-3308-8c7f-c859-d3ddacb577c0.png)

青線が比例入力成分 mag_x[0]×k　，赤線が微分入力成分dif_mag_x[0]×pです．

横軸はサンプリング回数（時刻）で，時刻200ぐらいに浮遊体を乗せ，時刻750ぐらいから安定が崩れ，落下しています．わかることは

* 比例入力成分に一定値が入っている
* 入力は255で飽和のため，片側は飽和ギリギリになってしまっている．
* 最終的にギリギリ側(負側)に発散して，落下している

ということが読み取れます．

仮に，浮遊体を中心に持ってこれた場合，制御入力は必要なく浮遊します（実際は外乱等で中心から少しぶれるため，入力がいる）．
![an2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/9d1325f8-019d-5d8e-ed34-1c70081d2f0d.png)

今の状態は
![an1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/22edbc71-c5e2-6038-b21c-2234d328e2e5.png)
このように，中心から少しずれた場所に浮遊しようとしているため，常時一定の入力が必要となっているのです．
なぜ，中心から少しずれた場所に浮遊しようとしているかというと，センサーで検出されるx-yは中心を(0,0)としているわけではなく，事前になんとなくここかなという場所を(0,0)と指定しているため，ずれが発生します．このずれにより，「中心から少しずれた場所に浮遊しようとしている」状態になります．

シミュレーションします．次のシステム，状態量$x$,センサー出力$y$

```math
\begin{eqnarray}
\ddot{x}=-kx-c\dot{x}+u\\
y=x+a\\
\end{eqnarray}
```
センサー出力には，状態量に$a$という偏差が与えられています．ここで，

```math
u=k1*y+k2*\dot{y}
```
というフィードバック制御をすると

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/a21a1264-c252-3dd7-aed7-06779d621ca7.png)
このグラフ（状態量）の青線のように0には収束せず，別の一定値に収束します．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/212578e1-d949-5837-8e5e-f1a9143dd329.png)
このグラフ（制御入力）のように，制御入力も一定値偏差が残って安定化します．

この問題への対応は，制御入力の積分を行うのが効果的です．

```math
u=k1*(y-k_{int}int_u)+k2*\dot{y}
```
このように，入力uの積分$int_u$が正に大きいほど，偏差aは正に大きいということなので，センサー出力から$int_u$を引いてやればよいと考えられます．あまり，この成分が支配的になると全然安定しないので，ゲイン$k_{int}$で効果を調整します．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/fc73a80e-07a0-a545-e984-4865c8de833c.png)
↑入力積分なし，↓入力積分あり
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/45857297-f6eb-2512-17fd-0faabfd61275.png)

入力積分項を制御入力の計算に導入することで0に収束できます．
詳しくは[こちらに](（https://qiita.com/hiRina/items/307954a512fd38a1e758)書いています．


実際に制御に取り入れると
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/b3e045b5-4f2d-ca8d-3d57-78a31b8ed6dd.png)
青線が比例入力成分 mag_x[0]×k　，赤線が微分入力成分dif_mag_x[0]×pです．
先ほどと違い，比例成分の中心がほぼ0に移動しました．
しかし，振動が収まりませんでした．
可能性を挙げていく

* 電流による磁界依存のホールセンサ出力の変動
* 浮遊体の姿勢角依存の振動

今後，解決次第更新します．
現状のセンサー配置では，姿勢角を計測することができないため，センサーを追加する必要がある．

# 制御入力にノッチフィルタを追加する
制御系が振動してしまっているので，制御帯域入力をカットします．磁石自体が回転する固有周波数ですが，これは磁石自体の物性値で決まる（$ \sqrt{k/m}$)ではないと考えるので（ここでいうばね定数が制御系の入力），制御入力をカットするのは有効でしょう．自励振動のデータを取ります．2.3Hz付近で発振していることがわかりました．
2.3Hzのノッチフィルタを生成します．
https://qiita.com/yknk0104/items/9519fa02dfd37ea4b228
いろいろ試しましたが，だめっぽいです．

# センサーを追加する
中心に3方向のセンサーを追加します．
計測データ
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/311c4a4b-eada-d63d-8b24-26450c736e7f.png)
制御中に計測，青：左右方向，赤：上下方向，緑；奥・手前方向
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/ea430fcb-3284-f5d7-28b5-d453eaabd0e0.png)
手で，左右，奥手前に少し動かすと，変動は小さい

以上より，本体の傾きが計測できているのではないか，青：左右，緑：奥手前
この2つの信号より，フィードバックする．

成功！
[youtube](https://www.youtube.com/watch?v=y4biVBVRUHo)

```C
#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);
      }
    }
  }

}
```

# 終わりに
この情報だけでは，わからないことが多いと思うので，何か質問があればください．