单独用位置环还可以, 弄个双环pid就废了,尤其速度环这块,始终调不出想要的效果,有没有哪位能够给点建议,比如mpu6050的数据需要怎么处理?P给大了等幅震荡,小了没力量.贴一段渣渣代码:
#include "PZT_motor.h"
#include "can.h"
#include "key.h"


pid_type Yaw_PID_S;//Æ«º½ ËÙ¶Èpid
pid_type Pitch_PID_S;//¸©Ñö ËÙ¶Èpid
pid_type Yaw_PID_A;//Æ«º½ ½Ç¶Èpid
pid_type Pitch_PID_A;//¸©Ñö ½Ç¶Èpid


//²ÉÑùʱ¼ä£º
#define arr14 200-1
#define psc14 840-1


//YAW
//½Ç¶È»·£º
#define Angle_P_Y 0.6
#define Angle_I_Y 0
#define Angle_D_Y 0.6
//ËٶȻ·£º
#define Speed_P_Y 0.28
#define Speed_I_Y 0.026
#define Speed_D_Y 0


//Pitch
//½Ç¶È»·£º
#define Angle_P_P 0.6
#define Angle_I_P 0
#define Angle_D_P 0.1
//ËٶȻ·£º
#define Speed_P_P 0.6
#define Speed_I_P 0
#define Speed_D_P 0


extern char a;
int flag=0;
u8 Can_buf_S[8]; //·¢ËÍcanbuf bit: 0,1->aw;2,3->itch;
u16 Angle_Yaw_S,Angle_Pitch_S,Angle_Yaw_A,Angle_Pitch_A;//»úе½Ç¶È(λÖÃ)
int Current_Yaw_S,Current_Pitch_S,Current_Yaw_A,Current_Pitch_A;//µçÁ÷Öµ
int Yaw,Pitch;
extern short x,Speed_Yaw_A,Speed_Pitch_A;




/**
* @brief ÔÆ̨³õʼ»¯º¯Êý
* @param
* @retval
* @note None
*/
void PZT_Init()
{
//<---------------¶¨Ê±Æ÷14 PID¼ÆËã Êý¾Ý²ÉÑù --------------------->
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14,ENABLE); ///ʹÄÜTIM14ʱÖÓ

TIM_TimeBaseInitStructure.TIM_Period = arr14; //×Ô¶¯ÖØ×°ÔØÖµ
TIM_TimeBaseInitStructure.TIM_Prescaler = psc14; //¶¨Ê±Æ÷·ÖƵ
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //ÏòÉϼÆÊýģʽ
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;

TIM_TimeBaseInit(TIM14,&TIM_TimeBaseInitStructure);//³õʼ»¯TIM14

TIM_ITConfig(TIM14,TIM_IT_Update,ENABLE); //ÔÊÐí¶¨Ê±Æ÷14¸üÐÂÖжÏ
TIM_Cmd(TIM14,ENABLE); //ʹÄܶ¨Ê±Æ1÷

NVIC_InitStructure.NVIC_IRQChannel=TIM8_TRG_COM_TIM14_IRQn; //¶¨Ê±Æ÷14ÖжÏ
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //ÇÀÕ¼ÓÅÏȼ¶1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //×ÓÓÅÏȼ¶1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
//<---------------PID ³õʼ»¯º¯Êý--------------------------------->
PID_Init(&Yaw_PID_S,Speed_P_Y,Speed_I_Y,Speed_D_Y); //Æ«º½ËٶȻ·
PID_Init(&Yaw_PID_A,Angle_P_Y,Angle_I_Y,Angle_D_Y); //Æ«º½½Ç¶È»·
PID_Init(&Pitch_PID_S,Speed_P_P,Speed_I_P,Speed_D_P);//¸©ÑöËٶȻ·
PID_Init(&Pitch_PID_A,Angle_P_P,Angle_I_P,Angle_D_P);//¸©Ñö½Ç¶È»·
//<---------------ÔÆ̨µç»úË«Öá¹éÁã------------------------------>
PZT_Yaw_Pitch_Ctrl(0, 0);
}




/**
* @brief ÔÆ̨¿ØÖƺ¯Êý
* @param Yaw->©Ñö¿ØÖÆ Pitch->­¹ö¿ØÖÆ
* @retval
* @note None
*/


void PZT_Yaw_Pitch_Ctrl(int Y, int P)
{
Yaw_PID_A.SetSpeed = Y;
Pitch_PID_A.SetSpeed = P;
}




/**
* @brief ÔÆ̨PIDÖÜÆÚ´¦Àíº¯Êý
* @param
* @retval
* @note Ò»¶¨ÆµÂʽøÐÐpid¼ÆËãºÍ¶ÔÔÆ̨µÄ¿ØÖÆ
*/
void TIM8_TRG_COM_TIM14_IRQHandler(void)
{
int Y_out,P_out;

if(TIM_GetITStatus(TIM14,TIM_IT_Update)==SET) //Òç³öÖжÏ
{
flag++;
if(flag>)
flag = 0;
Angle_Yaw_A = Can_buf_R[4][0]<<8 | Can_buf_R[4][1] ; //»úе½Ç¶È·¶Î§0-8191(0x1FFF)
// Current_Yaw_A = Can_buf_R[4][2]<<8| Can_buf_R[4][3]; //ʵ¼Êת¾ØµçÁ÷Öµ
// Current_Yaw_S = Can_buf_R[4][4]<<8| Can_buf_R[4][5]; //¸ø¶¨µçÁ÷Öµ
Angle_Pitch_A = Can_buf_R[5][0]<<8 | Can_buf_R[5][1] ;
// Current_Pitch_A = Can_buf_R[5][2]<<8 | Can_buf_R[5][3];
// Current_Pitch_S = Can_buf_R[5][4]<<8 | Can_buf_R[5][5];
// if (Current_Yaw_A>0000)
// Current_Yaw_A = Current_Yaw_A-65535;
// if (Current_Yaw_S>0000)
// Current_Yaw_S = Current_Yaw_S-65535;
// if (Current_Pitch_A>0000)
// Current_Pitch_A = Current_Pitch_A-65535;
// if (Current_Pitch_S>0000)
// Current_Pitch_S = Current_Pitch_S-65535;


if(flag == 7)
{
Yaw_PID_S.SetSpeed = -PID_realize_Angle(&Yaw_PID_A, Angle_Yaw_A);
Pitch_PID_S.SetSpeed = -PID_realize_Angle(&Pitch_PID_A, Angle_Pitch_A);
}
// if(a == 'a')
// Yaw_PID_S.SetSpeed = 800;
// else if(a == 'b')
// Yaw_PID_S.SetSpeed = -800;
// else
// Yaw_PID_S.SetSpeed = 0;
// Pitch_PID_S.SetSpeed = 0;
// Speed_Yaw_A /= 10;
// Speed_Pitch_A /= 10;

Yaw += PID_realize_speed(&Yaw_PID_S, -Speed_Yaw_A);
Pitch += PID_realize_speed(&Pitch_PID_S, -Speed_Pitch_A);


if(Yaw>&&Yaw<400)
{
Can_buf_S[0] = ((Yaw+200))/256;Can_buf_S[1] = ((Yaw+200))%256;
}
if(Yaw<0&&Yaw>-400)
{
Can_buf_S[0] = ((Yaw-275))/256;Can_buf_S[1] = ((Yaw-275))%256;
}
// if(Yaw>)
// Yaw += 400;
// if(Yaw<0)
// Yaw += -400;
// if(Pitch>)
// Pitch += 400;
// if(Pitch<0)
// Pitch += -400;
// if(Yaw>000)
// Yaw = 5000;
// else if(Yaw<-5000)
// Yaw = -5000;
// if(Pitch>000)
// Pitch = 5000;
// else if(Pitch<-5000)
// Pitch = -5000;


// Can_buf_S[0] = Yaw/256;Can_buf_S[1] = Yaw%256;
Can_buf_S[2] = Pitch/256;Can_buf_S[3] = Pitch%256;
CAN1_Send_PZT_Msg(Can_buf_S,8);

// printf("Y:%d\t P:%d \t err:%d \t P:%f \tout:%d\n",Angle_Yaw_A,Angle_Pitch_A,Pitch_PID_A.err,Pitch_PID_A.Kp,Pitch);

}
TIM_ClearITPendingBit(TIM14,TIM_IT_Update); //Çå³ýÖжϱê־λ
}


/**
* @brief pid³õʼ»¯º¯Êý
* @param pid½á¹¹Ìå P I DÈý¸ö²ÎÊý
* @retval ÎÞ
* @note None
*/
void PID_Init(pid_type *PID_x, float p, float i, float d)
{
PID_x->etSpeed = 0;
PID_x->utSpeed = 0.0;
PID_x->ctualSpeed = 0;
PID_x->rr = 0;
PID_x->rr_last = 0;
PID_x->rr_next = 0;
PID_x->ntegral = 0;
PID_x->p = p;
PID_x->i = i;
PID_x->d = d;
// PID_x->max = max;
// PID_x->min = min;
}


/**
* @brief ½Ç¶È»·pidº¯Êý ²ÉÓÃλÖÃʽËã·¨
* @param pid½á¹¹Ìå ËÙ¶Èʵ¼ÊÖµ
* @retval ·µ»ØpidÊä³ö Á¬½ÓËٶȻ·
* @note ²ÉÓÃλÖÃʽËã·¨
*/
int PID_realize_Angle(pid_type *PID_x, int ActualSpeed)
{
float P_set=1.0,I_set=1.0;
PID_x->ctualSpeed = ActualSpeed;
PID_x->rr_next = PID_x->rr;
PID_x->rr = PID_x->etSpeed - PID_x->ctualSpeed;
PID_x->ntegral += PID_x->rr;

if(PID_x->rr > 800||PID_x->rr < -800)
{
P_set=1.8;
I_set=0;
}
else if(PID_x->rr > 400||PID_x->rr < -400)
{
P_set=1.5;
I_set=0.3;
}
else if(PID_x->rr > 100||PID_x->rr < -100)
{
P_set=1.2;
I_set=1;
}
else
{
P_set=0.5;
I_set=0.3;
}
PID_x->ntegral*=I_set=1;
PID_x->utSpeed = PID_x->p * PID_x->rr * P_set+
PID_x->i * PID_x->ntegral * I_set+
PID_x->d * (PID_x->rr - PID_x->rr_next);


return PID_x->utSpeed;
}


/**
* @brief ËٶȻ·pidº¯Êý ²ÉÓÃÔöÁ¿Ê½Ëã·¨
* @param pid½á¹¹Ìå ËÙ¶Èʵ¼ÊÖµ
* @retval ·µ»ØpidÊä³ö Á¬½ÓµçÁ÷»·
* @note ²ÉÓÃÔöÁ¿Ê½Ëã·¨
*/
int PID_realize_speed(pid_type *PID_x, int ActualSpeed)
{
PID_x->ctualSpeed = ActualSpeed;
PID_x->rr_last = PID_x->rr_next;
PID_x->rr_next = PID_x->rr;
PID_x->rr = PID_x->etSpeed - PID_x->ctualSpeed;



PID_x->utSpeed = PID_x->p * (PID_x->rr - PID_x->rr_next) +
PID_x->i * PID_x->rr +
PID_x->d * (PID_x->rr - 2*PID_x->rr_next + PID_x->rr_last);


return PID_x->utSpeed;
}