LoginSignup
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@GANTZ(GANTZ G)

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

Last updated at Posted at 2021-01-19

概要

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

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

制御方式

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

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

ハードウェア構成

  • Arduino Due
  • 小型モーター：amazon
  • プロペラ:amazon
  • FET 2sk4017:ali
  • MPU9250
  • セメント抵抗0.5Ω：電流計測用
  • 安定化電源:max3Aは消費してるので，あったほうがいいです
  • 可変抵抗器：パラメータを調整するため

FET回路

WIN_20210119_09_58_28_Pro.jpg
ヒートシンクに圧着してます．
2.png

Arduino-Due-Pin-mapping.png

ワイヤーについて

電源，マイコン，FETはワイヤーで接続しているため，普通のドローンとは言えないですが，電源・マイコンだとパワーと軽量化が必要になるので，ワイヤーでつなぐのが楽だと思います．
また，z高さの制御は入れていないです．ワイヤーの重みと釣り合ったところで停止する感じです．
一方で，ワイヤーによって，x-y方向に行き過ぎると回転するトルクが発生するので，ワイヤーはある程度長くとる必要があります．
1.png

本体

WIN_20210119_10_02_52_Pro.jpg

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

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

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

可変抵抗

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

プログラム

要望があれば，解説します．

# 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;
}
1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?