ワイヤード・マイクロドローン製作～小型モーターとArduino Dueを添えて～

#概要
タイトル通りです．
以前，作ってうまくいって，引っ張りだして来たら，うまく動かなくて，せっかくだから記録を残そうという感じです．

動画：https://www.youtube.com/watch?v=LVpQ0gBg6kk
そこまで安定してないですが，これ以上はパラメータ調整だけでなく・制御方法自体の変更が必要そうです．

###制御方式
* PD制御+入力積分制御
* 下の論文の制御方法
* 角度検出方法：Magwickフィルタ

入力積分制御については[こちら](https://qiita.com/hiRina/items/307954a512fd38a1e758)
数式モデルと制御方法についてはこちらを参考にしました．https://www.jstage.jst.go.jp/article/sicejl/56/1/56_3/_pdf/-char/ja


###ハードウェア構成

* Arduino Due
* 小型モーター：amazon
* プロペラ:amazon
* FET 2sk4017:[ali](https://ja.aliexpress.com/item/32559722647.html?spm=a2g0s.9042311.0.0.27424c4djX9fEh)
* MPU9250
* セメント抵抗0.5Ω：電流計測用
* 安定化電源:max3Aは消費してるので，あったほうがいいです
* 可変抵抗器：パラメータを調整するため

###FET回路
![WIN_20210119_09_58_28_Pro.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/90249fbc-e98a-1067-1415-88b2e149135e.jpeg)
ヒートシンクに圧着してます．
![2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/a9e9ea4f-f72d-4f02-09ce-90034f358141.png)

![Arduino-Due-Pin-mapping.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/ce65e6cc-d09c-bfd4-bf0b-ef89eec5943d.png)

#ワイヤーについて
電源，マイコン，FETはワイヤーで接続しているため，普通のドローンとは言えないですが，電源・マイコンだとパワーと軽量化が必要になるので，ワイヤーでつなぐのが楽だと思います．
また，z高さの制御は入れていないです．ワイヤーの重みと釣り合ったところで停止する感じです．
一方で，ワイヤーによって，x-y方向に行き過ぎると回転するトルクが発生するので，ワイヤーはある程度長くとる必要があります．
![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/b415e037-7bf6-88e6-b6c1-60cd1cad2294.png)

#本体
![WIN_20210119_10_02_52_Pro.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/7508e197-daa9-fef4-b18c-b27077f4a2ed.jpeg)

モータと竹串フレームでできてます．3Dプリンタがあれば，非常に簡単，ただし接着剤固定のため，実験中に幾度か外れた．

###久しぶりに起動して気づいた注意点

* 外部電源をUSBかDCアダプターどちらにするかで勢いが違う：確か，DCアダプターもつけていた
* モーターの電圧忘れた：5Vっぽい

###可変抵抗

* Volume1:A7:kdT  :ロールピッチ微分制御定数
* Volume2:A8:kpT　:ロールピッチ比例制御定数　　：ここが一番重要だと考えている．
* Volume3:A9:kp3T　：使っていない，当初は何かに使っていただろうが不明
* Volume4:A10:kd3T
* Volume5:A11:kdzT  :ヨー方向微分制御定数

#プログラム
要望があれば，解説します．

```C
#define Roter1 7
#define Roter2 6
#define Roter3 5
#define Roter4 4
#define TEST 3

#define Current1 A1
#define Current2 A2
#define Current3 A3
#define Current4 A4
#define cs 10
#define Vol5 A11
#define Vol4 A10
#define Vol3 A9
#define Vol2 A8
#define Vol A7
#include <DueTimer.h>
#include <SPI.h>
#include <MadgwickDue.h>

//各種定数
#define jx 0.000018
#define jy 0.000018
#define jz 0.000036
//inv_Mixの定義
#define inv_Mix11 2.0584975602686915
#define inv_Mix12 0
#define inv_Mix13 -97.32848984721946
#define inv_Mix14 0.20584975602686914
#define inv_Mix21 2.819048650013983
#define inv_Mix22 133.28835224652403
#define inv_Mix23 0
#define inv_Mix24 -0.28190486500139833
#define inv_Mix31 2.1272286975063777
#define inv_Mix32 0
#define inv_Mix33 100.57818900739373
#define inv_Mix34 0.21272286975063776
#define inv_Mix41 2.118387347973932
#define inv_Mix42 -100.16015829663982
#define inv_Mix43 0
#define inv_Mix44 -0.21183873479739324



Madgwick MadgwickFilter;
volatile float data;
volatile float k = 2.1;
volatile boolean Timer10msFlag = false;
volatile boolean CurrentMejure_Flag = false;
volatile int n = 0;
volatile int N = 100;

volatile float Amp1 = 0;
volatile float Amp2 = 0;
volatile float Amp3 = 0;
volatile float Amp4 = 0;
volatile int Amp1s = 0;
volatile int Amp2s = 0;
volatile int Amp3s = 0;
volatile int Amp4s = 0;
volatile int Amp1n = 0;

volatile float TargetA1 = 0;
volatile float TargetA2 = 0;
volatile float TargetA3 = 0;
volatile float TargetA4 = 0;

volatile float gx = 0;
volatile float gy = 0;
volatile float gz = 0;

volatile int RotateVal1 = 0;
volatile int RotateVal2 = 0;
volatile int RotateVal3 = 0;
volatile int RotateVal4 = 0;

volatile float Rx = 0;
volatile float Ry = 0;
volatile float Rz[10] = {0, 0, 0, 0, 0, 0, 0, 0};
volatile float HRz[10] = {0, 0, 0, 0, 0, 0, 0, 0};
volatile float Vx = 0;
volatile float Vy = 0;
volatile float Vz = 0;
volatile float aox[4] = {0,  0, 0, 0};
volatile float aoy[4] = { 0, 0, 0, 0};
volatile float aoz[4] = { 0, 0, 0, 0};
volatile float Haox[4] = { 0, 0,  0, 0};
volatile float Haoy[4] = { 0, 0, 0,  0};
volatile float Haoz[4] = { 0, 0, 0, 0, };
volatile float vox2 = 0;
volatile float voy2 = 0;
volatile float voz2 = 0;
volatile float sumAz = 0;
volatile float MemoryAz[1000];
volatile int MemoryNow = 0;
volatile int MemoryN = 1000;

volatile float sumE3 = 0;
volatile float sumE2 = 0;
volatile float aax_g[10] = {0, 0, 0, 0};
volatile float aay_g[10] = {0, 0, 0, 0};
volatile float vax_g[10] = {0, 0, 0, 0};
volatile float vay_g[10] = {0, 0, 0, 0};
volatile float rax_g[10] = {0, 0, 0, 0};
volatile float ray_g[10] = {0, 0, 0, 0};
volatile float divTheta3 = 0;
volatile float divTheta2 = 0;

volatile float sumtauTil2 = 0;
volatile float sumtauTil3 = 0;
volatile float gx2[10] = { 0, 0, 0, 0, 0, 0, 0};
volatile float gy2[10] = { 0, 0, 0, 0, 0, 0, 0};
volatile float gz2[10] = { 0, 0, 0, 0, 0, 0, 0};
volatile float gx_L[10] = { 0, 0, 0, 0, 0, 0, 0};
volatile float gy_L[10] = { 0, 0, 0, 0, 0, 0, 0};
volatile float gz_L[10] = { 0, 0, 0, 0, 0, 0, 0};

void Current1_mejure() {
  CurrentMejure_Flag = true;
}
void Current_mejuring() {
  Amp1s += analogRead(Current1);
  Amp2s += analogRead(Current2);
  Amp3s += analogRead(Current3);
  Amp4s += analogRead(Current4);
  Amp1n++;
  if (Amp1n >= 100) {
    Amp1 = Amp1s * 0.000016210345 - 0.01902446;
    Amp2 = Amp2s * 0.000016210345 - 0.01902446;
    Amp3 = Amp3s * 0.000016210345 - 0.01902446;
    Amp4 = Amp4s * 0.000016210345 - 0.01902446;
    Amp1s = 0;
    Amp2s = 0;
    Amp3s = 0;
    Amp4s = 0;
    Amp1n = 0;
  }
}
void Timer10ms() {
  Timer10msFlag = true;
  n++;
  if (n >= N) {
    n = N;
  }
}
void setup() {
  for (int i = 0; i < MemoryN; i++) {
    MemoryAz[i] = 0;
  }
  pinMode(Roter1, OUTPUT);
  pinMode(Roter2, OUTPUT);
  pinMode(Roter3, OUTPUT);
  pinMode(Roter4, OUTPUT);
  pinMode(TEST, OUTPUT);
  pinMode(Current1, INPUT);
  pinMode(Current2, INPUT);
  pinMode(Current3, INPUT);
  pinMode(Current4, INPUT);
  pinMode(Vol, INPUT);
  pinMode(Vol2, INPUT);
  pinMode(Vol3, INPUT);
  pinMode(Vol4, INPUT);
  pinMode(Vol5, INPUT);
  Serial.begin(250000);
  analogWrite(Roter1, 0);
  analogWrite(Roter2, 0);
  analogWrite(Roter3, 0);
  analogWrite(Roter4, 0);

  Timer1.attachInterrupt(Current1_mejure);
  Timer1.start(90);                         // 10msec (10,000use
  Timer2.attachInterrupt(Timer10ms);
  Timer2.start(10000);                       // 10msec (10,000usec)
  analogReadResolution(12);

  pinMode(cs, OUTPUT);
  SPI.setBitOrder(MSBFIRST);  //最上位ビット(MSB)から送信
  SPI.setClockDivider(SPI_CLOCK_DIV32);  //通信速度をデフォルト
  SPI.setDataMode(SPI_MODE3);   //アイドル5Vで0V→5Vの変化で送信する
  SPI.begin();  //開始
  //Write(0x1B,0x07);   //500dps
  Write(0x1B, 0x10);  //1000dps
  //Write(0x1B,0x11);   //2000dps
  //Write(0x1A, 0x06);  //gyro LPF5Hz
  Write(0x1A, 0x03);  //gyro LPF41Hz
  Write(0x1D, 0x03);  //acce LPF45Hz

  MadgwickFilter.begin(100); //100Hz
}

void loop() {
  if (CurrentMejure_Flag == true) {
    CurrentMejure_Flag = false;
    Current_mejuring();
  }
  if (Timer10msFlag == true) {
    digitalWrite(TEST, HIGH);
    Timer10msFlag = false;
    //---------------------------------------角速度・加速度計測-----------
    //Serial.print( n / (float)N * 255 , 8);
    //Serial.print(" ");
    int Volume = analogRead(Vol);
    float VolumeValue = Volume / 4095.0 ;
    Volume = analogRead(Vol2);
    float VolumeValue2 = Volume / 4095.0 ;
    Volume = analogRead(Vol3);
    float VolumeValue3 = Volume / 4095.0 ;
    Volume = analogRead(Vol4);
    float VolumeValue4 = Volume / 4095.0 ;
    Volume = analogRead(Vol5);
    float VolumeValue5 = Volume / 4095.0 ;

    byte dataH = Read(0x3B);
    byte dataL = Read(0x3C);
    int AX = dataH << 8 | dataL;
    dataH = Read(0x3D);
    dataL = Read(0x3E);
    int AY = dataH << 8 | dataL;
    dataH = Read(0x3F);
    dataL = Read(0x40);
    int AZ = dataH << 8 | dataL;
    dataH = Read(0x43);
    dataL = Read(0x44);
    int GX = dataH << 8 | dataL;
    dataH = Read(0x45);
    dataL = Read(0x46);
    int GY = dataH << 8 | dataL;
    dataH = Read(0x47);
    dataL = Read(0x48);
    int GZ = dataH << 8 | dataL;
    if (AX > 32768) {
      AX = AX - 65535;
    }
    if (AY > 32768) {
      AY = AY - 65535;
    }
    if (AZ > 32768) {
      AZ = AZ - 65535;
    }
    if (GX > 32768) {
      GX = GX - 65535;
    }
    if (GY > 32768) {
      GY = GY - 65535;
    }
    if (GZ > 32768) {
      GZ = GZ - 65535;
    }
    GX = GX + 64;      //センサーオフセット補正：事前計測済みデータ使用
    GY = GY + 86;
    GZ = GZ + 40;
    AX = AX - 1345;
    AY = AY - 4005;
    AZ = AZ + 2771;
    float aax = AX * 0.59875488;  //[mm/s^2]
    float aay = AY * 0.59875488;
    float aaz = AZ * 0.59875488;
    gx = GX * 0.00053263222;
    gy = GY * 0.00053263222;
    gz = GZ * 0.00053263222;

    //MadgwickFilterにはLPFを掛けたデータを渡す。なんか大きくずれていってしまうため
    //Cat of Frequency 12.111894 Hz, 0.761013 rad , 76.101275 rad/s
    gx2[0] = gx;
    gy2[0] = gy;
    gz2[0] = gz;
    gx_L[0] = 1.000000 * gx2[0]  + 2.000000 * gx2[1]  + 1.000000 * gx2[2] ;
    gx_L[0] = (gx_L[0] - (-10.500000 * gx_L[1] + 3.714466 * gx_L[2])) / 10.785534;
    gy_L[0] = 1.000000 * gy2[0]  + 2.000000 * gy2[1]  + 1.000000 * gy2[2] ;
    gy_L[0] = (gy_L[0] - (-10.500000 * gy_L[1] + 3.714466 * gy_L[2])) / 10.785534;
    gz_L[0] = 1.000000 * gz2[0]  + 2.000000 * gz2[1]  + 1.000000 * gz2[2] ;
    gz_L[0] = (gz_L[0] - (-10.500000 * gz_L[1] + 3.714466 * gz_L[2])) / 10.785534;

    for (int i = 8; i >= 1; i--) {
      gx_L[i] = gx_L[i - 1];
      gx2[i] = gx2[i - 1];
      gy_L[i] = gy_L[i - 1];
      gy2[i] = gy2[i - 1];
      gz_L[i] = gz_L[i - 1];
      gz2[i] = gz2[i - 1];
    }

    Current_mejuring();
    MadgwickFilter.updateIMU(gx_L[0], gy_L[0], gz_L[0], AX, AY, AZ);
    Current_mejuring();

    float RollD = MadgwickFilter.getRoll();
    float YawD = MadgwickFilter.getYaw();
    float PitchD = MadgwickFilter.getPitch();
    Current_mejuring();
    /*
      Serial.print(RollD , 2);
      Serial.print(" ");
      Serial.print(YawD , 2);
      Serial.print(" ");
      Serial.print(PitchD , 2);
      Serial.println(" ");
    */
    //-----------------------------------姿勢制御---------------------------
    float theta3 = RollD * 0.01745329252;           //x  rad
    float theta2 = PitchD * 0.01745329252;          //y
    float theta1 = (YawD - 180) * 0.01745329252;    //z
    float c1 = cos(theta1 );
    float s1 = sin(theta1 );
    float c2 = cos(theta2 );
    float s2 = sin(theta2 );
    float c3 = cos(theta3 );
    float s3 = sin(theta3 );
    float c3c2 = c3 * c2;
    float s3c2 = s3 * c2;
    float s3s2 = s3 * s2;
    float c3s2 = c3 * s2;
    float invc2 = 1.0 / c2;
    Current_mejuring();

    //inv_PuThi*Omoa'
    float dtheta1 = (s3 * gy + c3 * gz) * invc2;
    float dtheta2 = (c3c2 * gy - s3c2 * gz) * invc2;
    float dtheta3 = (c2 * gx + s3s2 * gy + c3s2 * gz) * invc2;
    //-----加速度(フレームA) - Coa*g　より、　速度,変位（フレームA）を計算する
    float g = 9929.15222;
    aax_g[0] = aax + s2 * g;
    aay_g[0] = aay - s3c2 * g;
    if ( n >= N) {
      vax_g[0] = vax_g[0] * 0.99 + (aax_g[0] + aax_g[1]) * 0.005;
      vay_g[0] = vay_g[0] * 0.99 + (aay_g[0] + aay_g[1]) * 0.005;
      rax_g[0] = rax_g[0] * 0.99 + (vax_g[0] + vax_g[1]) * 0.005;
      ray_g[0] = ray_g[0] * 0.99 + (vay_g[0] + vay_g[1]) * 0.005;

    }
    for (int i = 3; i >= 1; i--) {
      aax_g[i] = aax_g[i - 1];
      aay_g[i] = aay_g[i - 1];
      vax_g[i] = vax_g[i - 1];
      vay_g[i] = vay_g[i - 1];
      rax_g[i] = rax_g[i - 1];
      ray_g[i] = ray_g[i - 1];
    }
    divTheta3 = 0.0001 * ray_g[0];
    divTheta2 = 0.0001 * rax_g[0];
    divTheta3 = 0;
    divTheta2 = 0;


    //-----非連成部分　比例・微分制御
    /*  //これくらいがよかった2020/4/17
      float kpT = 0.13431 * -4000; //-300ぐらいがよかった
      float kdT = 0.288798 * -1000; //-100
      float kp3T = 0.22073 * -5000; //-300ぐらいがよかった
      float kd3T = 0.80993 * -800; //-100
    */

    float kpT = VolumeValue2 * -4000; //-300ぐらいがよかった
    float kdT = VolumeValue * -1000; //-100
    float kp3T = VolumeValue3 * -5000; //-300ぐらいがよかった
    float kd3T = VolumeValue4 * -800; //-100

    float kpzT = -200;   //-300ぐらいがよかった
    float kdzT = -1 * VolumeValue5; //-100
    float kiT = 0;
    float error3 = theta3 - divTheta3;
    float error2 = theta2 - divTheta2;
    sumE3 += error3;
    sumE2 += error2;
    float sMax = 30;
    if (sumE3 > sMax) {
      sumE3 = sMax;
    } else if (sumE3 < -1 * sMax) {
      sumE3 = -1 * sMax;
    }
    if (sumE2 > sMax) {
      sumE2 = sMax;
    } else if (sumE2 < -1 * sMax) {
      sumE2 = -1 * sMax;
    }
    float Ukp2  = error2 * kpT ;
    float Dkp2 = dtheta2 * kdT ;
    float Ukp3  = error3 * kpT ;
    float Dkp3 = dtheta3 * kdT ;

    float kitau2 = 0.01 * 0.5;
    float kitau3 = 0.01 * 0.5;

    float tau_til1 = theta1 * kpzT  + dtheta1 * kdzT;
    float tau_til2 = Ukp2  + Dkp2  + kitau2 * sumtauTil2;
    float tau_til3 = Ukp3  + Dkp3  + kitau3 * sumtauTil3;

    //----入力積分　制御手法導入
    sumtauTil2 += Ukp2 * 0.01;
    sumtauTil3 += Ukp3 * 0.01;

    //----非線形成分計算-------------
    //J*PuThi
    float jp11 = -1 * s2 * jx;
    float jp12 = 0;
    float jp13 = jx;
    float jp21 = jy * s3c2;
    float jp22 = jy * c2;
    float jp23 = 0;
    float jp31 = jz * c3c2;
    float jp32 = -1 * jz * s3;
    float jp33 = 0;

    //J*PuThi*Dtheta
    float jpdt1 = jp11 * dtheta1 + jp12 * dtheta2 + jp13 * dtheta3;
    float jpdt2 = jp21 * dtheta1 + jp22 * dtheta2 + jp23 * dtheta3;
    float jpdt3 = jp31 * dtheta1 + jp32 * dtheta2 + jp33 * dtheta3;

    //J*DPuThi
    float jdp11 = -1 * jx * c2 * dtheta2;
    float jdp12 = 0;
    float jdp13 = 0;
    float jdp21 = jy * (c3c2 * dtheta3 - s3s2 * dtheta2);
    float jdp22 = -1 * jy * s3 * dtheta3;
    float jdp23 = 0;
    float jdp31 = -1 * jz * ( s3c2 * dtheta3 + c3s2 * dtheta2);
    float jdp32 = -1 * jz * c3 * dtheta3;
    float jdp33 = 0;
    Current_mejuring();

    //Omega~ * J*PuThi * DTheta
    float omega1 = -1 * gz * jpdt2 + gy * jpdt3;
    float omega2 = gz * jpdt1 - gx * jpdt3;
    float omega3 = -1 * gy * jpdt1 + gx * jpdt2;

    //C*DTheta = (J*DPuThi)*DTheta + Omega~ * J*PuThi * DTheta
    float C1 = jdp11 * dtheta1 + jdp12 * dtheta2 + jdp13 * dtheta3  + omega1;
    float C2 = jdp21 * dtheta1 + jdp22 * dtheta2 + jdp23 * dtheta3  + omega2;
    float C3 = jdp31 * dtheta1 + jdp32 * dtheta2 + jdp33 * dtheta3  + omega3;

    //tau = (J*PuThi)*tau_til + C
    float tau1 = jp11 * tau_til1 + jp12 * tau_til2 + jp13 * tau_til3 + C1;
    float tau2 = jp21 * tau_til1 + jp22 * tau_til2 + jp23 * tau_til3 + C2;
    float tau3 = jp31 * tau_til1 + jp32 * tau_til2 + jp33 * tau_til3 + C3;

    //-------------------------加速度回転と二回積分によって変位を計算する
    aox[0] = c1 * c2 * aax + (c1 * s3s2 - s1 * c3) * aay + (s1 * s3 + c1 * c3s2) * aaz  - 0.012723862;
    aoy[0] = s1 * c2 * aax + (c1 * c3 + s1 * s3s2) * aay + (s1 * c3s2 - s3 * c1) * aaz + 0.539643069;
    aoz[0] = -1 * s2 * aax + s3c2 * aay + c3c2 * aaz   -  9929.15222 - 9.495423753;

    /*
        //wa = 3.141593,Cat of Frequency 0.499959 Hz, 0.031413 rad , 3.141334 rad/s
        Haox[0] = 0.969074 * aox[0]  + -2.907224 * aox[1]  + 2.907226 * aox[2] + -0.969076 * aox[3] ;
        Haox[0] = Haox[0] - (-2.937179 * Haox[1] + 2.876315 * Haox[2] + -0.939106 * Haox[3]);
        Haoy[0] = 0.969074 * aoy[0]  + -2.907224 * aoy[1]  + 2.907226 * aoy[2] + -0.969076 * aoy[3] ;
        Haoy[0] = Haoy[0] - (-2.937179 * Haoy[1] + 2.876315 * Haoy[2] + -0.939106 * Haoy[3]);
        Haoz[0] = 0.969074 * aoz[0]  + -2.907224 * aoz[1]  + 2.907226 * aoz[2] + -0.969076 * aoz[3] ;
        Haoz[0] = Haoz[0] - (-2.937179 * Haoz[1] + 2.876315 * Haoz[2] + -0.939106 * Haoz[3]);
    */
    //wa = 0.314159,Cat of Frequency 0.050000 Hz, 0.003142 rad , 0.314159 rad/s
    Haox[0] = 0.996863 * aox[0]  + -1.996846 * aox[1]  + 0.999988 * aox[2] ;
    Haox[0] = Haox[0] - (-1.996846 * Haox[1] + 0.996851 * Haox[2]);
    Haoy[0] = 0.996863 * aoy[0]  + -1.996846 * aoy[1]  + 0.999988 * aoy[2] ;
    Haoy[0] = Haoy[0] - (-1.996846 * Haoy[1] + 0.996851 * Haoy[2]);
    Haoz[0] = 0.996863 * aoz[0]  + -1.996846 * aoz[1]  + 0.999988 * aoz[2] ;
    Haoz[0] = Haoz[0] - (-1.996846 * Haoz[1] + 0.996851 * Haoz[2]);

    MemoryAz[MemoryNow] = Haoz[0];
    sumAz += Haoz[0];
    if (MemoryNow == 0) {
      sumAz -= MemoryAz[MemoryN - 1];
    } else {
      sumAz -= MemoryAz[MemoryNow - 1];
    }
    MemoryNow ++;
    if (MemoryNow > MemoryN) {
      MemoryNow = 0;
    }

    if ( n >= N) {
      Vx += (Haox[0] + Haox[1]) * 0.005;
      Vy += (Haoy[0] + Haoy[1]) * 0.005;
      Vz += (Haoz[0] + Haoz[1] - sumAz * 2.0 / (float)MemoryN) * 0.005;
      Rx += ( Vx + vox2) * 0.005;
      Ry += ( Vy + voy2) * 0.005;
      Rz[0] += ( Vz + voz2) * 0.005;
      vox2 = Vx;
      voy2 = Vy;
      voz2 = Vz;
    }
    //wa = 0.628319,Cat of Frequency 0.100000 Hz, 0.006283 rad , 0.628316 rad/s
    HRz[0] = 0.993737 * Rz[0]  + -1.993668 * Rz[1]  + 0.999951 * Rz[2] ;
    HRz[0] = HRz[0] - (-1.993668 * HRz[1] + 0.993688 * HRz[2]);


    for (int i = 3; i >= 1; i--) {
      Haox[i] = Haox[i - 1];
      aox[i]  = aox[i - 1];
      Haoy[i] = Haoy[i - 1];
      aoy[i]  = aoy[i - 1];
      Haoz[i] = Haoz[i - 1];
      aoz[i]  = aoz[i - 1];
      Rz[i]  = Rz[i - 1];
      HRz[i]  = HRz[i - 1];
    }


    // 変位の制御はまだしないが、f1～4が負にならないよう,Faを一部与える
    float f = 0.253 / c3c2; //0.21 / c3c2;          //0.202;              //20g = 0.192N

    //----------[f1 f2 f3 f4] = inv_Mix * [f tau1~4] より　各ローターの目標電流fi[A]に変換する
    float f1 = inv_Mix11 * f + inv_Mix12 * tau1 + inv_Mix13 * tau2 + inv_Mix14 * tau3;
    float f2 = inv_Mix21 * f + inv_Mix22 * tau1 + inv_Mix23 * tau2 + inv_Mix24 * tau3;
    float f3 = inv_Mix31 * f + inv_Mix32 * tau1 + inv_Mix33 * tau2 + inv_Mix34 * tau3;
    float f4 = inv_Mix41 * f + inv_Mix42 * tau1 + inv_Mix43 * tau2 + inv_Mix44 * tau3;

    Current_mejuring();
    //-----------------------------------電流制御---------------------------
    float kpI = 200;    //電流比例制御部分
    TargetA1 = f1;
    TargetA2 = f2;
    TargetA3 = f3;
    TargetA4 = f4;
    RotateVal1 = TargetA1 * 220 - kpI * (Amp1 - TargetA1);
    RotateVal2 = TargetA2 * 220 - kpI * (Amp2 - TargetA2);
    RotateVal3 = TargetA3 * 220 - kpI * (Amp3 - TargetA3);
    RotateVal4 = TargetA4 * 220 - kpI * (Amp4 - TargetA4);

    if (RotateVal1 > 255) {
      RotateVal1 = 255;
    } else if (RotateVal1 < 0) {
      RotateVal1 = 0;
    }
    if (RotateVal2 > 255) {
      RotateVal2 = 255;
    } else if (RotateVal2 < 0) {
      RotateVal2 = 0;
    }
    if (RotateVal3 > 255) {
      RotateVal3 = 255;
    } else if (RotateVal3 < 0) {
      RotateVal3 = 0;
    }
    if (RotateVal4 > 255) {
      RotateVal4 = 255;
    } else if (RotateVal4 < 0) {
      RotateVal4 = 0;
    }
    /*
      analogWrite(Roter1, 0);
      analogWrite(Roter2, 0);
      analogWrite(Roter3, 0);
      analogWrite(Roter4, 0);
    */
    analogWrite(Roter1, RotateVal1);
    analogWrite(Roter2, RotateVal2);
    analogWrite(Roter3, RotateVal3);
    analogWrite(Roter4, RotateVal4);

    //Serial.print(RotateVal2);
    //Serial.print(" ");
    /*
      Serial.print(RotateVal1 );
      Serial.print(" ");
      Serial.print(RotateVal2);
      Serial.print(" ");
      Serial.print(RotateVal3 );
      Serial.print(" ");
      Serial.print(RotateVal4 );
      Serial.print(" ");
    */
    //Serial.print(analogRead(Current1));
    //Serial.print(" ");
    /*
      Serial.print(Amp1, 8);
      Serial.print(" ");
      Serial.print(Amp2, 8);
      Serial.print(" ");
      Serial.print(Amp3, 8);
      Serial.print(" ");
      Serial.println(Amp4, 8);
    */
    /*
        Serial.print(Ukp2, 8);
        Serial.print(" ");
        Serial.print(Dkp2, 8);
        Serial.print(" ");
        Serial.println(kitau2 * sumtauTil2, 8);
    */

    Serial.print(Ukp3, 8);
    Serial.print(" ");
    Serial.print(Dkp3, 8);
    Serial.print(" ");
    Serial.println(kitau3 * sumtauTil3, 8);

    /*
        Serial.print(TargetA1 * 10, 8);
        Serial.print(" ");
        Serial.print(Amp1 * 10, 8);
        Serial.print(" ");
        Serial.print(TargetA2 * 10, 8);
        Serial.print(" ");
        Serial.print(Amp2 * 10, 8);
        Serial.print(" ");
        Serial.print(TargetA3 * 10, 8);
        Serial.print(" ");
        Serial.print(Amp3 * 10, 8);
        Serial.print(" ");
        Serial.print(TargetA4 * 10, 8);
        Serial.print(" ");
        Serial.print(Amp4 * 10, 8);
        Serial.println(" ");
    */

    //if(Flag == 1){
    /*
          Serial.print(AX );
          Serial.print(" ");
          Serial.println(AY );/*
          Serial.print(" ");
          Serial.println(AZ );/*
          Serial.print(" ");
          Serial.print(GX );
          Serial.print(" ");
          Serial.print(GY );
          Serial.print(" ");
          Serial.print(GZ );
          Serial.println(" ");
    */
    /*
        Serial.print(gx , 8);
        Serial.print(" ");
        Serial.print(gy , 8);
        Serial.print(" ");
        Serial.print(gz , 8);
        Serial.println(" ");
    */
    /*
        Serial.print(f1 , 8);
        Serial.print(" ");
        Serial.print(f2 , 8);
        Serial.print(" ");
        Serial.print(f3 , 8);
        Serial.print(" ");
        Serial.print(f4 , 8);
        Serial.println(" ");
    */

    /*
        Serial.print(theta1 , 8);
        Serial.print(" ");
        Serial.print(theta2 , 8);
        Serial.print(" ");
        Serial.print(theta3 , 8);
        Serial.println(" ");
    */
    /*
      Serial.print(tau1 * 1000 , 8);
      Serial.print(" ");
      Serial.print(tau_til1 * 1000 , 8);
      Serial.print(" ");
      Serial.print(tau2 * 1000 , 8);
      Serial.print(" ");
      Serial.print(tau_til2 * 1000 , 8);
      Serial.print(" ");
      Serial.print(tau3 * 1000, 8);
      Serial.print(" ");
      Serial.print(tau_til3 * 1000 , 8);
      Serial.println(" ");
    */
    /*
        Serial.print(tau1 * 10000 , 8);
        Serial.print(" ");
        Serial.print(tau2 * 10000 , 8);
        Serial.print(" ");
        Serial.print(tau3 * 10000 , 8);
        Serial.println(" ");
    */
    /*
        Serial.print(RollD , 8);
        Serial.print(" ");
        Serial.print(YawD , 8);
        Serial.print(" ");
        Serial.print(PitchD , 8);
        Serial.println(" ");
    */

    /*
      Serial.print(Rx, 8);
      Serial.print(" ");
      Serial.print(Ry, 8);
      Serial.print(" ");c
    */
    /*
        Serial.print( VolumeValue, 8);
        Serial.print(" ");
        Serial.print(VolumeValue2 , 8);
        Serial.print(" ");
        Serial.print(VolumeValue3 , 8);
        Serial.print(" ");
        Serial.print(VolumeValue4 , 8);
        Serial.println(" ");
    */
    /*
      Serial.print(HRz[0], 8);
      Serial.println(" ");
    */
    /*
      Serial.print(sumAz, 8);
        Serial.println(" ");
    */

    /*
        Serial.print(Haox[0], 8);
        Serial.print(" ");
        Serial.print(Haoy[0], 8);
        Serial.print(" ");
        Serial.print(Haoz[0], 8);
        Serial.println(" ");
    */
    /*
      Serial.print(aox[0], 8);
      Serial.print(" ");
      Serial.print(aoy[0], 8);
      Serial.print(" ");
      Serial.print(aoz[0], 8);
      Serial.println(" ");
    */
    /*
      Serial.print(rax_g[0], 8);
      Serial.print(" ");
      Serial.print(ray_g[0], 8);
      Serial.println(" ");
    */
    digitalWrite(TEST, LOW);
  }


}

void Write(byte address, byte data) {
  digitalWrite(cs, LOW);
  SPI.transfer(address);
  SPI.transfer(data);
  digitalWrite(cs, HIGH);
}
byte Read(byte address) {
  digitalWrite(cs, LOW);
  SPI.transfer(address | 0x80);
  byte data = SPI.transfer(0);
  digitalWrite(cs, HIGH);
  return data;
}
```