基础配置见上篇串口中断配置
-
配置pwm
-
配置编码器
-
配置定时器
-
初始化配置
-
添加pid文件
-
修改中断函数
我要控制双电机位置速度串级闭环
| pwm | encoder | 使能引脚 | |
| 电机a | tim1（PE9 PE12） | tim3 | PE10 |
| 电机b | tim8（PC6 PC7） | tim4 | PC8 |
第一步
没截到图的地方都是默认
tim8和tim1一样
第二步
tim4和tim3同理
第三步
配置你自己想要的周期的定时器中断，我弄成了100hz，也就是10ms进一次定时器中断
配置结束，生成代码，新工程编译有问题的话，上篇有解决办法
第四步
main循环之前添加这些
/*打开PWM输出*/
HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
/*打开定时器中断*/
HAL_TIM_Base_Start_IT(&htim6);
/*打开编码器捕获*/
HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
PID_param_init();
set_motor1_enable();
set_motor2_enable();
is_motor_en = 1;
motor1_go(-30);
motor2_go(30);第五步
添加pid的c和h文件还有contral的c和h文件
pid的c文件
#include "bsp_pid.h"
//定义全局变量
_pid pid_location1;
_pid pid_speed1;
_pid pid_location2;
_pid pid_speed2;
/**
* @brief PID参数初始化
* @note 无
* @retval 无
*/
void PID_param_init(void)
{
/* 111位置相关初始化参数 */
pid_location1.target_val=0;
pid_location1.actual_val=0.0;
pid_location1.err=0.0;
pid_location1.err_last=0.0;
pid_location1.integral=0.0;
pid_location1.Kp = 2;
pid_location1.Ki = 0.0;
pid_location1.Kd = 0.3;
/* 111速度相关初始化参数 */
pid_speed1.target_val=0.0;
pid_speed1.actual_val=0.0;
pid_speed1.err=0.0;
pid_speed1.err_last=0.0;
pid_speed1.integral=0.0;
pid_speed1.Kp = 3;
pid_speed1.Ki = 0.4;
pid_speed1.Kd = 0.01;
/* 222位置相关初始化参数 */
pid_location2.target_val=0;
pid_location2.actual_val=0.0;
pid_location2.err=0.0;
pid_location2.err_last=0.0;
pid_location2.integral=0.0;
pid_location2.Kp = 2;
pid_location2.Ki = 0.0;
pid_location2.Kd = 0.3;
/* 222速度相关初始化参数 */
pid_speed2.target_val=0.0;
pid_speed2.actual_val=0.0;
pid_speed2.err=0.0;
pid_speed2.err_last=0.0;
pid_speed2.integral=0.0;
pid_speed2.Kp = 3;
pid_speed2.Ki = 0.4;
pid_speed2.Kd = 0.01;
}
/**
* @brief 设置目标值
* @param val 目标值
* @note 无
* @retval 无
*/
void set_pid_target(_pid *pid, float temp_val)
{
pid->target_val = temp_val; // 设置当前的目标值
}
/**
* @brief 获取目标值
* @param 无
* @note 无
* @retval 目标值
*/
float get_pid_target(_pid *pid)
{
return pid->target_val; // 当前的目标值
}
/**
* @brief 设置比例、积分、微分系数
* @param p：比例系数 P
* @param i：积分系数 i
* @param d：微分系数 d
* @note 无
* @retval 无
*/
void set_p_i_d(_pid *pid, float p, float i, float d)
{
pid->Kp = p; // 设置比例系数 P
pid->Ki = i; // 设置积分系数 I
pid->Kd = d; // 设置微分系数 D
}
/**
* @brief 位置PID算法实现
* @param actual_val:实际值
* @note 无
* @retval 通过PID计算后的输出
*/
float location_pid_realize_Motor1(_pid *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->err=pid->target_val-actual_val;
/* 设定闭环死区 */
if((pid->err >= -20) && (pid->err <= 20))
{
pid->err = 0;
pid->integral = 0;
}
pid->integral += pid->err; // 误差累积
/*PID算法实现*/
pid->actual_val = pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
/*误差传递*/
pid->err_last=pid->err;
/*返回当前实际值*/
return pid->actual_val;
}
float location_pid_realize_Motor2(_pid *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->err=pid->target_val-actual_val;
/* 设定闭环死区 */
if((pid->err >= -20) && (pid->err <= 20))
{
pid->err = 0;
pid->integral = 0;
}
pid->integral += pid->err; // 误差累积
/*PID算法实现*/
pid->actual_val = pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
/*误差传递*/
pid->err_last=pid->err;
/*返回当前实际值*/
return pid->actual_val;
}
/**
* @brief 速度PID算法实现
* @param actual_val:实际值
* @note 无
* @retval 通过PID计算后的输出
*/
float speed_pid_realize(_pid *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->err=pid->target_val-actual_val;
// if((pid->err<0.2f )&& (pid->err>-0.2f))
// pid->err = 0.0f;
pid->integral += pid->err; // 误差累积
/*PID算法实现*/
pid->actual_val = pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
/*误差传递*/
pid->err_last=pid->err;
/*返回当前实际值*/
return pid->actual_val;
}
/************************************ END OF FIER *******************************************************/
pid的h文件
#ifndef __BSP_PID_H
#define __BSP_PID_H
#include "stm32f4xx.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define PID_ASSISTANT_EN
typedef struct
{
float target_val; //目标值
float actual_val; //实际值
float err; //定义偏差值
float err_last; //定义上一个偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
float integral; //定义积分值
}_pid;
extern _pid pid_location1;
extern _pid pid_speed1;
extern _pid pid_location2;
extern _pid pid_speed2;
extern void PID_param_init(void);
extern void set_pid_target(_pid *pid, float temp_val);
extern float get_pid_target(_pid *pid);
extern void set_p_i_d(_pid *pid, float p, float i, float d);
extern float location_pid_realize_Motor1(_pid *pid, float actual_val);
extern float location_pid_realize_Motor2(_pid *pid, float actual_val);
extern float speed_pid_realize(_pid *pid, float actual_val);
#endif
contral的c文件
#include "tim.h"
#include "gpio.h"
#include "bsp_pid.h"
#include "contral.h"
#include <stdio.h>
#include <math.h>
#include "stdlib.h"
int mode1_flag=0,mode4_flag=1,mode4=0,num_jishu=1;
unsigned char location_control_count_Motor1 = 0; //执行频率不需要那么高的用这个事件计数，用在中断中
unsigned char location_control_count_Motor2 = 0;
float Motor1PWM =0.0 , Motor2PWM =0.0;
float speed_Outval_Motor1, speed_Outval_Motor2, location_Outval_Motor1, location_Outval_Motor2;
static motor_dir_t direction = MOTOR_FWD; // 记录电机方向
static uint16_t dutyfactor = 0; // 记录电机占空比
u8 is_motor_en = 0; // 电机使能
long g_lMotor1PulseSigma =0;
long g_lMotor2PulseSigma =0;
short g_nMotor1Pulse=0;//全局变量， 保存电机脉冲数值
short g_nMotor2Pulse=0;//全局变量， 保存电机脉冲数值
/*******************实际运行时读取编码器数值************************/
void GetMotor1Pulse(void)//读取电机脉冲
{
g_nMotor1Pulse = (short)(__HAL_TIM_GET_COUNTER(&htim3));//获取计数器值
__HAL_TIM_SET_COUNTER(&htim3,0);//TIM3计数器清零
g_lMotor1PulseSigma += g_nMotor1Pulse;//位置外环使用的脉冲累积
}
void GetMotor2Pulse(void)//读取电机脉冲
{
g_nMotor2Pulse = (short)(__HAL_TIM_GET_COUNTER(&htim4));//获取计数器值
__HAL_TIM_SET_COUNTER(&htim4,0);//TIM4计数器清零
g_lMotor2PulseSigma += g_nMotor2Pulse;//位置外环使用的脉冲累积
}
/*这里加将位置环的输入直接编程距离（cm）的 函数*/ //所有普通直行都用这个
void motor1_go(int32_t location_cm) //直走函数 //连续两次的直行好像会让巡线补偿失效
{
float motor_location;
motor_location = ( location_cm / (6.5 * 3.14) ) * (45*4*13) ; //motor_location 将 location_cm 转换为对应的脉冲数 g_fTargetJourney = 要转多少圈 * 1圈的脉冲数
set_pid_target(&pid_location1, motor_location);
set_motor1_enable(); //使能电机控制PWM输出
// g_lMotorPulseSigma = 0; //之前没清除所以用不了
}
void motor2_go(int32_t location_cm) //直走函数 //连续两次的直行好像会让巡线补偿失效
{
float motor_location;
motor_location = ( location_cm / (6.5 * 3.14) ) * (45*4*13) ; //motor_location 将 location_cm 转换为对应的脉冲数 g_fTargetJourney = 要转多少圈 * 1圈的脉冲数
set_pid_target(&pid_location2, motor_location);
set_motor2_enable(); //使能电机控制PWM输出
// g_lMotorPulseSigma = 0; //之前没清除所以用不了
}
/****速度环位置环串级PID控制*****/
void Location_Speed_control_Motor1(void)
{
if (is_motor_en == 1) // 电机在使能状态下才进行控制处理
{
location_control_count_Motor1++;
if(location_control_count_Motor1 >= 2)
{
location_control_count_Motor1 = 0;
location_Outval_Motor1 = location_pid_control_Motor1();
}
set_pid_target(&pid_speed1, location_Outval_Motor1); //每次都必须有位置环的值
speed_Outval_Motor1 = speed_pid_control_Motor1(); //要是电机转向不符合预期，就在这两句里取反数值
}
}
void Location_Speed_control_Motor2(void)
{
if (is_motor_en == 1) // 电机在使能状态下才进行控制处理
{
location_control_count_Motor2++;
if(location_control_count_Motor2 >= 2)
{
location_control_count_Motor2 = 0;
location_Outval_Motor2 = location_pid_control_Motor2();
}
set_pid_target(&pid_speed2, location_Outval_Motor2); //每次都必须有位置环的值
speed_Outval_Motor2 = speed_pid_control_Motor2(); //要是电机转向不符合预期，就在这两句里取反数值
}
}
float location_pid_control_Motor1(void)
{
float cont_val = 0.0;
int32_t actual_location;
actual_location = g_lMotor1PulseSigma; //1圈 = 2340个脉冲 = 45*13*4 //这里位置用圈数代替。
cont_val = location_pid_realize_Motor1(&pid_location1, actual_location);
/* 目标速度上限处理 */
if (cont_val > TARGET_SPEED_MAX)
{
cont_val = TARGET_SPEED_MAX;
}
else if (cont_val < -TARGET_SPEED_MAX)
{
cont_val = -TARGET_SPEED_MAX;
}
return cont_val;
}
float location_pid_control_Motor2(void)
{
float cont_val = 0.0;
int32_t actual_location;
actual_location = g_lMotor2PulseSigma; //1圈 = 2340个脉冲 = 45*13*4 //这里位置用圈数代替。
cont_val = location_pid_realize_Motor2(&pid_location2, actual_location);
/* 目标速度上限处理 */
if (cont_val > TARGET_SPEED_MAX)
{
cont_val = TARGET_SPEED_MAX;
}
else if (cont_val < -TARGET_SPEED_MAX)
{
cont_val = -TARGET_SPEED_MAX;
}
return cont_val;
}
float speed_pid_control_Motor1(void)
{
float cont_val = 0.0; // 当前控制值
int32_t actual_speed;
actual_speed = ((float)g_nMotor1Pulse*1000.0*60.0)/(4 * 13 * 45 * 20);
cont_val = speed_pid_realize(&pid_speed1, actual_speed); // 进行 PID 计算
return cont_val;
}
float speed_pid_control_Motor2(void)
{
float cont_val = 0.0; // 当前控制值
int32_t actual_speed;
actual_speed = ((float)g_nMotor2Pulse*1000.0*60.0)/(4 * 13 * 45 * 20);
cont_val = speed_pid_realize(&pid_speed2, actual_speed); // 进行 PID 计算
return cont_val;
}
/*****************电机的控制函数***************/
void Motor1Output(int nMotorPwm)//设置电机电压和方向
{
if (nMotorPwm >= 0) // 判断电机方向
{
set_motor1_direction(MOTOR_FWD); //正方向要对应
}
else
{
nMotorPwm = -nMotorPwm;
set_motor1_direction(MOTOR_REV); //正方向要对应
}
nMotorPwm = (nMotorPwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotorPwm; // 速度上限处理
set_motor1_speed(nMotorPwm); // 设置 PWM 占空比
}
void Motor2Output(int nMotorPwm)//设置电机电压和方向
{
if (nMotorPwm >= 0) // 判断电机方向
{
set_motor2_direction(MOTOR_FWD); //正方向要对应
}
else
{
nMotorPwm = -nMotorPwm;
set_motor2_direction(MOTOR_REV); //正方向要对应
}
nMotorPwm = (nMotorPwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotorPwm; // 速度上限处理
set_motor2_speed(nMotorPwm); // 设置 PWM 占空比
}
/**
* @brief 设置电机速度
* @param v: 速度（占空比）
* @retval 无
*/
void set_motor1_speed(uint16_t v)
{
dutyfactor = v;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor1(dutyfactor); // 设置速度
SET_REV_COMPAER_Motor1(0);
}
else
{
SET_FWD_COMPAER_Motor1(0);
SET_REV_COMPAER_Motor1(dutyfactor); // 设置速度
}
}
void set_motor2_speed(uint16_t v)
{
dutyfactor = v;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor2(dutyfactor); // 设置速度
SET_REV_COMPAER_Motor2(0);
}
else
{
SET_FWD_COMPAER_Motor2(0);
SET_REV_COMPAER_Motor2(dutyfactor); // 设置速度
}
}
/**
* @brief 设置电机方向
* @param 无
* @retval 无
*/
void set_motor1_direction(motor_dir_t dir)
{
direction = dir;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor1(dutyfactor); // 设置速度
SET_REV_COMPAER_Motor1(0); // 设置速度
}
else
{
SET_FWD_COMPAER_Motor1(0); // 设置速度
SET_REV_COMPAER_Motor1(dutyfactor); // 设置速度
}
}
void set_motor2_direction(motor_dir_t dir)
{
direction = dir;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor2(dutyfactor); // 设置速度
SET_REV_COMPAER_Motor2(0); // 设置速度
}
else
{
SET_FWD_COMPAER_Motor2(0); // 设置速度
SET_REV_COMPAER_Motor2(dutyfactor); // 设置速度
}
}
/**
* @brief 使能电机
* @param 无
* @retval 无
*/
void set_motor1_enable(void) //这俩个使能和禁用的函数对于双极性控制来说还有效吗？
{
is_motor_en = 1;
MOTOR1_ENABLE_SD();
MOTOR1_FWD_ENABLE();
MOTOR1_REV_ENABLE();
}
void set_motor2_enable(void) //这俩个使能和禁用的函数对于双极性控制来说还有效吗？
{
is_motor_en = 1;
MOTOR2_ENABLE_SD();
MOTOR2_FWD_ENABLE();
MOTOR2_REV_ENABLE();
}
/**
* @brief 禁用电机
* @param 无
* @retval 无
*/
void set_motor1_disable(void)
{
is_motor_en = 0;
MOTOR1_DISABLE_SD();
MOTOR1_FWD_DISABLE();
MOTOR1_REV_DISABLE();
}
void set_motor2_disable(void)
{
is_motor_en = 0;
MOTOR2_DISABLE_SD();
MOTOR2_FWD_DISABLE();
MOTOR2_REV_DISABLE();
}
contral的h文件
#ifndef __CONTROL_H
#define __CONTROL_H
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include "stm32f4xx.h"
#include "tim.h"
#include "bsp_pid.h"
extern _pid pid_location1, pid_location2;
extern float speed_Outval_Motor1, speed_Outval_Motor2, location_Outval_Motor1, location_Outval_Motor2;
extern u8 is_motor_en;
extern float Motor1PWM, Motor2PWM;
/* 电机 SD or EN 使能脚 */
#define MOTOR1_ENABLE_SD() HAL_GPIO_WritePin(MOTOR1EN_GPIO_Port, MOTOR1EN_Pin, GPIO_PIN_SET) // 高电平打开-高电平使能
#define MOTOR1_DISABLE_SD() HAL_GPIO_WritePin(MOTOR1EN_GPIO_Port, MOTOR1EN_Pin, GPIO_PIN_RESET) // 低电平关断-低电平禁用
#define MOTOR2_ENABLE_SD() HAL_GPIO_WritePin(MOTOR2EN_GPIO_Port, MOTOR2EN_Pin, GPIO_PIN_SET) // 高电平打开-高电平使能
#define MOTOR2_DISABLE_SD() HAL_GPIO_WritePin(MOTOR2EN_GPIO_Port, MOTOR2EN_Pin, GPIO_PIN_RESET) // 低电平关断-低电平禁用
/* 累计 TIM_Period个后产生一个更新或者中断*/
/* 当定时器从0计数到PWM_PERIOD_COUNT，即为PWM_PERIOD_COUNT+1次，为一个定时周期 */
#define PWM_PERIOD_COUNT (4200) //可以尝试把这个变大，这样PID控制可以更顺滑
#define PWM2_PERIOD_COUNT (4200)
/* 最大比较值 */
#define PWM_MAX_PERIOD_COUNT (PWM_PERIOD_COUNT - 100) //如果PWM弄成了满的，一些驱动板就会出现问题（硬件上的原因）
#define PWM2_MAX_PERIOD_COUNT (PWM2_PERIOD_COUNT - 100)
/****************电机引脚初始化**************/
/* 设置速度（占空比） */
#define SET_FWD_COMPAER_Motor1(ChannelPulse) __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,ChannelPulse) // 设置比较寄存器的值 //AIN1
#define SET_REV_COMPAER_Motor1(ChannelPulse) __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,ChannelPulse) // 设置比较寄存器的值 //AIN2
#define SET_FWD_COMPAER_Motor2(ChannelPulse) __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,ChannelPulse) // 设置比较寄存器的值 //AIN1
#define SET_REV_COMPAER_Motor2(ChannelPulse) __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,ChannelPulse) // 设置比较寄存器的值 //AIN2
/* 使能输出 */
#define MOTOR1_FWD_ENABLE() HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
#define MOTOR1_REV_ENABLE() HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
#define MOTOR2_FWD_ENABLE() HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
#define MOTOR2_REV_ENABLE() HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
/* 禁用输出 */
#define MOTOR1_FWD_DISABLE() HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_1);
#define MOTOR1_REV_DISABLE() HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_2);
#define MOTOR2_FWD_DISABLE() HAL_TIM_PWM_Stop(&htim8,TIM_CHANNEL_1);
#define MOTOR2_REV_DISABLE() HAL_TIM_PWM_Stop(&htim8,TIM_CHANNEL_2);
#define TARGET_SPEED_MAX 100 60rpm可以3s走完60cm
/* 电机方向控制枚举 */
typedef enum
{
MOTOR_FWD = 0,
MOTOR_REV,
}motor_dir_t;
void Location_Speed_control_Motor1(void);
void Location_Speed_control_Motor2(void);
float location_pid_control_Motor1(void);
float location_pid_control_Motor2(void);
float speed_pid_control_Motor1(void);
float speed_pid_control_Motor2(void);
void Motor1Output(int nMotorPwm);
void Motor2Output(int nMotorPwm);
void set_motor1_speed(uint16_t v);
void set_motor2_speed(uint16_t v);
void set_motor1_direction(motor_dir_t dir);
void set_motor2_direction(motor_dir_t dir);
void set_motor1_enable(void);
void set_motor1_disable(void);
void set_motor2_enable(void);
void set_motor2_disable(void);
void motor1_go(int32_t location_cm);
void motor2_go(int32_t location_cm);
void GetMotor1Pulse(void);
void GetMotor2Pulse(void);
#endif
我都比较粗暴的double了，不过实测好用就行了
第六步
中断文件里修改如下
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim==(&htim3))
{
}
else if(htim==(&htim4))
{
}
else if(htim==(&htim6))
{
GetMotor1Pulse();
GetMotor2Pulse();
if(is_motor_en == 1 ){
Location_Speed_control_Motor1();
Location_Speed_control_Motor2();
Motor1PWM = speed_Outval_Motor1;
Motor2PWM = speed_Outval_Motor2;
Motor1Output(Motor1PWM);
Motor2Output(Motor2PWM);
}
}
}然后就可以在初始化里通过这个函数让电机闭环了
motor1_go(-30);
motor2_go(30);
工程里注释不少，我都上传了，各位需要的可以自行下载，某宝搜一下相关的服务很便宜的
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
