Stepper_Motor.c

/******************************************************************************
文 件 名：Stepper_Motor.c
功能描述：步进电机驱动库文件
作    者：张暄
版    本：V1.0
日    期：2017.04.18
******************************************************************************/

#include "Stepper_Motor.h"
volatile uint16_t   StepperMotor0CurStep;
volatile uint16_t   StepperMotor0DstStep;

volatile uint16_t   StepperMotor1CurStep;
volatile uint16_t   StepperMotor1DstStep;

volatile uint16_t   StepperMotor2CurStep;
volatile uint16_t   StepperMotor2DstStep;

volatile uint16_t   StepperMotor3CurStep;
volatile uint16_t   StepperMotor3DstStep;

MotorGlobalControlStruct    StepperMotorGlobalCtrl;

#if STEPPER_MOTOR_0_ENABLE
MotorCtrlStruct             StepperMotor0;
#endif

#if STEPPER_MOTOR_1_ENABLE
MotorCtrlStruct             StepperMotor1;
#endif

#if STEPPER_MOTOR_2_ENABLE
MotorCtrlStruct             StepperMotor2;
#endif

#if STEPPER_MOTOR_3_ENABLE
MotorCtrlStruct             StepperMotor3;
#endif

/******************************************************************************
函数名：Stepper_Motor_Start_Up
功  能：步进电机启动请求
        步进电机启动后,会自动进行小角度归零,而后转入正常工作状态
参  数：无
返回值：无
******************************************************************************/
void Stepper_Motor_Start_Up(void)
{
  //步进电机处于停止状态或断电归零状态,直接请求上电
  if ((StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_STOP) || \
      (StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_BACK))
    StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_PWR_UP_REQ;
  
  //步进电机处于上电归零状态,如果请求过断电,则清除断电请求
  else if (StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_RST) 
  {
    if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_SHDN_REQ)
      StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
  }    
}

/******************************************************************************
函数名：Stepper_Motor_Shutdown
功  能：步进电机关闭请求
        步进电机启动后,会自动进行断电归零,断电归零后,电机进入STEPPER_MOTOR_STOP
        状态。
        在请求电机关闭后,必须在查询到电机进入STEPPER_MOTOR_STOP状态后,才可以令系
        统休眠。
参  数：无
返回值：无
******************************************************************************/
void Stepper_Motor_Shutdown(void)
{
  //步进电机处于上电归零状态或准备就绪状态,直接请求断电
  if ((StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_READY) || \
      (StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_RST))
    StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_SHDN_REQ;
  
  //步进电机处于断电归零状态,如果请求过上电,则清除上电请求
  else if (StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_BACK) 
  {
    if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_PWR_UP_REQ)
      StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
  }    
}

/******************************************************************************
函数名：Stepper_Motor_Get_Status
功  能：获取步进电机当前状态
参  数：无
返回值：STEPPER_MOTOR_STOP  - 步进电机已停止
        STEPPER_MOTOR_RST   - 步进电机小角度归零中
        STEPPER_MOTOR_READY - 步进电机准备就绪
        STEPPER_MOTOR_BACK  - 步进电机断电归零中
******************************************************************************/
uint8_t Stepper_Motor_Get_Status(void)
{
  return StepperMotorGlobalCtrl.Status;
}

/******************************************************************************
函数名：Stepper_Motor_Calibration_Mode
功  能：步进电机校准模式使能
        只有在电机准备就绪后才可以进行校准
        当步进电机进入校准模式,走动速度将会提高,用于快速完成仪表调校
参  数：En：校准模式使能 1 - 使能 0 - 禁止
返回值：0 - 请求成功 1- 请求失败
******************************************************************************/
uint8_t Stepper_Motor_Calibration_Mode(uint8_t En)
{
  if (En)
  {
    if (StepperMotorGlobalCtrl.Status == STEPPER_MOTOR_READY)
    {
      StepperMotorGlobalCtrl.CalMode = 1;
      return 0;
    }
    else
    {
      StepperMotorGlobalCtrl.CalMode = 0;
      return 1;
    }
  }

  StepperMotorGlobalCtrl.CalMode = 0;
  return 0;
}

/******************************************************************************
函数名：Stepper_Motor_Speed_Control
功  能：步进电机走动速度控制
参  数：pMotor  - 被控电机的控制结构体
        DstStep - 被控电机的走动目标微步数
返回值：被控电机的当前微步数
******************************************************************************/
static uint16_t Stepper_Motor_Speed_Control(MotorCtrlStruct *pMotor, uint16_t DstStep)
{
  uint8_t   Mode;
  uint16_t  CurStep;
  uint16_t  SpeedLimit;
  uint16_t  Interval;
  
  if (pMotor -> Accomp == 0)                //等待上一微步走完
    return pMotor -> CurStepBackup;
  
  //上一微步已走完,才会进行下一微步的速度调整
  //Step 1: 获取当前微步数,计算速度上限
  DisableInterrupts;
  CurStep = pMotor -> CurStep;
  EnableInterrupts;
  pMotor -> CurStepBackup = CurStep;
  
  if (DstStep >= CurStep)
    SpeedLimit = DstStep - CurStep;
  else
    SpeedLimit = CurStep - DstStep;
  
  SpeedLimit = (SpeedLimit << 3) / STEPPER_MOTOR_DAMPING_FACTOR;
  
  if (SpeedLimit < STEPPER_MOTOR_SPEED_MIN)
    SpeedLimit = STEPPER_MOTOR_SPEED_MIN;
  
  //Step 2: 判断运动模式
  Mode = STEPPER_MOTOR_MODE_NORMAL;         //预置为正常行走模式
  if (DstStep != pMotor -> DstStep)         //目标微步数发生改变时才进行运动模式判断
  {
    if (CurStep <= pMotor -> DstStep)                           //电机正在正向旋转
    {
      if (DstStep < CurStep)                                    //请求向相反方向旋转
      {
        if (pMotor -> Speed > STEPPER_MOTOR_DIR_SWITCH_SPEED)   //当前电机速度高于换向速度
        {
          Mode    = STEPPER_MOTOR_MODE_BRAKE;                   //制动模式
          DstStep = 36 * STEPPER_MOTOR_STEP_PER_10DEG;          //维持当前旋转方向(相当于 360度 x 每度对应的微步数)                                      
        }
      }
      else if (DstStep < pMotor -> DstStep)                     //请求旋转步数减少
      {
        if (pMotor -> Speed > SpeedLimit)                       //当前电机转速过快
        {
          Mode    = STEPPER_MOTOR_MODE_BRAKE;                   //制动模式
          DstStep = 36 * STEPPER_MOTOR_STEP_PER_10DEG;          //维持当前旋转方向(相当于 360度 x 每度对应的微步数)       
        }
      }
    }
    else                                                        //电机正在逆向旋转
    {
      if (DstStep > CurStep)                                    //请求向相反方向旋转
      {
        if (pMotor -> Speed > STEPPER_MOTOR_DIR_SWITCH_SPEED)   //当前电机速度高于换向速度
        {
          Mode    = STEPPER_MOTOR_MODE_BRAKE;                   //制动模式
          DstStep = 0;                                          //维持当前旋转方向                                      
        }
      }
      else if (DstStep > pMotor -> DstStep)                     //请求旋转步数减少
      {
        if (pMotor -> Speed > SpeedLimit)                       //当前电机转速过快
        {
          Mode    = STEPPER_MOTOR_MODE_BRAKE;                   //制动模式
          DstStep = 0;                                          //维持当前旋转方向    
        }
      }
    }
  }
  
  //Step 3: 调整走动速度
  if (Mode == STEPPER_MOTOR_MODE_BRAKE)     //制动模式
  {
    if (pMotor -> Speed <= STEPPER_MOTOR_BRAKE_ACC)             //使用制动加速度进行制动
      pMotor -> Speed  = 0;
    else
      pMotor -> Speed -= STEPPER_MOTOR_BRAKE_ACC;
  }
  else                                      //正常行走模式
  {
    if (pMotor -> Speed < SpeedLimit)                           //加速控制
    {
      pMotor -> Speed += pMotor -> Acc;
      
      if (pMotor -> Speed > SpeedLimit)
        pMotor -> Speed = SpeedLimit;
    }
    else                                                        //减速控制
    {
      if (StepperMotorGlobalCtrl.RstMode == 0)
      {
        if (pMotor -> Speed - SpeedLimit < STEPPER_MOTOR_ACC_MAX)
          pMotor -> Speed  = SpeedLimit;
        else
          pMotor -> Speed -= STEPPER_MOTOR_ACC_MAX;
      }
    }
  }
  
  if (pMotor -> Speed < STEPPER_MOTOR_SPEED_MIN)                //限速
    pMotor -> Speed = STEPPER_MOTOR_SPEED_MIN;
  else if (pMotor -> Speed > pMotor -> SpeedMax)
    pMotor -> Speed = pMotor -> SpeedMax;
  
  if (StepperMotorGlobalCtrl.CalMode)
  {
    if (pMotor -> Speed < STEPPER_MOTOR_CAL_SPEED_MIN)
      pMotor -> Speed = STEPPER_MOTOR_CAL_SPEED_MIN;
  }
  
  //Step 4: 转换走动速度至行走周期
  Interval = 20000 / pMotor -> Speed;
  
  //Step 5: 输出参数
  DisableInterrupts;
  pMotor -> Interval = Interval;
  pMotor -> DstStep  = DstStep;
  pMotor -> Accomp   = 0;
  EnableInterrupts;
  
  return pMotor -> CurStepBackup;
}

/******************************************************************************
函数名：Stepper_Motor_Fine_Reset_Init
功  能：初始化步进电机进入小角度归零模式
参  数：无
返回值：无
******************************************************************************/
static void Stepper_Motor_Fine_Reset_Init(void)
{
  StepperMotorGlobalCtrl.Flag    = 0;
  
  DisableInterrupts;

  Motor_Controller_Init();

  #if STEPPER_MOTOR_0_ENABLE
    StepperMotor0.Accomp        = 0;
    StepperMotor0.Phase         = 0;
    StepperMotor0.ZeroSw        = 0;
    StepperMotor0.Timer         = 0;
    StepperMotor0.Interval      = 20000 /(uint16_t) STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor0.DstStep       = 0;//(uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.Speed         = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor0.SpeedMax      = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor0.Acc           = 0;

    StepperMotorGlobalCtrl.Flag |= 0x01;
  #endif

  #if STEPPER_MOTOR_1_ENABLE
    StepperMotor1.Accomp        = 0;
    StepperMotor1.Phase         = 0;
    StepperMotor1.ZeroSw        = 0;
    StepperMotor1.Timer         = 0;
    StepperMotor1.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor1.DstStep       = 0;//(uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.Speed         = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor1.SpeedMax      = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor1.Acc           = 0;

    StepperMotorGlobalCtrl.Flag |= 0x02;
  #endif
  
  #if STEPPER_MOTOR_2_ENABLE
    StepperMotor2.Accomp        = 0;
    StepperMotor2.Phase         = 0;
    StepperMotor2.ZeroSw        = 0;
    StepperMotor2.Timer         = 0;
    StepperMotor2.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor2.DstStep       = 0;//(uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.Speed         = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor2.SpeedMax      = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor2.Acc           = 0;

    StepperMotorGlobalCtrl.Flag |= 0x04;
  #endif  
  
  #if STEPPER_MOTOR_3_ENABLE
    StepperMotor3.Accomp        = 0;
    StepperMotor3.Phase         = 0;
    StepperMotor3.ZeroSw        = 0;
    StepperMotor3.Timer         = 0;
    StepperMotor3.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor3.DstStep       = 0;//(uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FINE_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.Speed         = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor3.SpeedMax      = STEPPER_MOTOR_FINE_RST_SPEED;
    StepperMotor3.Acc           = 0;

    StepperMotorGlobalCtrl.Flag |= 0x08;
  #endif
  
  EnableInterrupts;
  
  StepperMotorGlobalCtrl.CalMode = 0;
  StepperMotorGlobalCtrl.RstMode = 1;
}

/******************************************************************************
函数名：Stepper_Motor_Running_Init
功  能：初始化步进电机进入正常工作模式
参  数：无
返回值：无
******************************************************************************/
static void Stepper_Motor_Running_Init(void)
{
  #if STEPPER_MOTOR_0_ENABLE
    StepperMotor0.Interval       = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor0.Speed          = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor0.SpeedMax       = STEPPER_MOTOR_0_SPEED_MAX;
    StepperMotor0.Acc            = STEPPER_MOTOR_0_ACC;
  #endif

  #if STEPPER_MOTOR_1_ENABLE
    StepperMotor1.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor1.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor1.SpeedMax      = STEPPER_MOTOR_1_SPEED_MAX;
    StepperMotor1.Acc           = STEPPER_MOTOR_1_ACC;
  #endif
  
  #if STEPPER_MOTOR_2_ENABLE
    StepperMotor2.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor2.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor2.SpeedMax      = STEPPER_MOTOR_2_SPEED_MAX;
    StepperMotor2.Acc           = STEPPER_MOTOR_2_ACC;
  #endif  
  
  #if STEPPER_MOTOR_3_ENABLE
    StepperMotor3.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor3.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor3.SpeedMax      = STEPPER_MOTOR_3_SPEED_MAX;
    StepperMotor3.Acc           = STEPPER_MOTOR_3_ACC;
  #endif
  
  StepperMotorGlobalCtrl.CalMode = 0;
  StepperMotorGlobalCtrl.RstMode = 0;
  
  StepperMotor0DstStep = 0;
  StepperMotor1DstStep = 0;
  StepperMotor2DstStep = 0;
  StepperMotor3DstStep = 0;
}

/******************************************************************************
函数名：Stepper_Motor_Rolling_Back_Init
功  能：初始化步进电机进入断电归零工作模式
参  数：无
返回值：无
******************************************************************************/
static void Stepper_Motor_Rolling_Back_Init(void)
{
  StepperMotorGlobalCtrl.Flag    = 0;

  #if STEPPER_MOTOR_0_ENABLE
    StepperMotor0.SpeedMax       = STEPPER_MOTOR_SHDN_RST_SPEED;
    StepperMotor0.Acc            = STEPPER_MOTOR_SHDN_RST_ACC;
    StepperMotorGlobalCtrl.Flag |= 0x01;
  #endif

  #if STEPPER_MOTOR_1_ENABLE
    StepperMotor1.SpeedMax       = STEPPER_MOTOR_SHDN_RST_SPEED;
    StepperMotor1.Acc            = STEPPER_MOTOR_SHDN_RST_ACC;
    StepperMotorGlobalCtrl.Flag |= 0x02;
  #endif
  
  #if STEPPER_MOTOR_2_ENABLE
    StepperMotor2.SpeedMax       = STEPPER_MOTOR_SHDN_RST_SPEED;
    StepperMotor2.Acc            = STEPPER_MOTOR_SHDN_RST_ACC;
    StepperMotorGlobalCtrl.Flag |= 0x04;
  #endif  
  
  #if STEPPER_MOTOR_3_ENABLE
    StepperMotor3.SpeedMax       = STEPPER_MOTOR_SHDN_RST_SPEED;
    StepperMotor3.Acc            = STEPPER_MOTOR_SHDN_RST_ACC;
    StepperMotorGlobalCtrl.Flag |= 0x08;
  #endif
}

/******************************************************************************
函数名：Stepper_Motor_Reset
功  能：步进电机初次上电时,满角度复位(归零)
        步进电机初次上电(KL30)时,因不确定当前指针位置,进行大角度的归零操作,迫使
        步指针回复至零位置
参  数：无
返回值：无
******************************************************************************/
void Stepper_Motor_Reset(void)
{
  uint16_t CurStep;
  
  StepperMotorGlobalCtrl.Flag    = 0;
  
  DisableInterrupts;
  
  Motor_Controller_Init();
  
  #if STEPPER_MOTOR_0_ENABLE
    StepperMotor0.Accomp        = 0;
    StepperMotor0.Phase         = 0;
    StepperMotor0.ZeroSw        = 0;
    StepperMotor0.Timer         = 0;
    StepperMotor0.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor0.DstStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor0.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor0.SpeedMax      = STEPPER_MOTOR_FULL_RST_SPEED;
    StepperMotor0.Acc           = STEPPER_MOTOR_FULL_RST_ACC;

    StepperMotorGlobalCtrl.Flag |= 0x01;
  #endif
  
  #if STEPPER_MOTOR_1_ENABLE
    StepperMotor1.Accomp        = 0;
    StepperMotor1.Phase         = 0;
    StepperMotor1.ZeroSw        = 0;
    StepperMotor1.Timer         = 0;
    StepperMotor1.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor1.DstStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor1.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor1.SpeedMax      = STEPPER_MOTOR_FULL_RST_SPEED;
    StepperMotor1.Acc           = STEPPER_MOTOR_FULL_RST_ACC;

    StepperMotorGlobalCtrl.Flag |= 0x02;
  #endif
  
  #if STEPPER_MOTOR_2_ENABLE
    StepperMotor2.Accomp        = 0;
    StepperMotor2.Phase         = 0;
    StepperMotor2.ZeroSw        = 0;
    StepperMotor2.Timer         = 0;
    StepperMotor2.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor2.DstStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor2.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor2.SpeedMax      = STEPPER_MOTOR_FULL_RST_SPEED;
    StepperMotor2.Acc           = STEPPER_MOTOR_FULL_RST_ACC;

    StepperMotorGlobalCtrl.Flag |= 0x04;
  #endif  
  
  #if STEPPER_MOTOR_3_ENABLE
    StepperMotor3.Accomp        = 0;
    StepperMotor3.Phase         = 0;
    StepperMotor3.ZeroSw        = 0;
    StepperMotor3.Timer         = 0;
    StepperMotor3.Interval      = 20000 / (uint16_t)STEPPER_MOTOR_SPEED_MIN;
    StepperMotor3.DstStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.CurStep       = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.CurStepBackup = (uint16_t)STEPPER_MOTOR_RST_FULL_DEG * (uint16_t)STEPPER_MOTOR_STEP_PER_10DEG / 10;
    StepperMotor3.Speed         = STEPPER_MOTOR_SPEED_MIN;
    StepperMotor3.SpeedMax      = STEPPER_MOTOR_FULL_RST_SPEED;
    StepperMotor3.Acc           = STEPPER_MOTOR_FULL_RST_ACC;

    StepperMotorGlobalCtrl.Flag |= 0x08;
  #endif
  
  EnableInterrupts;
  
  StepperMotorGlobalCtrl.CalMode = 0;
  StepperMotorGlobalCtrl.RstMode = 1;
  
  while (StepperMotorGlobalCtrl.Flag)
  {
    wdt_reset();
    
    #if STEPPER_MOTOR_0_ENABLE
      CurStep = Stepper_Motor_Speed_Control(&StepperMotor0, 0);
      if (CurStep == 0)
        StepperMotorGlobalCtrl.Flag &= 0x0E;
      
      #ifdef STEPPER_MOTOR_0_ZERO_SW
        if (STEPPER_MOTOR_0_ZERO_SW == 0)
        {
          if (StepperMotor0.ZeroSw == 0)
          {
            StepperMotorGlobalCtrl.Flag &= 0x0E;
            StepperMotor0.ZeroSw         = 1;
            
            DisableInterrupts;
            StepperMotor0.DstStep        = 0;
            StepperMotor0.CurStep        = 0;
            StepperMotor0.CurStepBackup  = 0;
            EnableInterrupts;
          }
        }
      #endif
    #endif
      
     #if STEPPER_MOTOR_1_ENABLE
      CurStep = Stepper_Motor_Speed_Control(&StepperMotor1, 0);
      if (CurStep == 0)
        StepperMotorGlobalCtrl.Flag &= 0x0D;
      
      #ifdef STEPPER_MOTOR_1_ZERO_SW
        if (STEPPER_MOTOR_1_ZERO_SW == 0)
        {
          if (StepperMotor1.ZeroSw == 0)
          {
            StepperMotorGlobalCtrl.Flag &= 0x0D;
            StepperMotor1.ZeroSw         = 1;
            
            DisableInterrupts;
            StepperMotor1.DstStep        = 0;
            StepperMotor1.CurStep        = 0;
            StepperMotor1.CurStepBackup  = 0;
            EnableInterrupts;
          }
        }
      #endif
    #endif     

     #if STEPPER_MOTOR_2_ENABLE
      CurStep = Stepper_Motor_Speed_Control(&StepperMotor2, 0);
      if (CurStep == 0)
        StepperMotorGlobalCtrl.Flag &= 0x0B;
      
      #ifdef STEPPER_MOTOR_2_ZERO_SW
        if (STEPPER_MOTOR_2_ZERO_SW == 0)
        {
          if (StepperMotor2.ZeroSw == 0)
          {
            StepperMotorGlobalCtrl.Flag &= 0x0B;
            StepperMotor2.ZeroSw         = 1;
            
            DisableInterrupts;
            StepperMotor2.DstStep        = 0;
            StepperMotor2.CurStep        = 0;
            StepperMotor2.CurStepBackup  = 0;
            EnableInterrupts;
          }
        }
      #endif
    #endif   

     #if STEPPER_MOTOR_3_ENABLE
      CurStep = Stepper_Motor_Speed_Control(&StepperMotor3, 0);
      if (CurStep == 0)
        StepperMotorGlobalCtrl.Flag &= 0x07;
      
      #ifdef STEPPER_MOTOR_3_ZERO_SW
        if (STEPPER_MOTOR_3_ZERO_SW == 0)
        {
          if (StepperMotor3.ZeroSw == 0)
          {
            StepperMotorGlobalCtrl.Flag &= 0x07;
            StepperMotor3.ZeroSw         = 1;
            
            DisableInterrupts;
            StepperMotor3.DstStep        = 0;
            StepperMotor3.CurStep        = 0;
            StepperMotor3.CurStepBackup  = 0;
            EnableInterrupts;
          }
        }
      #endif
    #endif
  }
  
  StepperMotorGlobalCtrl.RstMode = 0;
  StepperMotorGlobalCtrl.PwrReq  = STEPPER_MOTOR_NO_REQ;
  StepperMotorGlobalCtrl.Status  = STEPPER_MOTOR_STOP;
  
  DisableInterrupts;
  
  StepperMotorGlobalCtrl.Delay = 3000;
  Motor_Controller_Shutdown();
  
  EnableInterrupts;
}

/******************************************************************************
函数名：Stepper_Motor_Speed_Control_Service
功  能：步进电机的行走速度控制
参  数：无
返回值：无
*******************************************************************************
注  意：该服务函数必须于系统空闲时实时调用
******************************************************************************/
void Stepper_Motor_Speed_Control_Service(void)
{
  uint16_t CurStep;
  
  switch (StepperMotorGlobalCtrl.Status)
  {
    
    case STEPPER_MOTOR_STOP   :
    default                   : 
        if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_PWR_UP_REQ)
        {
          if (StepperMotorGlobalCtrl.Delay == 0)
          {
            Stepper_Motor_Fine_Reset_Init();
            StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
            StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_RST;
          }
        }
        break;
                                
    case STEPPER_MOTOR_RST    : 
        #if STEPPER_MOTOR_0_ENABLE
          CurStep = Stepper_Motor_Speed_Control(&StepperMotor0, 0);
          if (CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0E;
          
          #ifdef STEPPER_MOTOR_0_ZERO_SW
            if (STEPPER_MOTOR_0_ZERO_SW == 0)
            {
              if (StepperMotor0.ZeroSw == 0)
              {
                StepperMotor0.ZeroSw         = 1;
                
                DisableInterrupts;
                StepperMotor0.CurStep        = 5;
                StepperMotor0.CurStepBackup  = 5;
                EnableInterrupts;
              }
            }
          #endif
        #endif
          
        #if STEPPER_MOTOR_1_ENABLE
          CurStep = Stepper_Motor_Speed_Control(&StepperMotor1, 0);
          if (CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0D;
          
          #ifdef STEPPER_MOTOR_1_ZERO_SW
            if (STEPPER_MOTOR_1_ZERO_SW == 0)
            {
              if (StepperMotor1.ZeroSw == 0)
              {
                StepperMotor1.ZeroSw         = 1;
                
                DisableInterrupts;
                StepperMotor1.CurStep        = 5;
                StepperMotor1.CurStepBackup  = 5;
                EnableInterrupts;
              }
            }
          #endif
        #endif     

        #if STEPPER_MOTOR_2_ENABLE
          CurStep = Stepper_Motor_Speed_Control(&StepperMotor2, 0);
          if (CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0B;
          
          #ifdef STEPPER_MOTOR_2_ZERO_SW
            if (STEPPER_MOTOR_2_ZERO_SW == 0)
            {
              if (StepperMotor2.ZeroSw == 0)
              {
                StepperMotor2.ZeroSw         = 1;
                
                DisableInterrupts;
                StepperMotor2.CurStep        = 5;
                StepperMotor2.CurStepBackup  = 5;
                EnableInterrupts;
              }
            }
          #endif
        #endif   

        #if STEPPER_MOTOR_3_ENABLE
          CurStep = Stepper_Motor_Speed_Control(&StepperMotor3, 0);
          if (CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x07;
          
          #ifdef STEPPER_MOTOR_3_ZERO_SW
            if (STEPPER_MOTOR_3_ZERO_SW == 0)
            {
              if (StepperMotor3.ZeroSw == 0)
              {
                StepperMotor3.ZeroSw         = 1;
                
                DisableInterrupts;
                StepperMotor3.CurStep        = 5;
                StepperMotor3.CurStepBackup  = 5;
                EnableInterrupts;
              }
            }
          #endif
        #endif
        
        if (StepperMotorGlobalCtrl.Flag == 0)
        {
          if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_SHDN_REQ)
          {
            StepperMotorGlobalCtrl.CalMode = 0;
            Motor_Controller_Shutdown();
            StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_STOP;
          }                                  
          else
          {
            Stepper_Motor_Running_Init();
            StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_READY;
          }
          
          StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
        }
        break;
                                
    case STEPPER_MOTOR_READY  : 
        if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_SHDN_REQ)
        {
          Stepper_Motor_Rolling_Back_Init();
          StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
          StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_BACK;
        }                                  
        else
        {
          #if STEPPER_MOTOR_0_ENABLE
            StepperMotor0CurStep = Stepper_Motor_Speed_Control(&StepperMotor0, StepperMotor0DstStep);
          #endif
          
          #if STEPPER_MOTOR_1_ENABLE
            StepperMotor1CurStep = Stepper_Motor_Speed_Control(&StepperMotor1, StepperMotor1DstStep);
          #endif 
          
          #if STEPPER_MOTOR_2_ENABLE
            StepperMotor2CurStep = Stepper_Motor_Speed_Control(&StepperMotor2, StepperMotor2DstStep);
          #endif
          
          #if STEPPER_MOTOR_3_ENABLE
            StepperMotor3CurStep = Stepper_Motor_Speed_Control(&StepperMotor3, StepperMotor3DstStep);
          #endif
        }
        break;
                                
    case STEPPER_MOTOR_BACK   :
        #if STEPPER_MOTOR_0_ENABLE
          StepperMotor0CurStep = Stepper_Motor_Speed_Control(&StepperMotor0, 0);
          if (StepperMotor0CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0E;
        #endif
        
        #if STEPPER_MOTOR_1_ENABLE
          StepperMotor1CurStep = Stepper_Motor_Speed_Control(&StepperMotor1, 0);
          if (StepperMotor1CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0D;
        #endif 
        
        #if STEPPER_MOTOR_2_ENABLE
          StepperMotor2CurStep = Stepper_Motor_Speed_Control(&StepperMotor2, 0);
          if (StepperMotor2CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x0B;
        #endif
        
        #if STEPPER_MOTOR_3_ENABLE
          StepperMotor3CurStep = Stepper_Motor_Speed_Control(&StepperMotor3, 0);
          if (StepperMotor3CurStep == 0)
            StepperMotorGlobalCtrl.Flag &= 0x07;
        #endif
        
        if (StepperMotorGlobalCtrl.Flag == 0)
        {
          if (StepperMotorGlobalCtrl.PwrReq == STEPPER_MOTOR_PWR_UP_REQ)
          {
            Stepper_Motor_Fine_Reset_Init();
            StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_RST;
          }                                  
          else
          {
            StepperMotorGlobalCtrl.CalMode = 0;
            Motor_Controller_Shutdown();
            StepperMotorGlobalCtrl.Status = STEPPER_MOTOR_STOP;
          }
          
          StepperMotorGlobalCtrl.PwrReq = STEPPER_MOTOR_NO_REQ;
        }
        break;
  }
}

/******************************************************************************
函数名：Stepper_Motor_Rotating_ISR
功  能：步进电机旋转控制中断函数
参  数：无
返回值：无
*******************************************************************************
注  意：函数需要嵌入到定时中断之中
******************************************************************************/
void Stepper_Motor_Rotating_ISR(void)
{
  if (StepperMotorGlobalCtrl.Delay)
    StepperMotorGlobalCtrl.Delay--;
  
  #if STEPPER_MOTOR_0_ENABLE
  StepperMotor0.Timer++;
  if (StepperMotor0.Timer >= StepperMotor0.Interval)
  {
    if (StepperMotor0.CurStep != StepperMotor0.DstStep)
    {
      if (StepperMotor0.DstStep > StepperMotor0.CurStep)        //目标微步数 > 当前微步数, 正向旋转
      {
        StepperMotor0.CurStep++;
        
        #if STEPPER_MOTOR_0_REVERSE
          if (StepperMotor0.Phase == 0)
            StepperMotor0.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor0.Phase--;
        #else
          StepperMotor0.Phase++;
          if (StepperMotor0.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor0.Phase = 0;
        #endif
      }
      else                                                      //目标微步数 < 当前微步数, 逆向旋转
      {
        StepperMotor0.CurStep--;
        
        #if STEPPER_MOTOR_0_REVERSE
          StepperMotor0.Phase++;
          if (StepperMotor0.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor0.Phase = 0;
        #else
          if (StepperMotor0.Phase == 0)
            StepperMotor0.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor0.Phase--;
        #endif
      }
      Motor_Controller_Set_CH0_Duty_Cycle(StepperMotorCosDutyCycleTable[StepperMotor0.Phase]);
      Motor_Controller_Set_CH1_Duty_Cycle(StepperMotorSinDutyCycleTable[StepperMotor0.Phase]);
    }
    
    StepperMotor0.Timer  = 0;
    StepperMotor0.Accomp = 1;
  }
  #endif
  
  #if STEPPER_MOTOR_1_ENABLE
  StepperMotor1.Timer++;
  if (StepperMotor1.Timer >= StepperMotor1.Interval)
  {
    if (StepperMotor1.CurStep != StepperMotor1.DstStep)
    {
      if (StepperMotor1.DstStep > StepperMotor1.CurStep)        //目标微步数 > 当前微步数, 正向旋转
      {
        StepperMotor1.CurStep++;
        
        #if STEPPER_MOTOR_1_REVERSE
          if (StepperMotor1.Phase == 0)
            StepperMotor1.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor1.Phase--;
        #else
          StepperMotor1.Phase++;
          if (StepperMotor1.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor1.Phase = 0;
        #endif
      }
      else                                                      //目标微步数 < 当前微步数, 逆向旋转
      {
        StepperMotor1.CurStep--;
        
        #if STEPPER_MOTOR_1_REVERSE
          StepperMotor1.Phase++;
          if (StepperMotor1.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor1.Phase = 0;
        #else
          if (StepperMotor1.Phase == 0)
            StepperMotor1.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor1.Phase--;
        #endif
      }
      Motor_Controller_Set_CH2_Duty_Cycle(StepperMotorCosDutyCycleTable[StepperMotor1.Phase]);
      Motor_Controller_Set_CH3_Duty_Cycle(StepperMotorSinDutyCycleTable[StepperMotor1.Phase]);
    }
    
    StepperMotor1.Timer  = 0;
    StepperMotor1.Accomp = 1;
  }
  #endif

  #if STEPPER_MOTOR_2_ENABLE
  StepperMotor2.Timer++;
  if (StepperMotor2.Timer >= StepperMotor2.Interval)
  {
    if (StepperMotor2.CurStep != StepperMotor2.DstStep)
    {
      if (StepperMotor2.DstStep > StepperMotor2.CurStep)        //目标微步数 > 当前微步数, 正向旋转
      {
        StepperMotor2.CurStep++;
        
        #if STEPPER_MOTOR_2_REVERSE
          if (StepperMotor2.Phase == 0)
            StepperMotor2.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor2.Phase--;
        #else
          StepperMotor2.Phase++;
          if (StepperMotor2.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor2.Phase = 0;
        #endif
      }
      else                                                      //目标微步数 < 当前微步数, 逆向旋转
      {
        StepperMotor2.CurStep--;
        
        #if STEPPER_MOTOR_2_REVERSE
          StepperMotor2.Phase++;
          if (StepperMotor2.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor2.Phase = 0;
        #else
          if (StepperMotor2.Phase == 0)
            StepperMotor2.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor2.Phase--;
        #endif
      }
      Motor_Controller_Set_CH4_Duty_Cycle(StepperMotorCosDutyCycleTable[StepperMotor2.Phase]);
      Motor_Controller_Set_CH5_Duty_Cycle(StepperMotorSinDutyCycleTable[StepperMotor2.Phase]);
    }
    
    StepperMotor2.Timer  = 0;
    StepperMotor2.Accomp = 1;
  }
  #endif  
  
  #if STEPPER_MOTOR_3_ENABLE
  StepperMotor3.Timer++;
  if (StepperMotor3.Timer >= StepperMotor3.Interval)
  {
    if (StepperMotor3.CurStep != StepperMotor3.DstStep)
    {
      if (StepperMotor3.DstStep > StepperMotor3.CurStep)        //目标微步数 > 当前微步数, 正向旋转
      {
        StepperMotor3.CurStep++;
        
        #if STEPPER_MOTOR_3_REVERSE
          if (StepperMotor3.Phase == 0)
            StepperMotor3.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor3.Phase--;
        #else
          StepperMotor3.Phase++;
          if (StepperMotor3.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor3.Phase = 0;
        #endif
      }
      else                                                      //目标微步数 < 当前微步数, 逆向旋转
      {
        StepperMotor3.CurStep--;
        
        #if STEPPER_MOTOR_3_REVERSE
          StepperMotor3.Phase++;
          if (StepperMotor3.Phase >= STEPPER_MOTOR_TOTAL_PHASE)
            StepperMotor3.Phase = 0;
        #else
          if (StepperMotor3.Phase == 0)
            StepperMotor3.Phase = (STEPPER_MOTOR_TOTAL_PHASE - 1);
          else
            StepperMotor3.Phase--;
        #endif
      }
      Motor_Controller_Set_CH6_Duty_Cycle(StepperMotorCosDutyCycleTable[StepperMotor3.Phase]);
      Motor_Controller_Set_CH7_Duty_Cycle(StepperMotorSinDutyCycleTable[StepperMotor3.Phase]);
    }
    
    StepperMotor3.Timer  = 0;
    StepperMotor3.Accomp = 1;
  }
  #endif
}