Robofun 機器人論壇

標題: 六旋翼飛行平衡 PID控制求救 [打印本頁]

作者: SherlockRoy 時間: 2015-12-4 21:44
標題: 六旋翼飛行平衡 PID控制求救
目前小弟在做有六旋翼飛行平衡看到了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);
}
  }
}

歡迎光臨 Robofun 機器人論壇 (https://www.robofun.net/forum/)