六旋翼飛行平衡 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 = -(Px+Ix+Dx) + (Py+Iy+Dy);
tFix =(Py+Iy+Dy);
tFix =(Px+Ix+Dx) + (Py+Iy+Dy);
tFix =(Px+Ix+Dx) - (Py+Iy+Dy);
tFix = -(Py+Iy+Dy);
tFix = -(Px+Ix+Dx) - (Py+Iy+Dy);
lasttime = millis();
}
int val;
void setSpeed(int speed)
{
val = map(speed, 0 , 100 , 0 , 180);//10972000
for(int c=0; c<6; c++) {
float fix=0;
if ( (tFixST-0.04) <= tFix && (tFixST+0.04 ) >= tFix ) {
fix = tFixST;
Serial.println(fix);
if (c==0) myservo.write(val + fix + (int)speedFB - (int)speedRL + (int)speedHRL);
else if (c==1) myservo.write(val + fix - (int)speedRL - (int)speedHRL);
else if (c==2) myservo.write(val + fix - (int)speedFB - (int)speedRL + (int)speedHRL);
else if (c==3) myservo.write(val + fix - (int)speedFB + (int)speedRL - (int)speedHRL);
else if (c==4) myservo.write(val + fix + (int)speedRL + (int)speedHRL);
else myservo.write(val + fix + (int)speedFB + (int)speedRL - (int)speedHRL);
}
else {
tFixST = tFix;
fix = tFix;
Serial.println(fix);
if (c==0) myservo.write(val + fix + (int)speedFB - (int)speedRL + (int)speedHRL);
else if (c==1) myservo.write(val + fix - (int)speedRL - (int)speedHRL);
else if (c==2) myservo.write(val + fix - (int)speedFB - (int)speedRL + (int)speedHRL);
else if (c==3) myservo.write(val + fix - (int)speedFB + (int)speedRL - (int)speedHRL);
else if (c==4) myservo.write(val + fix + (int)speedRL + (int)speedHRL);
else myservo.write(val + fix + (int)speedFB + (int)speedRL - (int)speedHRL);
}
}
}
頁:
[1]