57&42步进电机_速度闭环_位置式PID STM32源程序

ID:301684 · 发表于 2019-9-25 09:50

黑石H7开发板步进电机驱动亲测可用

#include "stm32h7xx_hal.h"
#include "bsp_led.h"
#include "bsp_key.h"
#include "bsp_debug_usart.h"
#include "bsp_stepmotor.h"
#include "bsp_encoder.h"
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>
/* 私有类型定义 --------------------------------------------------------------*/
typedef struct
{
__IO float SetPoint; // 目标值单位:r/m
__IO int32_t LastError; // 前一次误差
__IO int32_t PrevError; // 前两次误差
__IO int32_t SumError; // 累计误差
float Proportion; // Kp系数
float Integral; // Ki系数
float Derivative; // Kd系数
}PID_Typedef;
/* 私有宏定义 ----------------------------------------------------------------*/
#define PID_DEFAULT_P 0.4f //
#define PID_DEFAULT_I 0.2f //
#define PID_DEFAULT_D 0.0f //
#define SPEED 5.3f // 移动速度 mm/s
#define MM_ROUND 5 // 丝杆导程 mm/r
#define INTERVAL 20 // 采样和PID计算时间间隔 ms
/* 私有变量 ------------------------------------------------------------------*/
__IO static PID_Typedef vPID; // 速度环PID结构体
__IO uint16_t time_count = 0; // 时间计数，每1ms增加一(与滴答定时器频率有关)
__IO uint8_t Time_Flag = 0; // 任务时间标记
bool Start_flag = false; // 启动/停止
/* 扩展变量 ------------------------------------------------------------------*/
/* 私有函数原形 --------------------------------------------------------------*/
static void SystemClock_Config( void );
static void CPU_CACHE_Enable( void );
/* 函数体 --------------------------------------------------------------------*/
/**
* 函数功能：位置式PID速度环计算
* 输入参数: @NextPoint 由编码器得到的计数值
* @TargetVal 目标值
* 返回值：经过PID运算得到的增量值
* 说明：位置式 PID 速度环控制设计,计算得到的结果仍然是速度值
*/
float IncPIDCalc(int NextPoint,float TargetVal) //临时变量,期望值
{
float iError = 0,iIncpid = 0; //当前误差
iError = TargetVal - NextPoint; // 增量计算
if((iError < 0.5f)&&(iError>-0.5f))
iError = 0; // |e| < 0.5,不做调整
vPID.PrevError = iError - vPID.LastError;
vPID.SumError += iError;
iIncpid=(vPID.Proportion * iError) // E[k]项
+(vPID.Integral * vPID.SumError) // E[k-1]项
+(vPID.Derivative * vPID.PrevError); // E[k-2]项
vPID.PrevError = vPID.LastError; // 存储误差，用于下次计算
vPID.LastError = iError;
return(iIncpid); // 返回增量值
}
/**
* 函数功能: PID结构体初始化
* 输入参数: 无
* 返回值: 无
* 说明: 初始化PID参数
*/
void Init_PIDStruct()
{
vPID.SetPoint = SPEED; // 目标值单位:mm/s
vPID.Proportion = PID_DEFAULT_P; // Kp系数
vPID.Integral = PID_DEFAULT_I; // Ki系数
vPID.Derivative = PID_DEFAULT_D; // Kd系数
vPID.LastError = 0;
vPID.PrevError = 0;
vPID.SumError = 0;
}
/**
* 函数功能: 主函数.
* 输入参数: 无
* 返回值: 无
* 说明: 无
*/
int main(void)
{
float Exp_Val = 0; // PID计算出来的期望值
float Vel_Target = 0;; // 速度目标值
int16_t MSF = 0; // 电机反馈速度
int32_t Enc_Cap = 0; // 编码器输入捕获数
int32_t lastEnc_Cap = 0; // 编码器上一次捕获值
CPU_CACHE_Enable();
/* 复位所有外设，初始化Flash接口和系统滴答定时器 */
HAL_Init();
/* 配置系统时钟 */
SystemClock_Config();
/* 板载LED,按键初始化 */
LED_GPIO_Init();
KEY_GPIO_Init();
/* 初始化PID参数结构体 */
Init_PIDStruct();
/* 调试串口初始化 */
MX_DEBUG_UART_Init();
/* 编码器定时器初始化并配置输入捕获功能 */
ENCODER_TIMx_Init();
/* 启动编码器接口 */
HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL);
HAL_Delay(10);
/* 步进电机定时器初始化*/
SMOTOR1_TIMx_Init();
/* 首先禁止步进电机动作*/
SMotor_Set_State(MOTOR_DISABLE);
/* 比较输出和中断 */
HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
/* 移动速度转换为PID的目标值
* 目标值是每个采样周期的编码器计数值,SetPoint是移动速度(mm/s),INTERVAL是采样周期间隔
* 1mm/s对应的编码器计数值是 ENCODER_RESOLUTION/MM_ROUND -> 2400/5 = 480 Hz
* 每个采样周期之间的编码器计数值就是 480/(1000/INTERVAL) = 9.6f
* SetPoint*9.6 就是 SetPoint mm/s 对应的采样周期间隔编码器计数值
*/
Vel_Target = round(fabs(vPID.SetPoint) * (ENCODER_RESOLUTION/MM_ROUND)/(1000/INTERVAL) );//每单位采样周期内的脉冲数(频率)
__HAL_DBGMCU_FREEZE_TIM8();// debug 的时候停止定时器时钟
__HAL_DBGMCU_FREEZE_TIM2();// debug 的时候停止定时器时钟
while (1)
{
/* KEY1启动电机,并且使能PID计算 */
if(KEY1_StateRead() == KEY_DOWN)
{
SMotor_Set_State(MOTOR_ENABLE);
Exp_Val = 0;
Init_PIDStruct();
if(vPID.SetPoint < 0 )
SMotor_Set_Dir( MOTOR_DIR_CCW );
else
SMotor_Set_Dir( MOTOR_DIR_CW );
SMotor_Set_Speed(0.01); // 低速启动
HAL_TIM_OC_Start_IT(&htimx_SMotor1,TIM_CHANNEL_1);
Start_flag = true; // 启动/停止
printf("启动PID计算\n");
printf("设置的目标值是 %f mm/s \n",vPID.SetPoint);//
printf("对应的编码器计数值是 %d pulse/s \n",(int32_t)Vel_Target*(1000/INTERVAL));//
}
/* KEY2停止电机,并且停止PID计算 */
if(KEY2_StateRead() == KEY_DOWN)
{
SMotor_Set_State(MOTOR_DISABLE);
HAL_TIM_OC_Stop_IT(&htimx_SMotor1,TIM_CHANNEL_1);
Start_flag = false; // 启动/停止
}
/* KEY3/KEY4用于调控电机位置 */
if(KEY3_StateRead() == KEY_DOWN)
{
Start_flag = false; // 启动/停止
SMotor_Set_State( MOTOR_ENABLE );
SMotor_Set_Dir( MOTOR_DIR_CW );
SMotor_Set_Speed( 160 ); //设置低速启动
HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
}
if(KEY4_StateRead() == KEY_DOWN)
{
Start_flag = false; // 启动/停止
SMotor_Set_State( MOTOR_ENABLE );
SMotor_Set_Dir( MOTOR_DIR_CCW );
SMotor_Set_Speed( 160 ); // 设置低速启动
HAL_TIM_OC_Start_IT(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
}
// 采样和控制周期为20ms
if(Time_Flag & 0x01)
{
//获得编码器的脉冲值
Enc_Cap = YS_Encoder_GetCounting();
//M法测速度
MSF = Enc_Cap - lastEnc_Cap;
lastEnc_Cap = Enc_Cap;
MSF = abs(MSF);
Exp_Val = IncPIDCalc(MSF, Vel_Target);
Exp_Val = fabs(Exp_Val);
SMotor_Set_Speed(Exp_Val);
Time_Flag &= ~0x01;
}
// 数据发送周期为1s, 显示转速
if(Time_Flag & 0x02)
{
int32_t pulse = YS_Encoder_GetCounting();
static int32_t tmp = 0;
float speed = (float)( pulse - tmp ) / ENCODER_RESOLUTION ;
printf("1s内的编码器计数值: %d\n", pulse - tmp);
printf("电机转速: %.2f r/s \n", speed );
printf("移动速度: %.1f mm/s \n", speed*5.0f);// 1r就是5mm
tmp = pulse ;
Time_Flag &= ~0x02;
}
}
}
/**
* 函数功能: 系统滴答定时器中断回调函数
* 输入参数: 无
* 返回值: 无
* 说明: 每发生一次滴答定时器中断进入该回调函数一次
*/
void HAL_SYSTICK_Callback(void)
{
// 每1ms自动增一
time_count++;
if( (time_count % INTERVAL) == 0)// 20ms读取一次编码器数值
{
if(Start_flag )
Time_Flag |= 0x01;
}
else if(time_count >= 1000) // 1s发送一次数据
{
Time_Flag |= 0x02;
time_count = 0;
}
}
/**
* 函数功能: 定时器比较输出中断回调函数
* 输入参数: htim：定时器句柄指针
* 返回值: 无
* 说明: 控制脉冲输出
*/
void HAL_TIM_OC_DelayElapsedCallback( TIM_HandleTypeDef * htim)
{
uint32_t Toggle = 0;
if(htim == &htimx_SMotor1)
{
/* 设置下一次中断的间隔 */
Toggle = __HAL_TIM_GET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx);
Toggle += SMotor1.PulseWidth; // 计算实际的比较值
__HAL_TIM_SET_COMPARE(&htimx_SMotor1, SMOTOR1_PUL_TIMx_CHANNELx, (uint16_t)Toggle);
}
}
/**
* 函数功能: 系统时钟配置
* 输入参数: 无
* 返回值: 无
* 说明: 无
*/
static void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct={0};
RCC_ClkInitTypeDef RCC_ClkInitStruct={0};
/* 使能PWR配置更新 */
MODIFY_REG(PWR->CR3, PWR_CR3_SCUEN, 0);
/* 设置调压器输出电压级别1 */
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/* 等待D3区域VOS 输出准备就绪 */
while ((PWR->D3CR & (PWR_D3CR_VOSRDY)) != PWR_D3CR_VOSRDY) // 等待内核电源就绪
{
}
/* RCC 内部/外部晶振配置 ,用于CPU，AHB，APH总线时钟 */
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;// 外部晶振
RCC_OscInitStruct.HSEState = RCC_HSE_ON;//选择外部时钟晶振 HSE 25MHz
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;//使能PLL
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;//PLL时钟源选择HSE
RCC_OscInitStruct.PLL.PLLM = 5; // 外部时钟2分频,得到5MHz 作为PLL时钟源
RCC_OscInitStruct.PLL.PLLN = 160;// PLL 160倍频,得到800MHz PLLCLK
RCC_OscInitStruct.PLL.PLLP = 2; // PLLCLK 2分频得到400MHz SYSCLK
RCC_OscInitStruct.PLL.PLLQ = 2; // PLLCLK 2分频得到400MHz用于部分外设的时钟
RCC_OscInitStruct.PLL.PLLR = 2; // PLLCLK 2分频得到400MHz用于部分外设的时钟
RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2;// PLL时钟输入范围4-8MHz
RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;// PLL时钟输出范围192-836MHz
RCC_OscInitStruct.PLL.PLLFRACN = 0; // 此参数用于微调 PLL1 VCO 范围0~2^13-1
HAL_RCC_OscConfig(&RCC_OscInitStruct);
/* RCC CPU，AHB，APH总线时钟配置*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
|RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;//SYSCLK 时钟源400MHz
RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;// SYSCLK 1分频 HCLK=400MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; // HCLK 2分频，AHB = 200Mhz
RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; // APB3 2分频，APB3 = 100MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; // APB1 2分频，APB1 = 100MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; // APB2 2分频，APB2 = 100MHz
RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; // APB4 2分频，APB4 = 100MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
/* 使能时钟安全机制 */
HAL_RCC_EnableCSS();
/* 滴答定时器配置1ms */
// SystemCoreClock/1000 1ms中断一次
// SystemCoreClock/100000 10us中断一次
// SystemCoreClock/1000000 1us中断一次
HAL_SYSTICK_Config(SystemCoreClock/(1000));
/* 系统滴答定时器时钟源 */
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* 系统滴答定时器中断优先级配置 */
HAL_NVIC_SetPriority(SysTick_IRQn, 1, 1);
}
/**
* 函数功能: 使能CPU L1-Cache
* 输入参数: 无
* 返回值: 无
* 说明: 无
*/
static void CPU_CACHE_Enable(void)
{
/* 使能 I-Cache */
SCB_EnableICache();
/* 使能 D-Cache */
SCB_EnableDCache();
}

复制代码

帐号		自动登录	找回密码
密码			立即注册