电机驱动程序，简单PID调试

ggber

#include "stm32f10x.h"
#include "pwm.h"
#include "delay.h"
#include "oled.h"
#include "encode.h"
#include "usart.h"
#include "string.h"
#include "sys.h"

#define PWMPeriod   719

    u16 sum=0,n=1;
    float num0=0,num1=0,num4=0,num3=0;
    float a[3],w[3],angle[3],T;
        char str[6],str1[3],str2[5],str3[12];
    float x,xv;
    u16 V,num;
   
   
extern unsigned char Re_buf[11],temp_buf[11],counter;
extern unsigned char sign;


/*绝对式PID算法，接口参数结构类型*/
typedef struct
{
/*PID算法接口变量，用于给用户获取或修改PID算法的特性*/
float kp;     //比例系数
float ki;     //积分系数
float kd;     //微分系数
float errILim;//误差积分上限

float errNow;//当前的误差
float ctrOut;//控制量输出

/*PID算法内部变量，其值不能修改*/
float errOld;
float errP;
float errI;
float errD;

}PID_AbsoluteType;


/*增量式PID算法，接口参数结构类型*/
typedef struct
{
/*PID算法接口变量，用于给用户获取或修改PID算法的特性*/
float kp;     //比例系数
float ki;     //积分系数
float kd;     //微分系数

float errNow; //当前的误差
float dCtrOut;//控制增量输出
float  ctrOut;//控制输出

/*PID算法内部变量，其值不能修改*/
float errOld1;
float errOld2;

}PID_IncrementType;
   

void drc(int a)   //3表示电机制动，1表示电机正转，2表示电机反转，PA12接IN1，PA11接IN2
{
   
    GPIO_InitTypeDef    GPIO_InitStructure;   
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
    GPIO_InitStructure.GPIO_Mode    =    GPIO_Mode_Out_PP;   
    GPIO_InitStructure.GPIO_Pin    =    GPIO_Pin_8|GPIO_Pin_1;        
    GPIO_InitStructure.GPIO_Speed    =GPIO_Speed_50MHz;   
    GPIO_Init(GPIOB,&GPIO_InitStructure);   
    GPIO_ResetBits(GPIOB,GPIO_Pin_8|GPIO_Pin_1);   
   
    switch(a)
    {
        
        case 1:        GPIO_ResetBits(GPIOB,GPIO_Pin_8);        GPIO_SetBits(GPIOB,GPIO_Pin_1);break;//正转(逆时针)
        case 2:        GPIO_ResetBits(GPIOB,GPIO_Pin_1);      GPIO_SetBits(GPIOB,GPIO_Pin_8);break;//反转（顺时针）
        case 3:   GPIO_ResetBits(GPIOB,GPIO_Pin_8);   GPIO_ResetBits(GPIOB,GPIO_Pin_1);    break;//电机立即制动
    }
   
}

    s32 MotorSpeed,PwmOne_Set,PwmOne_Mode;

void UserMotorSpeedSetOne(s32 control)//电机1转速/转向设置（TIM1）
{        


    MotorSpeed = control;//读取PID的输入值
                                               
    if(MotorSpeed > PWMPeriod)  MotorSpeed =   PWMPeriod-1 ;//上限 CCR的值必须小于或等于ARR的值
    if(MotorSpeed <-PWMPeriod)  MotorSpeed = -(PWMPeriod-1);//下限

    if(MotorSpeed<0) { PwmOne_Set = -MotorSpeed; PwmOne_Mode =0;}//反
    else { PwmOne_Set = MotorSpeed; PwmOne_Mode =1;}//正
   
    if(PwmOne_Mode) { TIM4->CCR4 = PwmOne_Set;drc(1);} //MotorOneForward正转
    else { TIM4->CCR4 = PwmOne_Set;drc(2); } //MotorOneBack    反转
   

}
   
//绝对式PID算法
void PID_AbsoluteMode(PID_AbsoluteType* PID)
{
if(PID->kp      < 0)    PID->kp      = -PID->kp;
if(PID->ki      < 0)    PID->ki      = -PID->ki;
if(PID->kd      < 0)    PID->kd      = -PID->kd;
if(PID->errILim < 0)    PID->errILim = -PID->errILim;

PID->errP = PID->errNow;  //读取现在的误差，用于kp控制

PID->errI += PID->errNow; //误差积分，用于ki控制

if(PID->errILim != 0)       //微分上限和下限
{
  if(     PID->errI >  PID->errILim)    PID->errI =  PID->errILim;
  else if(PID->errI < -PID->errILim)    PID->errI = -PID->errILim;
}

PID->errD = PID->errNow - PID->errOld;//误差微分，用于kd控制

PID->errOld = PID->errNow;    //保存现在的误差

PID->ctrOut = PID->kp * PID->errP + PID->ki * PID->errI + PID->kd * PID->errD;//计算绝对式PID输出

}


/************闭环增量*****************************/

void PID_IncrementMode(PID_IncrementType* PID)
{
float dErrP, dErrI, dErrD;

if(PID->kp < 0)    PID->kp = -PID->kp;
if(PID->ki < 0)    PID->ki = -PID->ki;
if(PID->kd < 0)    PID->kd = -PID->kd;

dErrP = PID->errNow - PID->errOld1;

dErrI = PID->errNow;

dErrD = PID->errNow - 2 * PID->errOld1 + PID->errOld2;

PID->errOld2 = PID->errOld1; //二阶误差微分
PID->errOld1 = PID->errNow;  //一阶误差微分

/*增量式PID计算*/
PID->dCtrOut = PID->kp * dErrP + PID->ki * dErrI + PID->kd * dErrD;

if(PID->kp == 0 && PID->ki == 0 && PID->kd == 0)   PID->ctrOut = 0;

else PID->ctrOut += PID->dCtrOut;
}

/*****************************************电机速度环伺服***********************************************/

s32 spdTag, spdNow, control;//定义一个目标速度，采样速度，控制量

//PID_AbsoluteType PID_Control;//定义PID算法的结构体（位置）

PID_IncrementType PID_Control;//定义PID算法的结构体（增量）

void User_PidSpeedControl(s32 SpeedTag)
{
   spdNow = V; spdTag = SpeedTag;

   PID_Control.errNow = spdTag - spdNow; //计算并写入速度误差
      
   PID_Control.kp      = 0.045;             //写入比例系数为15
   PID_Control.ki      = 0.015;              //写入积分系数为5
   PID_Control.kd      = 0.015;              //写入微分系数为5
   
//   PID_Control.errILim = 10000000;           //写入误差积分上限为1000 下限为-1000
//   PID_AbsoluteMode(&ampID_Control);       //执行位置式PID算法
   
   PID_IncrementMode(&ampID_Control);       //执行增量式PID算法
   
   control = PID_Control.ctrOut;         //读取控制值
//if( PID_Control.errNow>10|| PID_Control.errNow<-10)
   UserMotorSpeedSetOne(control);        //放入PWM，用于收敛速度的控制中


}





void TIM3_IRQHandler()//读取脉冲计时器
{
    sum++;
    TIM3->CNT=0;
    TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}

void TIM2_IRQHandler()//0.01s固定时间获取TIM3的计数,num4输出0.1s的脉冲
{
    num0=num1;//0.01s前的脉冲个数
    num1=(float)TIM3->CNT;//0.01s后的脉冲个数
    num3=(num1-num0)+num3;//num3累积计算每一次中断的脉冲，10次后输出
    if(n%10==0)
    {
        num4=num3;
        num3=0;
        if(n==100000)
            n=0;
    }
    n++;
    TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除中断
}




int main()
{   

int i=0;
    USART1_Config();
    delay_init(72);                     //延时函数初始化     


    pwm_PB9_init(719,0);    //D13控制电机
    TIM4->CCR4=500;

    while(1)
    {
//  i++;
//            TIM4->CCR4=i;
//        if(i==718)
//            i=0;
//        delay_ms(100);
    }
}

feipeng