步进电机位置环控制

/* 包含头文件 ----------------------------------------------------------------*/
#include "stm32f1xx_hal.h"
#include "StepMotor/bsp_STEPMOTOR.h" 
#include "usart/bsp_debug_usart.h"
#include "EncoderTIM/bsp_EncoderTIM.h"
#include  <stdlib.h>
#include  <string.h>
/* 私有类型定义 --------------------------------------------------------------*/
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)<0?(-x):(x))
#define SENDBUFF_SIZE               100     // 串口DMA发送缓冲区大小

/* 私有变量 ------------------------------------------------------------------*/
__IO static PID sPID; 

__IO uint16_t time_count = 0;              // 时间计数，每1ms增加一(与滴答定时器频率有关)
__IO uint8_t Time_Flag   = 0;              // 任务时间标记
/* 扩展变量 ------------------------------------------------------------------*/
extern int16_t OverflowCount;              //编码器计数溢出 计数器
/* 私有函数原形 --------------------------------------------------------------*/

/* 函数体 --------------------------------------------------------------------*/
/**
  * 函数功能：增量式PID速度环计算
  * 输入参数：NextPoint     由编码器得到的速度值 
  *           TargetVal    目标值
  * 返 回 值：经过PID运算得到的增量值
  * 说    明：增量式 PID 速度环控制设计,计算得到的结果仍然是速度值
  */
float IncPIDCalc(int NextPoint,float TargetVal)       //临时变量,期望值 
{
  float iError = 0,iIncpid = 0;                       //当前误差
  iError = TargetVal - NextPoint;                     // 增量计算
  if((iError<0.5)&&(iError>-0.5))
    iError = 0;                                       // |e| < 0.5,不做调整
  iIncpid=(sPID.Proportion * iError)                  // E[k]项
              -(sPID.Integral * sPID.LastError)       // E[k-1]项
              (sPID.Derivative * sPID.PrevError);    // E[k-2]项
  
  sPID.PrevError= sPID.LastError;                      // 存储误差，用于下次计算
  sPID.LastError = iError;
  return(iIncpid);                                    // 返回增量值
}

/**
  * 函数功能: 系统时钟配置
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 无
  */
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);
}
/**
  * 函数功能: PID结构体初始化
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 初始化PID参数
  */
void Init_PID()
{
  sPID.SetPoint   = 100;       // 目标值  单位:mm/s
  sPID.Proportion = 0.06;      // Kp系数
  sPID.Derivative = 0;         // Ki系数
  sPID.Integral   = 0;         // Kd系数
  sPID.LastError  = 0;
  sPID.PrevError  = 0;
  sPID.SumError   = 0; 
}
/**
  * 函数功能: 主函数.
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 无
  */
int main(void)
{ 
  static float Exp_Val = 0;        // PID计算出来的期望值
  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();

  Init_PID();
  Vel_Target = (sPID.SetPoint*PPM);
  /* 调试串口初始化 */
  MX_DEBUG_USART_Init(); 

  /* 编码器定时器初始化并配置输入捕获功能 */
  ENCODER_TIMx_Init();
  /* 启动编码器接口 */
  HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL);

  HAL_Delay(10);
  /* 步进电机定时器初始化*/
  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;
      Exp_Val = IncPIDCalc(CaptureNumber,Vel_Target);
      Motion_Dir= Exp_Val<0?CCW:CW;//
      Exp_Val = abs(Exp_Val);
      //由于位置环的PID运算一开始是以最大加速度输出,这里对速度做出限制,防止步进电机堵转
      if(Exp_Val >= MAX_SPEED)
        Exp_Val = MAX_SPEED;
      /* 经过PID计算得到的结果是编码器的输出期望值的增量,
         需要转换为步进电机的控制量(频率值),这里乘上一个系数6400/2400
      */
      STEPMOTOR_Motion_Ctrl(Motion_Dir,Exp_Val*FEEDBACK_CONST);//乘上一个系数,6400/2400,将PID计算结果转换为步进电机的频率(速度)
      Time_Flag &= ~SAMPLING; 
    }
    //数据发送周期为1s
    if(Time_Flag & TXD)
    {
      /* 速度值计算: v= 1s内的总步数*编码器单步步进距离 */
      /* 当前位置 = 编码器捕获值*编码器单步步进距离 */
      sprintf(aTxBuffer,"捕获值:%d 当前位置:%.2fmm 速度:%.2f mm/s\n",CaptureNumber,(float)(CaptureNumber*MPP),(float)SUM_Pulse*MPP);
      sprintf(aTxBuffer strlen((const char*)aTxBuffer),"1s内编码器计数值:%d\n",SUM_Pulse);
      HAL_UART_Transmit_DMA(&husart_debug, aTxBuffer, strlen((const char*)aTxBuffer));
      SUM_Pulse = 0;
      Time_Flag &= ~TXD;
    }
  }
}

/**
  * 函数功能: 系统滴答定时器中断回调函数
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 每发生一次滴答定时器中断进入该回调函数一次
  */
void HAL_SYSTICK_Callback(void)
{
  // 每1ms自动增一
  time_count ;         
  if(time_count%(SAMPLING_PERIOD) == 0) // 20ms
  {
    Time_Flag |= SAMPLING;
  }
  if(time_count >= TXDCYCLE)            // 1s
  {
    Time_Flag |= TXD;
    time_count = 0;
  }
}