mini开发板使用pid算法控制舵机转动

阳光男孩

我用的是mini开发板使用PID算法控制舵机转动的，但是舵机转动过程中感觉转动的不对，自己看不出来哪里出错了，可以帮忙给指点吗，我用的是sg90舵机。#include "control.h"
#include "led.h"
#include "pwm.h"
#include "lcd.h"
#include "key.h"
#include "usart.h"          


PID_TYPE X_PID;
PID_TYPE Y_PID;
UINT8_XY Coordinate = {80,60};//摄像头捕获坐标值40,34

float PidAlgorithm(PID_TYPE *SETPID,float SetValue ,float SensorValue);
void Motordrive(u16 *pwmx,u16 *pwmy);


//注意：C语言中，全局变量和局部静态变量是存储在静态存储区的，他们在分配的时候都被系统默认初始化为0；
//而局部自动变量是在栈上分配内存的，如果不对它们进行初始化，那么他们可能是任意的随机值。
/******************************************************************
*函数名称:        SetParameter
*函数功能:        修改pid参数
*函数参数:       
*返 回 值:       
*******************************************************************/
void SetParameter(PID_TYPE *SETPID,float p,float i,float d)
{
        SETPID->P = p;
        SETPID->I = i;
        SETPID->D = d;
}

/******************************************************************
*函数名称:        TIM4_IRQHandler
*函数功能:        定时器4用于5ms中断
*函数参数:        无
*返 回 值:        无
*******************************************************************/
void TIM4_IRQHandler(void)   //TIM3中断
{
                static u16 time = 0;                        //呼吸灯计数变量
          static u16 OutputX,OutputY;
                const u8 Benchmark_X = 80,Benchmark_Y = 60;//基准值 40，34
       
        if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
                {       
                        time++;
                        if(time > 7){LED0 = !LED0;LED1 = !LED1;time = 0;}
                       
                        SetParameter(&X_PID,0.35,0.0025,0.1);
                        SetParameter(&Y_PID,0.35,0.0025,0.1);
                       
                        //SetParameter(&X_PID,0.05,0.0025,0.01);
                        //etParameter(&Y_PID,0.05,0.0025,0.01);

                        OutputX = MEDIAN_X + PidAlgorithm(&X_PID,Benchmark_X,Coordinate.X);
                        OutputY = MEDIAN_Y + PidAlgorithm(&Y_PID,Benchmark_Y,Coordinate.Y);
                       
                       
                        Motordrive(&OutputX,&OutputY);
                       
                        TIM_ClearITPendingBit(TIM4, TIM_IT_Update );  //清除TIMx的中断待处理位:TIM 中断源
                }
}

/******************************************************************
*函数名称: PidAlgorithm
*函数功能: pid算法
*函数参数:
*返 回 值:
*******************************************************************/
float PidAlgorithm(PID_TYPE *SETPID,float SetValue ,float SensorValue){
               
                SETPID->Error = SetValue - SensorValue;
          SETPID->Integral += SETPID->Error;
          SETPID->Differ = SETPID->Error - SETPID->LastError;
       
//                if(SETPID->Integral > 2300) SETPID->Integral = 2300;
//    if(SETPID->Integral < -2300) SETPID->Integral = -2300;
       
          SETPID->OutPut = (  SETPID->P * SETPID->Error
                                                                                        + SETPID->I * SETPID->Integral
                                                                                        + SETPID->D * SETPID->Differ    );
       
                SETPID->LastError = SETPID->Error;
       
          return SETPID->OutPut;
}

/******************************************************************
*函数名称:     Motordrive
*函数功能:     驱动电机
*函数参数:                         X/Y轴方向PID闭环反馈输出
*******************************************************************/
void Motordrive(u16 *pwmx,u16 *pwmy)
{
                if(*pwmx > 100) *pwmx = 100;
          if(*pwmx < 40) *pwmx = 40;
                if(*pwmy > 100) *pwmy = 100;  
          if(*pwmy < 50) *pwmy = 50;
//                        printf("\n\r\n\rOUT_X : %d\n\r",*pwmx);
//                        printf("\n\rOUT_Y : %d\n\r",*pwmy);

                SERVO_X = *pwmx;
          SERVO_Y = *pwmy;
}

宇智波白

帮顶                                       

WZTENG

顶一顶

阳光男孩

请问SG90舵机的
TIM_TimeBaseStructure.TIM_Period = arr;
TIM_TimeBaseStructure.TIM_Prescaler = prc;
arr，prc一般设置多少，20ms周期

PeterYu

帮忙顶