步进电机位置速度双闭环串级调控
代码说明:
/** ****************************************************************************** ****************************************************************************** */ /* 包含头文件 ----------------------------------------------------------------*/ #include "stm32f1xx_hal.h" #include "StepMotor/bsp_STEPMOTOR.h" #include "usart/bsp_debug_usart.h" #include "EncoderTIM/bsp_EncoderTIM.h" #include #include /* 私有类型定义 --------------------------------------------------------------*/ typedef struct { __IO float SetPoint; // 目标值 单位:mm __IO int LastError; // 前一次误差 __IO int PrevError; // 前两次误差 __IO long SumError; // 累计误差 __IO double Proportion; // Kp系数 __IO double Integral; // Ki系数 __IO double Derivative; // Kd系数 }PID; /* 私有宏定义 ----------------------------------------------------------------*/ #define TXDCYCLE 1000 // 数据发送周期;单位:ms #define SAMPLING 0x01 // 采样标记 #define TXD 0x02 // 发送数据标记 #define MAX_SPEED 200 #define abs(x) ((x)Proportion * iError) // E[k]项 -(ptr->Integral * ptr->LastError) // E[k-1]项 (ptr->Derivative * ptr->PrevError); // E[k-2]项 ptr->PrevError = ptr->LastError; // 存储误差,用于下次计算 ptr->LastError = iError; return(iIncpid); // 返回增量值 } /** * 函数功能:PID参数初始化 * 输入参数:无 * 返 回 值:无 * 说 明:无 */ void PID_Init() { sPID.Proportion = 0.01; //比例系数 sPID.Integral = 0; //积分系数 sPID.Derivative = 0; //微分系数 sPID.LastError = 0; //前一次的误差 sPID.PrevError = 0; //前两次的误差 sPID.SetPoint = 50; //目标值 sPID.SumError = 0; //累计误差 vPID.Proportion = 0.035; //比例系数 vPID.Integral = 0.005; //积分系数 vPID.Derivative = 0; //微分系数 vPID.LastError = 0; //前一次的误差 vPID.PrevError = 0; //前两次的误差 vPID.SetPoint = 7; //目标值 vPID.SumError = 0; //累计误差 } /** * 函数功能: 系统时钟配置 * 输入参数: 无 * 返 回 值: 无 * 说 明: 无 */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct; RCC_ClkInitTypeDef RCC_ClkInitStruct; RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // 外部晶振,8MHz RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; // 9倍频,得到72MHz主时钟 HAL_RCC_OscConfig(&RCC_OscInitStruct); RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // 系统时钟:72MHz RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // AHB时钟:72MHz RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; // APB1时钟:36MHz RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; // APB2时钟:72MHz HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2); // HAL_RCC_GetHCLKFreq()/1000 1ms中断一次 // HAL_RCC_GetHCLKFreq()/100000 10us中断一次 // HAL_RCC_GetHCLKFreq()/1000000 1us中断一次 HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); // 配置并启动系统滴答定时器 /* 系统滴答定时器时钟源 */ HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); /* 系统滴答定时器中断优先级配置 */ HAL_NVIC_SetPriority(SysTick_IRQn, 1, 0); } /** * 函数功能: 主函数. * 输入参数: 无 * 返 回 值: 无 * 说 明: 无 */ int main(void) { static float Dis_Exp_Val = 0; // PID计算出来的期望值 static float Vel_Exp_Val = 0; // PID计算出来的期望值 float Dis_Target = 0; // 目标位置所对应编码器脉冲值 float Vel_Target = 0; // 每单位采样周期内的脉冲数(频率) uint16_t SUM_Pulse = 0; // 1秒内的总脉冲 int16_t MSF = 0; // 电机反馈速度 __IO int32_t CaptureNumber=0; // 输入捕获数 __IO int32_t Last_CaptureNumber=0;// 上一次捕获值 uint8_t aTxBuffer[SENDBUFF_SIZE]; // 串口DMA发送缓冲区 uint8_t Motion_Dir = 0; // 电机运动方向 /* 复位所有外设,初始化Flash接口和系统滴答定时器 */ HAL_Init(); /* 配置系统时钟 */ SystemClock_Config(); /* 调试串口初始化 */ MX_DEBUG_USART_Init(); /* 编码器定时器初始化并配置输入捕获功能 */ ENCODER_TIMx_Init(); /* 启动编码器接口 */ HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL); /* PID参数初始化*/ PID_Init(); Dis_Target = (sPID.SetPoint*PPM);//目标位置所对应编码器脉冲值 Vel_Target = (vPID.SetPoint*P_PERIOD);//每单位采样周期内的脉冲数(频率) /* 步进电机定时器初始化*/ STEPMOTOR_TIMx_Init(); /* 首先禁止步进电机动作*/ STEPMOTOR_OUTPUT_DISABLE(); /* 启动定时器 */ HAL_TIM_Base_Start(&htimx_STEPMOTOR); /* 启动比较输出并使能中断 */ HAL_TIM_OC_Start_IT(&htimx_STEPMOTOR,TIM_CHANNEL_1); /* 无限循环 */ while (1) { //采样和控制周期为20ms if(Time_Flag & SAMPLING) { //获得编码器的脉冲值 CaptureNumber = OverflowCount*65535 __HAL_TIM_GET_COUNTER(&htimx_Encoder); //M法 测速度 MSF = CaptureNumber - Last_CaptureNumber; Last_CaptureNumber = CaptureNumber; MSF = abs(MSF); //对速度进行累计,得到1s内的脉冲数 SUM_Pulse = MSF; //位置环PID计算,根据计算结果判断电机运动方向 Dis_Exp_Val = IncPIDCalc(CaptureNumber,Dis_Target,&sPID); Motion_Dir= Dis_Exp_Val= Vel_Target) Dis_Exp_Val = Vel_Target; Vel_Exp_Val = IncPIDCalc(MSF,Dis_Exp_Val,&vPID); //当到达目标位置的时候,这时候已经电机非常慢了.为了减少超调,可以直接将速度环的输出清零 if(Vel_Exp_Val = TXDCYCLE) // 1s { Time_Flag |= TXD; time_count = 0; } }
下载说明:请别用迅雷下载,失败请重下,重下不扣分!