[嵌入式] 请问大家的云台速度环是怎么调的?

[复制链接]
英雄就这样吧
2017-11-28 16:39:30 显示全部楼层

马上注册,玩转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;
}


TIM图片20171128163549.png
跳转到指定楼层

[嵌入式] 请问大家的云台速度环是怎么调的?

[复制链接]
梯队队员小草小草
2021-3-3 09:50:18 显示全部楼层
电流值对应的速度大小是什么关系?
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

触屏版 | 电脑版

Copyright © 2023 RoboMasters 版权所有 备案号 粤ICP备2022092332号

快速回复 返回顶部 返回列表