/******************************************************************************
�� �� ����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
��  �ܣ��������У׼ģʽʹ��
        ֻ���ڵ��׼��������ſ��Խ���У׼
        �������������У׼ģʽ,�߶��ٶȽ������,���ڿ�������DZ���У
��  ����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
}