|
黑石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();
- }
- /********** (C) COPYRIGHT 2019-2030 硬石嵌入式开发团队 *******END OF FILE******/
复制代码
|
|