六旋翼飛行平衡 PID控制求救

SherlockRoy

目前小弟在做有六旋翼飛行平衡看到了PID控制這一塊
目前用到了PI這兩個部分而已
但是始終無法讓他在快起飛時穩定
我想請問一下關於這部分得程式碼要怎麼打比較好
目前就是X的模式
　�　�
－－＊－－
　�　�
我固定左右兩軸先做X軸的方向
可是會前後晃動
感覺是Ki值的問題
因為我有乘上一個小數好讓他不要過度補償
但我也不知道Kp值是不是對
不知道是不是有人有經驗可以提供一下
還有就是加速度計的數值有辦法穩定嗎?
不然感覺會影響到

程式碼：

void calculatePID() {

  int c;
   // calculate error

    errorX = x - (-15) ; // (-15)
    errorY = y - 22 ; //  (22)
    x = lastx;
    y = lasty;

    // PI control
    if (errorX >= 5 || errorX <= -5) {
      Px = errorX * Kp;                     //Kp=0.005
      Ix = Ix * 0.9 + (errorX * Ki);    //Ki=0.001
    }

    if (errorY >= 5 || errorY <= -5) {
      Py = errorY * Kp;
      Iy = Iy * 0.9 + (errorY * Ki);   
    }

//   Dx = (errorX - lastErrX) * Kd ;
//   Dy = (errorY - lastErrY) * Kd ;

//   lastErrX = errorX;
//   lastErrY = errorY;

    tFix[0] = -(Px+Ix+Dx) + (Py+Iy+Dy);
    tFix[1] =  (Py+Iy+Dy);
    tFix[2] =  (Px+Ix+Dx) + (Py+Iy+Dy);
    tFix[3] =  (Px+Ix+Dx) - (Py+Iy+Dy);
    tFix[4] = -(Py+Iy+Dy);
    tFix[5] = -(Px+Ix+Dx) - (Py+Iy+Dy);
    lasttime = millis();
}


int val;
void setSpeed(int speed)
{
  val = map(speed, 0 , 100 , 0 , 180);  //1097  2000
  for(int c=0; c<6; c++) {
    float fix=0;
    if ( (tFixST[c]-0.04) <= tFix[c] && (tFixST[c]+0.04 ) >= tFix[c] ) {
      fix = tFixST[c];
      Serial.println(fix);
      if (c==0) myservo[c].write(val + fix + (int)speedFB - (int)speedRL + (int)speedHRL);
      else if (c==1) myservo[c].write(val + fix - (int)speedRL - (int)speedHRL);
      else if (c==2) myservo[c].write(val + fix - (int)speedFB - (int)speedRL + (int)speedHRL);
      else if (c==3) myservo[c].write(val + fix - (int)speedFB + (int)speedRL - (int)speedHRL);
      else if (c==4) myservo[c].write(val + fix + (int)speedRL + (int)speedHRL);
      else myservo[c].write(val + fix + (int)speedFB + (int)speedRL - (int)speedHRL);
    }
    else {
      tFixST[c] = tFix[c];
      fix = tFix[c];
      Serial.println(fix);
      if (c==0) myservo[c].write(val + fix + (int)speedFB - (int)speedRL + (int)speedHRL);
      else if (c==1) myservo[c].write(val + fix - (int)speedRL - (int)speedHRL);
      else if (c==2) myservo[c].write(val + fix - (int)speedFB - (int)speedRL + (int)speedHRL);
      else if (c==3) myservo[c].write(val + fix - (int)speedFB + (int)speedRL - (int)speedHRL);
      else if (c==4) myservo[c].write(val + fix + (int)speedRL + (int)speedHRL);
      else myservo[c].write(val + fix + (int)speedFB + (int)speedRL - (int)speedHRL);
    }
  }
}