基于STM32完成M3508电机PID速度环和角度环控制以及过零处理
PID_clac函数是PID计算函数，填入PID结构体指针，反馈数据（即读到的电机数据），设定值，即可输出PID计算结果；将角度外环的输出结果作为速度内环的SET值，再将速度环的输出给电机函数，即可完成串环控制。先定义pid速度环和角度环结构体变量，再分别给这两个环的PID三参赋值。这个函数写的是每按下一次按键，编码增加1024；创建电机结构体变量，用于接收电机数据。再加入CAN屏蔽器初始化。具体
·
首先用CubeMX生成一个项目
具体步骤在下面这篇有讲基于stm32f103的HAL库CAN通信控制3508电机开环转动，以及Cube软件如何配置。_stm32f103 hal can-CSDN博客
然后生成项目之后，加入bsp文件
bsp_can.c
#include "bsp_can.h"//1
#include "main.h"
extern CAN_HandleTypeDef hcan;
void can_filter_init(void)
{
CAN_FilterTypeDef can_filter_st;
can_filter_st.FilterActivation = ENABLE;
can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
can_filter_st.FilterIdHigh = 0x0000;
can_filter_st.FilterIdLow = 0x0000;
can_filter_st.FilterMaskIdHigh = 0x0000;
can_filter_st.FilterMaskIdLow = 0x0000;
can_filter_st.FilterBank = 0;
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
HAL_CAN_ConfigFilter(&hcan, &can_filter_st);
HAL_CAN_Start(&hcan);
HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
}
bsp_can.h
#ifndef BSP_CAN_H
#define BSP_CAN_H
#include "struct_typedef.h"//1
extern fp32 kp;
extern void can_filter_init(void);
#endif
CAN_recive.c
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file can_receive.c/h
* @brief there is CAN interrupt function to receive motor data,
* and CAN send function to send motor current to control motor.
* 这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. done
* V1.1.0 Nov-11-2019 RM 1. support hal lib
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#include "CAN_receive.h"
#include "main.h"
#include "bsp_rng.h"
extern CAN_HandleTypeDef hcan;
/**
* @brief 解码宏定义
* @param[in] prt:解码后存放的数据
* @param[in] data:待解码数据
* @retval none
*/
#define get_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
}
static motor_measure_t motor_chassis[7];
static CAN_TxHeaderTypeDef chassis_tx_message;
static uint8_t chassis_can_send_data[8];
/**
* @brief hal库CAN回调函数,接收电机数据
* @param[in] hcan:CAN句柄指针
* @retval none
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
switch (rx_header.StdId)
{
case CAN_3508_M1_ID:
case CAN_3508_M2_ID:
case CAN_3508_M3_ID:
case CAN_3508_M4_ID:
{
static uint8_t i = 0;
//获取电机ID
i = rx_header.StdId - CAN_3508_M1_ID;
get_motor_measure(&motor_chassis[i], rx_data);
break;
}
default:
{
break;
}
}
}
/**
* @brief 发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
* @param[in] none
* @retval none
*/
void CAN_cmd_chassis_reset_ID(void)
{
uint32_t send_mail_box;
chassis_tx_message.StdId = 0x700;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
chassis_can_send_data[0] = 0;
chassis_can_send_data[1] = 0;
chassis_can_send_data[2] = 0;
chassis_can_send_data[3] = 0;
chassis_can_send_data[4] = 0;
chassis_can_send_data[5] = 0;
chassis_can_send_data[6] = 0;
chassis_can_send_data[7] = 0;
HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}
/**
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
* @param[in] motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
* @retval none
*/
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
uint32_t send_mail_box;
chassis_tx_message.StdId = 0x200;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
chassis_can_send_data[0] = motor1 >> 8;
chassis_can_send_data[1] = motor1;
chassis_can_send_data[2] = motor2 >> 8;
chassis_can_send_data[3] = motor2;
chassis_can_send_data[4] = motor3 >> 8;
chassis_can_send_data[5] = motor3;
chassis_can_send_data[6] = motor4 >> 8;
chassis_can_send_data[7] = motor4;
HAL_CAN_AddTxMessage(&hcan, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}
/**
* @brief 返回底盘电机 3508电机数据指针
* @param[in] i: 电机编号,范围[0,3]
* @retval 电机数据指针
*/
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
return &motor_chassis[(i & 0x03)];
}
CAN_receive.h
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file can_receive.c/h
* @brief there is CAN interrupt function to receive motor data,
* and CAN send function to send motor current to control motor.
* 这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. done
* V1.1.0 Nov-11-2019 RM 1. support hal lib
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#ifndef CAN_RECEIVE_H
#define CAN_RECEIVE_H
#include "struct_typedef.h"
#define CHASSIS_CAN hcan1
/* CAN send and receive ID */
typedef enum
{
CAN_CHASSIS_ALL_ID = 0x200,
CAN_3508_M1_ID = 0x201,
CAN_3508_M2_ID = 0x202,
CAN_3508_M3_ID = 0x203,
CAN_3508_M4_ID = 0x204,
} can_msg_id_e;
//rm 电机数据
typedef struct
{
uint16_t ecd;//机械角度
int16_t speed_rpm;//电机的转速值
int16_t given_current;//扭矩电流
uint8_t temperate;//温度
int16_t last_ecd;//上一次电机的角度
}motor_measure_t;
/**
* @brief 发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
* @param[in] none
* @retval none
*/
extern void CAN_cmd_chassis_reset_ID(void);
/**
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
* @param[in] motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
* @param[in] motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
* @retval none
*/
extern void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4);
/**
* @brief 返回底盘电机 3508电机数据指针
* @param[in] i: 电机编号,范围[0,3]
* @retval 电机数据指针
*/
extern const motor_measure_t *get_chassis_motor_measure_point(uint8_t i);
#endif
bsp_rng.c
#include "bsp_rng.h"
#include "main.h"
//extern RNG_HandleTypeDef hrng;
uint32_t RNG_get_random_num(void)
{
static uint32_t rng;
// HAL_RNG_GenerateRandomNumber(&hrng, &rng);
return rng;
}
int32_t RNG_get_random_rangle(int min, int max)
{
static int32_t random;
random = (RNG_get_random_num() % (max - min + 1)) + min;
return random;
}
bsp_rng.h
#ifndef BSP_RNG_H
#define BSP_RNG_H
#include "struct_typedef.h"//1
extern uint32_t RNG_get_random_num(void);
extern int32_t RNG_get_random_rangle(int min, int max);
#endif
pid.c
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file pid.c/h
* @brief pid实现函数，包括初始化，PID计算函数，
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. 完成
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#include "pid.h"
#include "main.h"
//限幅函数
#define LimitMax(input, max) \
{ \
if (input > max) \
{ \
input = max; \
} \
else if (input < -max) \
{ \
input = -max; \
} \
}
void PID_angle_control(pid_type_def *pid, fp32 set, fp32 angle)
{
// 计算误差
fp32 error = set - angle;
// 死区处理
if (error < 100 && error > -100)
{
error = 0.0f;
}
// 比例项
fp32 Pout = pid->Kp * error;
// 积分项
pid->Iout += pid->Ki * error;
// 限制积分项
if (pid->Iout > pid->max_iout) {
pid->Iout = pid->max_iout;
}
else if (pid->Iout < -pid->max_iout) {
pid->Iout = -pid->max_iout;
}
// 微分项
fp32 Dout = pid->Kd * (error - pid->error[0]);
// 计算输出
pid->out = Pout + pid->Iout + Dout;
// 限制输出
if (pid->out > pid->max_out) {
pid->out = pid->max_out;
}
else if (pid->out < -pid->max_out) {
pid->out = -pid->max_out;
}
// 更新误差历史记录
pid->error[2] = pid->error[1];
pid->error[1] = pid->error[0];
pid->error[0] = error;
}
/**
* @brief pid struct data init
* @param[out] pid: PID结构数据指针
* @param[in] mode: PID_POSITION:普通PID
* PID_DELTA: 差分PID
* @param[in] PID: 0: kp, 1: ki, 2:kd
* @param[in] max_out: pid最大输出
* @param[in] max_iout: pid最大积分输出
* @retval none
*/
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)
{
if (pid == NULL || PID == NULL)
{
return;
}
pid->mode = mode;
pid->Kp = PID[0];
pid->Ki = PID[1];
pid->Kd = PID[2];
pid->max_out = max_out;
pid->max_iout = max_iout;
pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
}
/**
* @brief pid计算
* @param[out] pid: PID结构数据指针
* @param[in] ref: 反馈数据
* @param[in] set: 设定值
* @retval pid输出
*/
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
if (pid == NULL)
{
return 0.0f;
}
pid->error[2] = pid->error[1];
pid->error[1] = pid->error[0];
pid->set = set;
pid->fdb = ref;
pid->error[0] = set - ref;//误差=期望减去实际
if (pid->mode == PID_POSITION)
{
pid->Pout = pid->Kp * pid->error[0];
pid->Iout += pid->Ki * pid->error[0];
pid->Dbuf[2] = pid->Dbuf[1];
pid->Dbuf[1] = pid->Dbuf[0];
pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
pid->Dout = pid->Kd * pid->Dbuf[0];
LimitMax(pid->Iout, pid->max_iout);
pid->out = pid->Pout + pid->Iout + pid->Dout;
LimitMax(pid->out, pid->max_out);
}
else if (pid->mode == PID_DELTA)
{
pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
pid->Iout = pid->Ki * pid->error[0];
pid->Dbuf[2] = pid->Dbuf[1];
pid->Dbuf[1] = pid->Dbuf[0];
pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
pid->Dout = pid->Kd * pid->Dbuf[0];
pid->out += pid->Pout + pid->Iout + pid->Dout;
LimitMax(pid->out, pid->max_out);
}
return pid->out;
}
/**
* @brief pid 输出清除
* @param[out] pid: PID结构数据指针
* @retval none
*/
void PID_clear(pid_type_def *pid)
{
if (pid == NULL)
{
return;
}
pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;
pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;
pid->fdb = pid->set = 0.0f;
}
pid.h
/**
****************************(C) COPYRIGHT 2016 DJI****************************
* @file pid.c/h
* @brief pid实现函数，包括初始化，PID计算函数，
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. 完成
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2016 DJI****************************
*/
#ifndef PID_H
#define PID_H
#include "struct_typedef.h"
enum PID_MODE
{
PID_POSITION,
PID_DELTA
};
typedef struct
{
uint8_t mode;
//PID 三参数
fp32 Kp;
fp32 Ki;
fp32 Kd;
fp32 max_out; //最大输出
fp32 max_iout; //最大积分输出
fp32 set;
fp32 fdb;
fp32 out;
fp32 Pout;
fp32 Iout;
fp32 Dout;
fp32 Dbuf[3]; //微分项 0最新 1上一次 2上上次
fp32 error[3]; //误差项 0最新 1上一次 2上上次
} pid_type_def;
void PID_angle_control(pid_type_def *pid, fp32 set, fp32 angle);
/**
* @brief pid struct data init
* @param[out] pid: PID结构数据指针
* @param[in] mode: PID_POSITION:普通PID
* PID_DELTA: 差分PID
* @param[in] PID: 0: kp, 1: ki, 2:kd
* @param[in] max_out: pid最大输出
* @param[in] max_iout: pid最大积分输出
* @retval none
*/
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout);
/**
* @brief pid计算
* @param[out] pid: PID结构数据指针
* @param[in] ref: 反馈数据
* @param[in] set: 设定值
* @retval pid输出
*/
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);
/**
* @brief pid 输出清除
* @param[out] pid: PID结构数据指针
* @retval none
*/
extern void PID_clear(pid_type_def *pid);
#endif
main.c
#include "main.h"
#include "can.h"
#include "gpio.h"
#include "bsp_can.h"
#include "CAN_receive.h"
#include "pid.h"
int16_t target,current,speed,angle,angle_output,error;//期望角度，PID输出电流，电机速度，电机角度，角度PID输出
float angle_test;
float Handle_Angle8191_PID_Over_Zero(float tar, float cur);
void SystemClock_Config(void);
int main(void)
{
pid_type_def pid_speed = {0};//速度
pid_type_def pid_angle = {0};//位置
static fp32 kp =10.0; // 设置 kp
static fp32 ki = 0.01; // 设置 ki
static fp32 kd = 1.0; // 设置 kd
fp32 ap = 0.21; // 设置 ap
fp32 ai = 0; // 设置 ai
fp32 ad = 20.0; // 设置 ad
HAL_Init();
SystemClock_Config();
GPIO_Init();
MX_GPIO_Init();
MX_CAN_Init();
can_filter_init();//CAN屏蔽器初始化
PID_clear(&pid_speed);
PID_clear(&pid_angle);
PID_init(&pid_speed,PID_POSITION,(const fp32[3]){kp, ki, kd},10000,2000);//速度环PID初始�?
PID_init(&pid_angle,PID_POSITION,(const fp32[3]){ap, ai, ad},1000,1);//角度环PID初始�?
const motor_measure_t *result = get_chassis_motor_measure_point(0);//接收电机1数据
HAL_CAN_RxFifo0MsgPendingCallback(&hcan);//回调函数
target = 0;
while (1)
{
if(1==HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1))//按键事件
{
HAL_Delay(20);
while(1==HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1));
HAL_Delay(20);
target += 1024;//获取目标位置
}
angle = (int16_t)(result->ecd);//获取当前角度
speed = (int16_t)(result->speed_rpm);//获取当前速度
angle_test = angle ;
angle_test = Handle_Angle8191_PID_Over_Zero(target,angle);//过零
angle_output = PID_calc(&pid_angle,angle_test,0); // 角度外环
PID_calc(&pid_speed,speed,angle_output); // 速度内环
CAN_cmd_chassis(pid_speed.out,0,0,0);//发送电机数
}
}
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
{
Error_Handler();
}
}
float Handle_Angle8191_PID_Over_Zero(float tar, float cur)//过零处理
{
if(tar - cur > 4096) //4096 ：半圈机械角�?
{
angle_test += 8191;
}
else if(tar - cur < -4096)
{
angle_test = angle_test - 8191;
}
else
{
}
return angle_test;
}
void Error_Handler(void)
{
__disable_irq();
while (1)
{
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif
main讲解
先定义pid速度环和角度环结构体变量，再分别给这两个环的PID三参赋值
再加入CAN屏蔽器初始化
PID清除函数
PID初始化函数，
初始化函数需要的参数为
创建电机结构体变量，用于接收电机数据
这个函数写的是每按下一次按键，编码增加1024；
PID_clac函数是PID计算函数，填入PID结构体指针，反馈数据（即读到的电机数据），设定值，即可输出PID计算结果；
将角度外环的输出结果作为速度内环的SET值，再将速度环的输出给电机函数，即可完成串环控制
过零处理部分
float Handle_Angle8191_PID_Over_Zero(float tar, float cur) // 过零处理
{
if(tar - cur > 4096) // 4096：半圈机械角度
{
angle_test += 8191; // 目标角度比当前角度大超过半圈，说明需要正向过零
}
else if(tar - cur < -4096)
{
angle_test = angle_test - 8191; // 目标角度比当前角度小超过半圈，说明需要反向过零
}
else
{
// 不过零的情况，保持原值
}
return angle_test;
}
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
5
0- 0
已为社区贡献4条内容
所有评论(0)