STM32实现编码器电机【速度与位置环闭环控制】

/* 此文件为编码器电机闭环调试，包括速度环和位置环

 * 配置：TIM2(PA0、PA1)：编码器模式

 *                          TIM1-CH1(PE9)：PWM输出

 *                         IN1：PF1

 *                          IN2: PF2

 *              ENABLE: PF0

 * 使用方法：1、初始化Motor_Init()

 *                                          2、发送电流SetCurrent()

 */

#include  "encoder.h"



encoderMotor_t encoderMoto[ENCODER_MOTO_COUNT];                //编码器电机结构体



pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体

pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体





void Motor_Init(void)

{

                HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);      //开启编码器定时器

                __HAL_TIM_ENABLE_IT(&htim2,TIM_IT_UPDATE);                  //开启编码器定时器更新中断,防溢出处理

                __HAL_TIM_SET_COUNTER(&htim2, 10000);                //编码器定时器初始值设定为10000

        

                encoderMoto[0].IOControl.htim_pwm = htim1;

                encoderMoto[0].IOControl.IN1_Port = GPIOF;

                encoderMoto[0].IOControl.IN2_Port = GPIOF;

                encoderMoto[0].IOControl.IN1_Pin = GPIO_PIN_1;

                encoderMoto[0].IOControl.IN2_Pin = GPIO_PIN_2;

}





//

/* 编码器电机发送电流函数

 * motor：编码器电机参数结构体

 * val：转动的速度或角度，SPEED最大为110，POSITION一圈为3960

 * mode：模式选择：速度环：SPEED

 *                                                                  位置环：POSITION

 */

void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode)

{

                float pos_output,spd_output;

                if(mode == 1)

                        spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, val);

                else

                {

                        pos_output = pid_calc(&Encoder_Motor_Pid_Pos[0], motor->totalAngle, val);

                        spd_output = pid_calc(&Encoder_Motor_Pid_Spd[0], motor->speed, pos_output);

                }



                if(spd_output > 0)

                {

                                HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_SET);                //控制正反转

                                HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_RESET);

                                __HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(spd_output));

                }

                else

                {

                                HAL_GPIO_WritePin(motor->IOControl.IN1_Port, motor->IOControl.IN1_Pin, GPIO_PIN_RESET);

                                HAL_GPIO_WritePin(motor->IOControl.IN2_Port, motor->IOControl.IN2_Pin, GPIO_PIN_SET);

                                __HAL_TIM_SET_COMPARE(&motor->IOControl.htim_pwm, TIM_CHANNEL_1, (uint32_t)(-spd_output));

                }

        

}



void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)

{

        static int16_t count = 0;

        if(htim->Instance==htim6.Instance)                         //1ms中断

        {

                count++;

                if(count >= 10)

                {

                        count = 0;

                        int16_t pluse = COUNTERNUM - RELOADVALUE/2;                                                                                        

                        encoderMoto[0].totalAngle = pluse + encoderMoto[0].loopNum * RELOADVALUE/2;  

                        encoderMoto[0].speed = (float)(encoderMoto[0].totalAngle - encoderMoto[0].lastAngle)/(4*PLUSE_OF_CIRCLE*RR)*6000;                        //进行速度计算,根据前文所说的,4倍频,编码器13位,减速比30,再乘以6000即为每分钟输出轴多少转

                        encoderMoto[0].lastAngle = encoderMoto[0].totalAngle;         //更新转过的圈数

                }

        }

        else if(htim->Instance == htim2.Instance)      //如果是编码器更新中断,即10ms内,脉冲数超过了计数范围,需要进行处理

        {

                if(COUNTERNUM < 10000)        encoderMoto[0].loopNum++;

                else if(COUNTERNUM > 10000)        encoderMoto[0].loopNum--;

                __HAL_TIM_SetCounter(&htim2, 10000);             //重新设定初始值                        

        }

}

#ifndef __ENCODER_H

#define __ENCODER_H



#include "tim.h"

#include "gpio.h"

#include "main.h"

#include "stm32_hal_legacy.h"

#include "pid.h"



#define ENCODER_MOTO_COUNT                1                //编码器电机数量

#define RR        90        //电机减速比

#define PLUSE_OF_CIRCLE                11

#define RELOADVALUE         __HAL_TIM_GetAutoreload(&htim2)    //获取自动装载值,本例中为20000

#define COUNTERNUM                 __HAL_TIM_GetCounter(&htim2)        //获取编码器定时器中的计数值





#define MOTOR_1                1



enum{

    POSITION        = 0,

    SPEED         = 1,

};



/* 编码器电机接口定义结构体 */

typedef struct _IOControl

{

        TIM_HandleTypeDef htim_encoder;

        TIM_HandleTypeDef htim_pwm;

        GPIO_TypeDef *IN1_Port;

        GPIO_TypeDef *IN2_Port;

        uint16_t IN1_Pin;

        uint16_t IN2_Pin;

}IOControl_t;



/* 编码器电机参数结构体 */

typedef struct _EncoderMotor{

        int8_t ID;

        int16_t loopNum;          //防超上限

        int32_t lastAngle;        //上1ms转的角度

        int32_t totalAngle;       //总角度

        float speed;              //电机输出轴转速，单位RPM

        float set;

        IOControl_t IOControl;

}encoderMotor_t;



extern encoderMotor_t encoderMoto[ENCODER_MOTO_COUNT];

extern pid_t Encoder_Motor_Pid_Pos[ENCODER_MOTO_COUNT];  //编码器电机位置环PID结构体

extern pid_t Encoder_Motor_Pid_Spd[ENCODER_MOTO_COUNT];  //编码器电机速度环PID结构体



void SetCurrent(encoderMotor_t *motor, int32_t val, uint32_t mode);

void Motor_Init(void);

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim);



#endif

#include "pid.h"





void abs_limit(float *a, float ABS_MAX)

{

    if(*a > ABS_MAX)

        *a = ABS_MAX;

    if(*a < -ABS_MAX)

        *a = -ABS_MAX;

}



void PID_struct_init(pid_t* pid,

                                                                                 uint32_t maxout,

                                                                                 uint32_t intergral_limit,

                                                                                 float         kp, 

                                                                                 float         ki, 

                                                                           float         kd)

{

    pid->IntegralLimit = intergral_limit;

    pid->MaxOutput = maxout;

    

    pid->p = kp;

    pid->i = ki;

    pid->d = kd;

}





float pid_calc(pid_t* pid, float get, float set)

{

    pid->get = get;

    pid->set = set;

    pid->err = set - get;        /*set - measure，得到偏差*/

    

                pid->pout = pid->p * pid->err;

                pid->iout += pid->i * pid->err;

                pid->dout = pid->d * (pid->err - pid->lastError);

        

                abs_limit(&(pid->iout), pid->IntegralLimit);  /*积分限幅*/

                pid->pos_out = pid->pout + pid->iout + pid->dout;

        

                abs_limit(&(pid->pos_out), pid->MaxOutput);  /*限定输出值的大小*/



    

    /*更新数据*/

    pid->lastError = pid->err;



    return pid->pos_out; /*PID输出*/

}

#ifndef __PID_H_

#define __PID_H_



#include "main.h"



typedef struct __pid_t

{

    float p,i,d;

    float err,lastError;        //误差



    float set;        //目标值

    float get;        //测量值

    

    float pout;                //P输出                                

    float iout;                //I输出                                        

    float dout;                //D输出                

    

    float pos_out;                        //本次位置式输出，即 pos_out = pout + iout + dout

    float last_pos_out;                //上次位置式输出

    



    uint32_t MaxOutput;                        //输出限幅

    uint32_t IntegralLimit;                //积分限幅

    



}pid_t;



void abs_limit(float *a, float ABS_MAX);

void PID_struct_init(pid_t* pid,

                                                                                 uint32_t maxout,

                                                                                 uint32_t intergral_limit,

                                                                                 float         kp, 

                                                                                 float         ki, 

                                                                           float         kd);



float pid_calc(pid_t* pid, float get, float set);



#endif

#include "encoder.h"

#include "pid.h"



int main(void)

{

    HAL_Init();

    SystemClock_Config();

    MX_GPIO_Init();

    MX_TIM1_Init();

    MX_TIM2_Init();

    MX_TIM6_Init();

    /*以上为cube生成*/

  

        HAL_TIM_Base_Start_IT(&htim6);                       //开启1ms定时器中断

        HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);

        Motor_Init();

        

        PID_struct_init(&Encoder_Motor_Pid_Pos[0], 100000, 1000, 0.2, 0.0, 0);

        PID_struct_init(&Encoder_Motor_Pid_Spd[0], 1000, 1000, 30, 0.05, 0.01);



    while (1)

    {

                SetCurrent(&encoderMoto[0], 20, SPEED);        

    }

}