马上注册，玩转Robomaster！
您需要 登录 才可以下载或查看，没有帐号？立即注册
x
单独用位置环还可以, 弄个双环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->Yaw;2,3->Pitch;
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>8)
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>30000)
// Current_Yaw_A = Current_Yaw_A-65535;
// if (Current_Yaw_S>30000)
// Current_Yaw_S = Current_Yaw_S-65535;
// if (Current_Pitch_A>30000)
// Current_Pitch_A = Current_Pitch_A-65535;
// if (Current_Pitch_S>30000)
// 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>0&&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>0)
// Yaw += 400;
// if(Yaw<0)
// Yaw += -400;
// if(Pitch>0)
// Pitch += 400;
// if(Pitch<0)
// Pitch += -400;
// if(Yaw>5000)
// Yaw = 5000;
// else if(Yaw<-5000)
// Yaw = -5000;
// if(Pitch>5000)
// 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->SetSpeed = 0;
PID_x->OutSpeed = 0.0;
PID_x->ActualSpeed = 0;
PID_x->err = 0;
PID_x->err_last = 0;
PID_x->err_next = 0;
PID_x->integral = 0;
PID_x->Kp = p;
PID_x->Ki = i;
PID_x->Kd = d;
// PID_x->Emax = max;
// PID_x->Emin = 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->ActualSpeed = ActualSpeed;
PID_x->err_next = PID_x->err;
PID_x->err = PID_x->SetSpeed - PID_x->ActualSpeed;
PID_x->integral += PID_x->err;
if(PID_x->err > 800||PID_x->err < -800)
{
P_set=1.8;
I_set=0;
}
else if(PID_x->err > 400||PID_x->err < -400)
{
P_set=1.5;
I_set=0.3;
}
else if(PID_x->err > 100||PID_x->err < -100)
{
P_set=1.2;
I_set=1;
}
else
{
P_set=0.5;
I_set=0.3;
}
PID_x->integral*=I_set=1;
PID_x->OutSpeed = PID_x->Kp * PID_x->err * P_set+
PID_x->Ki * PID_x->integral * I_set+
PID_x->Kd * (PID_x->err - PID_x->err_next);
return PID_x->OutSpeed;
}
/**
* @brief ËٶȻ·pidº¯Êý ²ÉÓÃÔöÁ¿Ê½Ëã·¨
* @param pid½á¹¹Ìå ËÙ¶Èʵ¼ÊÖµ
* @retval ·µ»ØpidÊä³ö Á¬½ÓµçÁ÷»·
* @note ²ÉÓÃÔöÁ¿Ê½Ëã·¨
*/
int PID_realize_speed(pid_type *PID_x, int ActualSpeed)
{
PID_x->ActualSpeed = ActualSpeed;
PID_x->err_last = PID_x->err_next;
PID_x->err_next = PID_x->err;
PID_x->err = PID_x->SetSpeed - PID_x->ActualSpeed;
PID_x->OutSpeed = PID_x->Kp * (PID_x->err - PID_x->err_next) +
PID_x->Ki * PID_x->err +
PID_x->Kd * (PID_x->err - 2*PID_x->err_next + PID_x->err_last);
return PID_x->OutSpeed;
}
|
分享
顶
踩
照妖镜