倒立摆，pid算法-yjh的回答

倒立摆，pid算法

2019-08-16 19:32发布

站内问答 / STM32/STM8

11844 14

各个模块都正常，但好像算法部分就是有问题，救助啊

#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "timer.h"
#include "adc.h"
#include "lcd.h"
#include "encode.h"

u8 mode;
u16 adcx,count,num;
u8 flag=0;

int err,err_last,err2,err2_last,set_num=1000,set_adcx=2075;
int  pid_bianma,pid_adc;
//float  kp=0,kd=0,ki=0,kp2=3.3,kd2=5,ki2=0,i=0,i2=0; //kp ±àÂëÆ÷ kp2 adc
float  kp=0,kd=0,ki=0,kp2=4.3,kd2=15.0,ki2=0,i=0,i2=0; //kp ±àÂëÆ÷ kp2 adc
int pwma=0,pwmb=0;

float pidbianma(void );
float pidadc(void );
u16 Get_Adc_Average(u8 ch,u8 times);

//int Error,Error_Last,Error1,Error1_Last,Target1 = 3000,Target = 535; //515
//float PID_Out,PID_Out1,PID_Out2;
//float Kp =0,Kd = 0,Kp1 = 0,Kp2 = 50,Kd2 =0;
int main(void)
{
   //u16 led0pwmval=0;
//u16 adcx,volt;
u16 temp;
float jiaodu;
u8 dir=1;
// u8 mode=1;
   u16 count;//±àÂëÆ÷¼ÆÊý PB6 PB7

delay_init();          //ÑÓÊ±º¯Êý³õÊ¼»¯
NVIC_Configuration();    //ÉèÖÃNVICÖÐ¶Ï·Ö×é2:2Î»ÇÀÕ¼ÓÅÏÈ¼¶£¬2Î»ÏìÓ¦ÓÅÏÈ¼¶
uart_init(9600);    //´®¿Ú³õÊ¼»¯Îª9600
   LED_Init();                //LED¶Ë¿Ú³õÊ¼»¯
LCD_Init();
   Adc_Init();                //ADC³õÊ¼»¯
TIM4_Init();//¶¨Ê±Æ÷±àÂëÆ÷½Ó¿Ú³õÊ¼»¯

   TIM3_PWM_Init(999,17);    //²»·ÖÆµ¡&#163

WMÆµÂÊ=4Khz
  TIM5_Int_Init(999,1439);  //¶¨Ê± 10ms 719 5ms 1439

POINT_COLOR=BLUE;//ÉèÖÃ×ÖÌåÎªÀ¶É«
LCD_ShowString(60,130,200,16,16,"ADC_CH0_VAL:");
LCD_ShowString(60,150,200,16,16,"ADC_CH0_JDU:");
   while(1)
{
//       delay_ms(10);
//       if(dir)led0pwmval++;
//       else led0pwmval--;

//       if(led0pwmval>300)dir=0;
//       if(led0pwmval==0)dir=1;
//       //TIM_SetCompare2(TIM3,led0pwmval);

      if(num>1500) num =1500;
      if(num<500)  num =500;

      adcx=Get_Adc_Average(ADC_Channel_1,10);
         LCD_ShowxNum(156,130,adcx,4,16,0);//ÏÔÊ¾ADCµÄÖµ
      jiaodu=adcx*(360/4096);
         LCD_ShowxNum(156,150,jiaodu ,4,16,0);//ÏÔÊ¾µçÑ¹Öµ

      count = TIM4->CNT/4;//»ñÈ¡¼ÆÊýÖµ
      num=count ;
      //delay_ms(500);//Ã¿¸ô1s´òÓ¡Ò»´Î±àÂëÆ÷½Ç¶È,ÓÃÊÖÈ¥²¦¶¯±àÂëÆ÷  Ê¹ÆäÂýËÙÐý×
      printf ("adcx%d  ",adcx );
//       printf ("%d  ",pid_bianma );
      printf ("%d  ",pwma  );
      printf("count%d ",num  );

//          printf("%d ",pwma );
//          printf("%d ",pwmb );

         //delay_ms (500);

//    if(mode ==1)
//    {
//             TIM_SetCompare2(TIM3,500); //TIM3_CH2 PA7
//             TIM_SetCompare1(TIM3,0);    //TIM3_CH1 PA6
//             delay_ms (1000);          //TIM3_CH3 PB0
//             TIM_SetCompare2(TIM3,0);
//             TIM_SetCompare1(TIM3,500);
//             delay_ms (500);
//    }
//    else if(mode==2)
//    {
//             TIM_SetCompare2(TIM3,0);
//             TIM_SetCompare1(TIM3,300);
//             delay_ms (1000);
//             TIM_SetCompare2(TIM3,800);
//             TIM_SetCompare1(TIM3,0);
//             delay_ms (1000);
//          }
//          else if(mode ==3)
//          {
////             pwma =500+pid_bianma +pid_adc ;
////             pwmb= 500-pid_bianma -pid_adc ;
//             if(flag >=4)
//             {
//             flag =0;
//             pwma =500-pidbianma()+pidadc() ;
//             pwmb =500+pidbianma()-pidadc() ;
//             }
//             else
//             {
//                   pwma =500+pidadc ();
//                   pwmb =500-pidadc ();
//                   flag ++;
//             }
//
//             if(pwma >999) pwma =999;
//             if(pwma<0) pwma =0;
//             if(pwmb >999) pwmb =999;
//             if(pwmb<0) pwmb =0;
//
//             TIM_SetCompare2(TIM3,pwma ); //TIM3_CH2 PA7
//             TIM_SetCompare1(TIM3,pwmb );    //TIM3_CH1 PA6
////             delay_ms (1000);          //TIM3_CH3 PB0
////             TIM_SetCompare2(TIM3,0);
////             TIM_SetCompare1(TIM3,500);
////             delay_ms (500);
//
//          }
//          else if(mode ==4)
//          {
//             TIM_SetCompare2(TIM3,0 ); //TIM3_CH2 PA7
//             TIM_SetCompare1(TIM3,0 );    //TIM3_CH1 PA6
//          }
         }


}
// float PID_Cal2()          //??? ???
//{
//Error1 = Target1 -num;       //Target1 = 3000,Target = 515;
//PID_Out2 = Error1*Kp2 + (Error1 - Error1_Last)*Kd2;    //num = 3000
//Error1_Last = Error1;
//return PID_Out2;
//}
      float pidbianma()
      {
         err=set_num-num  ;
         pid_bianma=err*kp+(err-err_last )*kd;
         err_last =err ;
         return pid_bianma ;
      }
      float pidadc()
      {
//       adcx=Get_Adc_Average(ADC_Channel_1,10);
//          err2 =set_adcx - adcx ;
         err2 =set_adcx -Get_Adc_Average(ADC_Channel_1,10) ;
         pid_adc =err2 *kp2 +(err2 -err2_last )*kd2 ;
         err2_last =err2 ;
         return pid_adc ;
      }
void TIM5_IRQHandler(void) //TIM3ÖÐ¶Ï
{
if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) //¼ì²éÖ¸¶¨µÄTIMÖÐ¶Ï·¢ÉúÓë·ñ:TIM ÖÐ¶ÏÔ´
      {
      TIM_ClearITPendingBit(TIM5, TIM_IT_Update  );  //Çå³ýTIMxµÄÖÐ¶Ï´ý´¦ÀíÎ»:TIM ÖÐ¶ÏÔ´
      LED1=!LED1;
         if(adcx <1690||adcx >2570)//2650 3530
         {
            mode =4;
         }
         else  mode =3;

//          if(mode ==1)
//    {
//             TIM_SetCompare2(TIM3,500); //TIM3_CH2 PA7
//             TIM_SetCompare1(TIM3,0);    //TIM3_CH1 PA6
//             delay_ms (1000);          //TIM3_CH3 PB0
//             TIM_SetCompare2(TIM3,0);
//             TIM_SetCompare1(TIM3,500);
//             delay_ms (500);
//    }
//    else if(mode==2)
//    {
//             TIM_SetCompare2(TIM3,0);
//             TIM_SetCompare1(TIM3,300);
//             delay_ms (1000);
//             TIM_SetCompare2(TIM3,800);
//             TIM_SetCompare1(TIM3,0);
//             delay_ms (1000);
//          }
         if(mode ==3)
         {
      err2 =set_adcx -adcx  ;
            pid_adc =err2 *kp2 +(err2 -err2_last )*kd2 +i2*ki2 ;
            err2_last =err2 ;
            i2=i2+err2;

            err=set_num-num  ;
            pid_bianma=err*kp+(err-err_last )*kd+i*ki;
            err_last =err ;
            i=i+err ;

            if(flag >=4)
            {
            flag =0;
//             pwma =500-pidbianma()+pidadc() ;
//             pwmb =500+pidbianma()-pidadc() ;
                  pwma =pid_bianma+pid_adc;
               pwmb =pid_bianma-pid_adc;
            }
            else
            {
//                   pwma =500+pidadc ();
//                   pwmb =500-pidadc ();
                  pwma =pid_adc;
               pwmb =-pid_adc;
                  flag ++;
            }
            if(pwma<0)
            {
                  pwma =-pwma ;
                  if(pwma >600) pwma =600;
                  if(pwma<0) pwma =0;

                  TIM_SetCompare2(TIM3,pwma ); //TIM3_CH2 PA7
               TIM_SetCompare1(TIM3,0  );    //TIM3_CH1 PA6
            }
            else
            {
                  if(pwma >600) pwma =600;
                  if(pwma<0) pwma =0;
                  TIM_SetCompare2(TIM3,0 ); //TIM3_CH2 PA7
               TIM_SetCompare1(TIM3,pwma );    //TIM3_CH1 PA6
            }

//             if(pwma >999) pwma =999;
//             if(pwma<0) pwma =0;
//             if(pwmb >999) pwmb =999;
//             if(pwmb<0) pwmb =0;
//
//             TIM_SetCompare2(TIM3,pwma ); //TIM3_CH2 PA7
//             TIM_SetCompare1(TIM3,pwmb  );    //TIM3_CH1 PA6
//             delay_ms (1000);          //TIM3_CH3 PB0
//             TIM_SetCompare2(TIM3,0);
//             TIM_SetCompare1(TIM3,500);
//             delay_ms (500);

         }
         else if(mode ==4)
         {
            TIM_SetCompare2(TIM3,0 ); //TIM3_CH2 PA7
            TIM_SetCompare1(TIM3,0 );    //TIM3_CH1 PA6
         }



      }
}

友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。

14条回答

2019-08-17 04:30

利用ad采样和编码器的脉冲数构成pid来控制电机的pwm的占空比，检测时单给d时，为什么没有输出值

if(mode ==3)
{
      err2 =set_adcx -adcx  ;
   pid_adc =err2 *kp2 +(err2 -err2_last )*kd2 +i2*ki2 ;
   err2_last =err2 ;
i2=i2+err2;

err=set_num-num  ;
   pid_bianma=err*kp+(err-err_last )*kd+i*ki;
   err_last =err ;
i=i+err ;

if(flag >=4)
{
flag =0;
// pwma =500-pidbianma()+pidadc() ;
// pwmb =500+pidbianma()-pidadc() ;
pwma =500-pid_bianma+pid_adc;
   pwmb =500+pid_bianma-pid_adc;
}
else
{
// pwma =500+pidadc ();
// pwmb =500-pidadc ();
pwma =500+pid_adc;
   pwmb =500-pid_adc;
flag ++;
}
if(pwma<0)
{
pwma =-pwma ;
if(pwma >600) pwma =600;
if(pwma<0) pwma =0;

TIM_SetCompare2(TIM3,pwma ); //TIM3_CH2 PA7
   TIM_SetCompare1(TIM3,0  );    //TIM3_CH1 PA6
}
else
{
if(pwma >600) pwma =600;
if(pwma<0) pwma =0;
TIM_SetCompare2(TIM3,0 ); //TIM3_CH2 PA7
   TIM_SetCompare1(TIM3,pwma );    //TIM3_CH1 PA6
}

查看其它14个回答

一周热门更多>

相关问题

相关文章