6Step_Lib.c

Go to the documentation of this file.

/**
 ******************************************************************************
 * @file    6Step_Lib.c
 * @author  System lab - Automation and Motion control team
 * @version V1.0.0
 * @date    06-July-2015
 * @brief   This file provides the set of functions for Motor Control library 
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
 */

/*! ****************************************************************************
================================================================================   
                 ###### Main functions for 6-Step algorithm ######
================================================================================     
The main function are the following:

1) MC_SixStep_TABLE(...) -> Set the peripherals (TIMx, GPIO etc.) for each step
2) MC_SixStep_ARR_step() -> Generate the ARR value for Low Frequency TIM during start-up
3) MC_SixStep_INIT()     -> Init the main variables for motor driving from MC_SixStep_param.h
4) MC_SixStep_RESET()    -> Reset all variables used for 6Step control algorithm
5) MC_SixStep_Ramp_Motor_calc() -> Calculate the acceleration profile step by step for motor during start-up 
6) MC_SixStep_NEXT_step()-> Generate the next step number according with the direction (CW or CCW)
7) MC_Task_Speed()       -> Speed Loop with PI regulator
8) MC_Set_Speed(...)     -> Set the new motor speed value
9) MC_StartMotor()       -> Start the Motor
10)MC_StopMotor()       -> Stop the Motor
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "6Step_Lib.h"

#include <string.h>

/** @addtogroup MIDDLEWARES     MIDDLEWARES 
  * @brief  Middlewares Layer
  * @{ 
  */


/** @addtogroup MC_6-STEP_LIB       MC_6-STEP_LIB 
  * @brief  Motor Control driver
  * @{ 
  */

/* Data struct ---------------------------------------------------------------*/
SIXSTEP_Base_InitTypeDef SIXSTEP_parameters;            /*!< Main SixStep structure*/ 
SIXSTEP_PI_PARAM_InitTypeDef_t PI_parameters;           /*!< SixStep PI regulator structure*/ 

/* Variables -----------------------------------------------------------------*/
uint16_t Rotor_poles_pairs;                         /*!<  Number of pole pairs of the motor */ 
uint32_t mech_accel_hz = 0;                         /*!<  Hz -- Mechanical acceleration rate */
uint32_t constant_k = 0;                            /*!<  1/3*mech_accel_hz */
uint32_t Time_vector_tmp = 0;                      /*!<  Startup variable  */
uint32_t Time_vector_prev_tmp = 0 ;                 /*!<  Startup variable  */
uint32_t T_single_step = 0;                         /*!<  Startup variable  */
uint32_t T_single_step_first_value = 0;             /*!<  Startup variable  */
int32_t  delta = 0;                                 /*!<  Startup variable  */
uint16_t index_array = 1;                           /*!<  Speed filter variable */
int16_t speed_tmp_array[FILTER_DEEP];               /*!<  Speed filter variable */
uint16_t speed_tmp_buffer[FILTER_DEEP];             /*!<  Potentiometer filter variable */
uint16_t HFBuffer[HFBUFFERSIZE];                    /*!<  Buffer for Potentiometer Value Filtering at the High-Frequency ADC conversion */ 
uint16_t HFBufferIndex = 0;                         /*!<  High-Frequency Buffer Index */ 
uint8_t  array_completed = FALSE;                   /*!<  Speed filter variable */
uint8_t  buffer_completed = FALSE;                  /*!<  Potentiometer filter variable */
uint8_t  UART_FLAG_RECEIVE = FALSE;                 /*!<  UART commmunication flag */
uint32_t ARR_LF = 0;                                /*!<  Autoreload LF TIM variable */
int32_t Mech_Speed_RPM = 0;                         /*!<  Mechanical motor speed */
int32_t El_Speed_Hz = 0;                            /*!<  Electrical motor speed */
uint16_t index_adc_chn = 0;                         /*!<  Index of ADC channel selector for measuring */
uint16_t index_motor_run = 0;                       /*!<  Tmp variable for DEMO mode */
uint16_t test_motor_run = 1;                        /*!<  Tmp variable for DEMO mode */
uint8_t Enable_start_button = TRUE;                 /*!<  Start/stop button filter to avoid double command */
uint16_t index_ARR_step = 1;                           
uint32_t n_zcr_startup = 0;
uint16_t index_startup_motor = 1;
uint16_t target_speed = TARGET_SPEED;               /*!<  Target speed for closed loop control */
uint16_t shift_n_sqrt = 14;
uint16_t cnt_bemf_event = 0;
uint8_t startup_bemf_failure = 0;
uint8_t speed_fdbk_error = 0;
__IO uint32_t uwTick = 0;                        /*!<  Tick counter - 1msec updated */
uint8_t dac_status = DAC_ENABLE;
uint16_t index_align = 1;
int32_t speed_sum_sp_filt = 0;
int32_t speed_sum_pot_filt = 0;
uint16_t index_pot_filt = 1; 
int16_t potent_filtered = 0;
uint32_t Tick_cnt = 0;  
uint32_t counter_ARR_Bemf = 0;
uint64_t constant_multiplier_tmp = 0;
 
int16_t MC_PI_Controller(SIXSTEP_PI_PARAM_InitTypeDef_t *, int16_t);
uint16_t MC_Potentiometer_filter(uint16_t);
uint64_t MCM_Sqrt(uint64_t );
int32_t MC_GetElSpeedHz(void);
int32_t MC_GetMechSpeedRPM(void);
void MC_SixStep_NEXT_step(void);
void MC_Speed_Filter(void);
void MC_SixStep_ARR_step(void);
void MC_SixStep_TABLE(uint8_t);
void MC_SixStep_Speed_Potentiometer(void);
void MC_Set_PI_param(SIXSTEP_PI_PARAM_InitTypeDef_t *);
void MC_Task_Speed(void); 
void MC_SixStep_Alignment(void);
void MC_Bemf_Delay(void);
void MC_TIMx_SixStep_timebase(void);
void MC_ADCx_SixStep_Bemf(void);
void MC_SysTick_SixStep_MediumFrequencyTask(void);
void MC_SixStep_Ramp_Motor_calc(void);
void MC_SixStep_EnableInput_CH1_E_CH2_E_CH3_D(void);
void MC_SixStep_EnableInput_CH1_E_CH2_D_CH3_E(void);
void MC_SixStep_EnableInput_CH1_D_CH2_E_CH3_E(void);
void MC_SixStep_DisableInput_CH1_D_CH2_D_CH3_D(void);
void MC_SixStep_Start_PWM_driving(void);
void MC_SixStep_Stop_PWM_driving(void);
void MC_SixStep_HF_TIMx_SetDutyCycle_CH1(uint16_t);    
void MC_SixStep_HF_TIMx_SetDutyCycle_CH2(uint16_t);    
void MC_SixStep_HF_TIMx_SetDutyCycle_CH3(uint16_t);   
void MC_SixStep_Current_Reference_Start(void);         
void MC_SixStep_Current_Reference_Stop(void);          
void MC_SixStep_Current_Reference_Setvalue(uint16_t);  
void MC_SixStep_ARR_Bemf(uint8_t);
void MC_UI_INIT(void);
void UART_Set_Value(void);
void UART_Communication_Task(void);
void MC_SixStep_Init_main_data(void);
void CMD_Parser(char* pCommandString);
void MC_SixStep_Speed_Val_target_potentiometer(void);

/** @defgroup MC_SixStep_TABLE    MC_SixStep_TABLE
  *  @{
    * @brief Set the peripherals (TIMx, GPIO etc.) for each step
    * @param  step_number: step number selected
    * @retval None
  */

void MC_SixStep_TABLE(uint8_t step_number)
{ 
 if(GPIO_COMM == 1)
 {
  HAL_GPIO_TogglePin(GPIO_PORT_COMM,GPIO_CH_COMM);  
 }
 
 switch (step_number)
  { 
    case 1:
      {  
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(0);          
          MC_SixStep_EnableInput_CH1_E_CH2_E_CH3_D();
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[3];         
      }
     break;
    case 2:
      {                   
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(0);                              
          MC_SixStep_EnableInput_CH1_E_CH2_D_CH3_E();   
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[2];        
      }
     break;     
    case 3:
      {     
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(0);  
          MC_SixStep_EnableInput_CH1_D_CH2_E_CH3_E();           
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[1];                    
      }
     break;     
    case 4:
      { 
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(0);   
          MC_SixStep_EnableInput_CH1_E_CH2_E_CH3_D();        
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[3]; 
      }
     break;  
    case 5:
      {
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(0);  
          MC_SixStep_EnableInput_CH1_E_CH2_D_CH3_E();                    
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[2];        
      }
     break;
    case 6:
      {   
          MC_SixStep_HF_TIMx_SetDutyCycle_CH3(SIXSTEP_parameters.pulse_value);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH2(0);
          MC_SixStep_HF_TIMx_SetDutyCycle_CH1(0);  
          MC_SixStep_EnableInput_CH1_D_CH2_E_CH3_E();              
          SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[1]; 
      }
     break;        
  }
}

/**
  * @}
  */

/** @defgroup MC_SixStep_NEXT_step    MC_SixStep_NEXT_step
  *  @{
    * @brief Generate the next step number according with the direction (CW or CCW)
    * @retval uint8_t SIXSTEP_parameters.status
  */
void MC_SixStep_NEXT_step()
{
 
 if(SIXSTEP_parameters.CMD == TRUE)
 { 
    SIXSTEP_parameters.CMD = FALSE;
    MC_SixStep_Start_PWM_driving();     
 } 
  ARR_LF = __HAL_TIM_GetAutoreload(&LF_TIMx);
   
  if(SIXSTEP_parameters.ALIGN_OK == TRUE) 
  {
   SIXSTEP_parameters.speed_fdbk = MC_GetMechSpeedRPM();    
   SIXSTEP_parameters.demagn_counter = 1;
   if(SIXSTEP_parameters.status_prev != SIXSTEP_parameters.step_position)
   {
     n_zcr_startup = 0;
   }
   if(PI_parameters.Reference>=0)
   { 
    SIXSTEP_parameters.step_position++;  
    if(SIXSTEP_parameters.CL_READY == TRUE)
    {
      SIXSTEP_parameters.VALIDATION_OK = TRUE;   
    }
    if(SIXSTEP_parameters.step_position>6)
    { 
     SIXSTEP_parameters.step_position = 1;
    }
   }
   else
   {
    SIXSTEP_parameters.step_position--;  
    if(SIXSTEP_parameters.CL_READY == TRUE)
    {
      SIXSTEP_parameters.VALIDATION_OK = TRUE;   
    }
    if(SIXSTEP_parameters.step_position < 1)
     { 
      SIXSTEP_parameters.step_position = 6; 
     } 
   }
  }
  
  if(SIXSTEP_parameters.VALIDATION_OK == 1) 
  { 
    /* Motor Stall condition detection and Speed-Feedback error generation */
    SIXSTEP_parameters.BEMF_Tdown_count++;
    if (SIXSTEP_parameters.BEMF_Tdown_count>BEMF_CONSEC_DOWN_MAX)
    { 
      speed_fdbk_error = 1;
    }
    else
    {  
      __HAL_TIM_SetAutoreload(&LF_TIMx,0xFFFF);        
    }
  } 
  MC_SixStep_TABLE(SIXSTEP_parameters.step_position);
   
  /*  It controls if the changing step request appears during DOWNcounting
   *  in this case it changes the ADC channel */
  
  /* UP-COUNTING direction started  DIR = 0*/ 
  if(__HAL_TIM_DIRECTION_STATUS(&HF_TIMx))
  {
   switch (SIXSTEP_parameters.step_position)
   { 
    case 1:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[3];  
     } 
    break; 
    case 2:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[2];   
     }  
    break;      
    case 3:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[1];
     }     
    break;      
    case 4:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[3]; 
     }  
    break;      
    case 5:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[2];   
     }  
    break;      
    case 6:
     {
        SIXSTEP_parameters.CurrentRegular_BEMF_ch = SIXSTEP_parameters.Regular_channel[1];      
     }     
    break;      
    } /* end switch case*/
   
     MC_SixStep_ADC_Channel(SIXSTEP_parameters.CurrentRegular_BEMF_ch);
   }
  
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_RESET    MC_SixStep_RESET
  *  @{
    * @brief Reset all variables used for 6Step control algorithm
    * @retval None
  */

void MC_SixStep_RESET()
{   
 SIXSTEP_parameters.CMD = TRUE; 
 SIXSTEP_parameters.numberofitemArr = NUMBER_OF_STEPS;  
 SIXSTEP_parameters.ADC_BEMF_threshold_UP = BEMF_THRSLD_UP; 
 SIXSTEP_parameters.ADC_BEMF_threshold_DOWN = BEMF_THRSLD_DOWN;  
 SIXSTEP_parameters.Ireference = STARTUP_CURRENT_REFERENCE;  
 SIXSTEP_parameters.Speed_Loop_Time = SPEED_LOOP_TIME;
 SIXSTEP_parameters.pulse_value = SIXSTEP_parameters.HF_TIMx_CCR;
 SIXSTEP_parameters.Speed_target_ramp = MAX_POT_SPEED;
 SIXSTEP_parameters.ALIGNMENT = FALSE;
 SIXSTEP_parameters.Speed_Ref_filtered = 0;
 SIXSTEP_parameters.demagn_value = INITIAL_DEMAGN_DELAY;
 
 SIXSTEP_parameters.CurrentRegular_BEMF_ch = 0;
 SIXSTEP_parameters.status_prev = 0;
 SIXSTEP_parameters.step_position = 0;

 
 LF_TIMx.Init.Prescaler = SIXSTEP_parameters.LF_TIMx_PSC;
 LF_TIMx.Instance->PSC =  SIXSTEP_parameters.LF_TIMx_PSC;
 LF_TIMx.Init.Period =    SIXSTEP_parameters.LF_TIMx_ARR;
 LF_TIMx.Instance->ARR =  SIXSTEP_parameters.LF_TIMx_ARR; 
 HF_TIMx.Init.Prescaler = SIXSTEP_parameters.HF_TIMx_PSC;
 HF_TIMx.Instance->PSC =  SIXSTEP_parameters.HF_TIMx_PSC;
 HF_TIMx.Init.Period =    SIXSTEP_parameters.HF_TIMx_ARR;
 HF_TIMx.Instance->ARR =  SIXSTEP_parameters.HF_TIMx_ARR;
 HF_TIMx.Instance->HF_TIMx_CCR1 = SIXSTEP_parameters.HF_TIMx_CCR;  
 
 Rotor_poles_pairs = SIXSTEP_parameters.NUMPOLESPAIRS; 
 SIXSTEP_parameters.SYSCLK_frequency = HAL_RCC_GetSysClockFreq();
 
 MC_SixStep_HF_TIMx_SetDutyCycle_CH1(0);
 MC_SixStep_HF_TIMx_SetDutyCycle_CH2(0);
 MC_SixStep_HF_TIMx_SetDutyCycle_CH3(0); 

 SIXSTEP_parameters.Regular_channel[1] = ADC_Bemf_CH1;   /*BEMF1*/
 SIXSTEP_parameters.Regular_channel[2] = ADC_Bemf_CH2;   /*BEMF2*/
 SIXSTEP_parameters.Regular_channel[3] = ADC_Bemf_CH3;   /*BEMF3*/
 SIXSTEP_parameters.ADC_SEQ_CHANNEL[0] = ADC_CH_1;       /*CURRENT*/
 SIXSTEP_parameters.ADC_SEQ_CHANNEL[1] = ADC_CH_2;       /*SPEED*/
 SIXSTEP_parameters.ADC_SEQ_CHANNEL[2] = ADC_CH_3;       /*VBUS*/
 SIXSTEP_parameters.ADC_SEQ_CHANNEL[3] = ADC_CH_4;       /*TEMP*/
 
 SIXSTEP_parameters.step_position = 0;
 SIXSTEP_parameters.demagn_counter = 0; 
 SIXSTEP_parameters.ALIGN_OK = FALSE;
 SIXSTEP_parameters.VALIDATION_OK = 0;
 SIXSTEP_parameters.ARR_OK = 0;
 SIXSTEP_parameters.speed_fdbk_filtered = 0;
 SIXSTEP_parameters.Integral_Term_sum = 0;
 SIXSTEP_parameters.Current_Reference = 0;
 SIXSTEP_parameters.Ramp_Start = 0;
 SIXSTEP_parameters.RUN_Motor = 0;
 SIXSTEP_parameters.speed_fdbk = 0;
 SIXSTEP_parameters.BEMF_OK = FALSE;
 SIXSTEP_parameters.CL_READY = FALSE;
 SIXSTEP_parameters.SPEED_VALIDATED = FALSE;
 SIXSTEP_parameters.BEMF_Tdown_count = 0;   /* Reset of the Counter to detect Stop motor condition when a stall condition occurs*/
 
 uwTick = 0;
 index_motor_run = 0;                       
 test_motor_run = 1;                        
 T_single_step = 0;                      
 T_single_step_first_value = 0;          
 delta = 0;                               
 Time_vector_tmp = 0;                  
 Time_vector_prev_tmp = 0;              
 Mech_Speed_RPM = 0;
 El_Speed_Hz = 0;
 index_adc_chn = 0;
 mech_accel_hz = 0;           
 constant_k = 0;               
 ARR_LF = 0;
 index_array = 1;  
 Enable_start_button = TRUE; 
 index_ARR_step = 1;
 n_zcr_startup = 0;
 cnt_bemf_event = 0;
 startup_bemf_failure = 0;
 speed_fdbk_error = 0;
 
 index_align = 1;
 speed_sum_sp_filt = 0;
 speed_sum_pot_filt = 0; 
 index_pot_filt = 1;  
 potent_filtered = 0;
 Tick_cnt = 0;   
 counter_ARR_Bemf = 0;
 constant_multiplier_tmp = 0;
 
 HFBufferIndex =0;
 for(uint16_t i = 0; i < HFBUFFERSIZE;i++)
 {
   HFBuffer[i]=0;
 }
 
 for(uint16_t i = 0; i < FILTER_DEEP;i++)
 {
   speed_tmp_array[i] = 0;
   speed_tmp_buffer[i]= 0;   
 } 
 array_completed = FALSE;
 buffer_completed = FALSE;
 
 if(PI_parameters.Reference < 0)  
 {
   SIXSTEP_parameters.step_position = 1;
 }
  target_speed = TARGET_SPEED;
  MC_Set_PI_param(&PI_parameters); 
  MC_SixStep_Current_Reference_Start(); 
  MC_SixStep_Current_Reference_Setvalue(SIXSTEP_parameters.Ireference); 
  index_startup_motor = 1;
  MC_SixStep_Ramp_Motor_calc(); 
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_Ramp_Motor_calc    MC_SixStep_Ramp_Motor_calc
  *  @{
    * @brief Calculate the acceleration profile step by step for motor during start-up 
    * @retval None
*/
void MC_SixStep_Ramp_Motor_calc()
{
  uint32_t constant_multiplier = 100;
  uint32_t constant_multiplier_2 = 4000000000;  
  
  if(index_startup_motor == 1)
  { 
    mech_accel_hz = SIXSTEP_parameters.ACCEL * Rotor_poles_pairs / 60; 
    constant_multiplier_tmp = (uint64_t)constant_multiplier*(uint64_t)constant_multiplier_2;
    constant_k = constant_multiplier_tmp/(3*mech_accel_hz);    
    MC_SixStep_Current_Reference_Setvalue(SIXSTEP_parameters.Ireference); 
    Time_vector_prev_tmp = 0;
  }
  if(index_startup_motor < NUMBER_OF_STEPS)  
  {
    Time_vector_tmp = ((uint64_t) 1000 * (uint64_t)1000 * (uint64_t) MCM_Sqrt(((uint64_t)index_startup_motor * (uint64_t)constant_k)))/632455;   
    delta = Time_vector_tmp - Time_vector_prev_tmp;
    if(index_startup_motor==1)
    { 
      T_single_step_first_value = (2 * 3141)*delta/1000;
      SIXSTEP_parameters.ARR_value = (uint32_t)(65535); 
    }
    else 
    {
      T_single_step = (2 * 3141)*delta/1000;
      SIXSTEP_parameters.ARR_value = (uint32_t)(65535 * T_single_step)/(T_single_step_first_value);
    }
  }
  else index_startup_motor=1;
  
  if(index_startup_motor==1)
  {
    SIXSTEP_parameters.prescaler_value = (((SIXSTEP_parameters.SYSCLK_frequency/1000000)*T_single_step_first_value)/65535) - 1;      
  }
  if(SIXSTEP_parameters.STATUS != ALIGNMENT && SIXSTEP_parameters.STATUS != START)
  {
    index_startup_motor++;     
  }
  else Time_vector_tmp = 0;
  Time_vector_prev_tmp =  Time_vector_tmp;
  
}

/**
  * @} 
  */


/**
  * @brief  It calculates the square root of a non-negative s64. 
  *   It returns 0 for negative s64.
  * @param  Input uint64_t number
  * @retval int32_t Square root of Input (0 if Input<0)
  */
uint64_t MCM_Sqrt(uint64_t wInput)
{
  uint8_t biter = 0u;
  uint64_t wtemproot;
  uint64_t wtemprootnew;
  
    if (wInput <= (uint64_t)((uint64_t)2097152<<shift_n_sqrt))  
    {
      wtemproot = (uint64_t)((uint64_t)128<<shift_n_sqrt);  
    }
    else
    {
      wtemproot = (uint64_t)((uint64_t)8192<<shift_n_sqrt);  
    }
    
    do
    {
      wtemprootnew = (wtemproot + wInput/wtemproot)>>1;
      if (wtemprootnew == wtemproot)
      {
        biter = (shift_n_sqrt-1);
      }
      else
      {
        biter ++;
        wtemproot = wtemprootnew;
      }
    }
    while (biter < (shift_n_sqrt-1));
  
  return (wtemprootnew); 
}


/** @defgroup MC_SixStep_ARR_step    MC_SixStep_ARR_step
  *  @{
    * @brief Generate the ARR value for Low Frequency TIM during start-up
    * @retval None
*/

void MC_SixStep_ARR_step()
{ 
   
 if(SIXSTEP_parameters.ALIGNMENT == FALSE) 
 {    
   SIXSTEP_parameters.ALIGNMENT = TRUE;   
 }
 if(SIXSTEP_parameters.ALIGN_OK == TRUE)
 {
  if(PI_parameters.Reference >= 0) 
   {
     if(SIXSTEP_parameters.VALIDATION_OK != TRUE)
     {  
      SIXSTEP_parameters.STATUS = STARTUP;
      MC_SixStep_Ramp_Motor_calc();       
      if(index_ARR_step < SIXSTEP_parameters.numberofitemArr)
      {
        LF_TIMx.Init.Period = SIXSTEP_parameters.ARR_value;
        LF_TIMx.Instance->ARR = (uint32_t)LF_TIMx.Init.Period;
        index_ARR_step++;
      } 
      else if(SIXSTEP_parameters.ARR_OK == 0)
      {  
       index_ARR_step = 1; 
       SIXSTEP_parameters.ACCEL>>=1;  
       if(SIXSTEP_parameters.ACCEL < MINIMUM_ACC)
       {
         SIXSTEP_parameters.ACCEL = MINIMUM_ACC;
       }       
       MC_StopMotor(); 
       SIXSTEP_parameters.STATUS = STARTUP_FAILURE;
      }
     }
     else 
     { 
       SIXSTEP_parameters.ARR_OK = 1;
       index_startup_motor = 1;       
       index_ARR_step = 1;   
     } 
   }
  else 
    {
     if(SIXSTEP_parameters.VALIDATION_OK != TRUE)
      {  
      SIXSTEP_parameters.STATUS = STARTUP;
      MC_SixStep_Ramp_Motor_calc();       
      if(index_ARR_step < SIXSTEP_parameters.numberofitemArr)
      {
        LF_TIMx.Init.Period = SIXSTEP_parameters.ARR_value;
        LF_TIMx.Instance->ARR = (uint32_t)LF_TIMx.Init.Period;
        index_ARR_step++;
      } 
      else if(SIXSTEP_parameters.ARR_OK==0)
      {  
         index_ARR_step = 1;   
         SIXSTEP_parameters.ACCEL>>=1; 
         if(SIXSTEP_parameters.ACCEL < MINIMUM_ACC)
         {
           SIXSTEP_parameters.ACCEL = MINIMUM_ACC;
         }         
         MC_StopMotor();  
         SIXSTEP_parameters.STATUS = STARTUP_FAILURE;  
      }
      }
      else 
       { 
         SIXSTEP_parameters.ARR_OK = 1;
         index_startup_motor = 1;       
         index_ARR_step = 1;   
       } 
   }
 } 
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_Alignment    MC_SixStep_Alignment
  *  @{
    * @brief Generate the motor alignment
    * @retval None
*/

void MC_SixStep_Alignment()
{
   SIXSTEP_parameters.step_position = 6;
   LF_TIMx.Init.Period = SIXSTEP_parameters.ARR_value;
   LF_TIMx.Instance->ARR = (uint32_t)LF_TIMx.Init.Period;
   SIXSTEP_parameters.STATUS = ALIGNMENT;    
   MC_SixStep_Speed_Val_target_potentiometer();
   index_align++;
   if(index_align >= TIME_FOR_ALIGN+1) 
    { 
      SIXSTEP_parameters.ALIGN_OK = TRUE;
      SIXSTEP_parameters.STATUS = STARTUP;
      index_startup_motor = 1;      
      MC_SixStep_Ramp_Motor_calc();    
      LF_TIMx.Init.Prescaler = SIXSTEP_parameters.prescaler_value;
      LF_TIMx.Instance->PSC = LF_TIMx.Init.Prescaler;         
      index_align = 0;
    }
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_Speed_Val_target_potentiometer    MC_SixStep_Speed_Val_target_potentiometer
  *  @{
    * @brief Calculate the Motor Speed validation threshold according with the potentiometer value
    * @retval None
*/

void MC_SixStep_Speed_Val_target_potentiometer()
{  
  target_speed = SIXSTEP_parameters.ADC_Regular_Buffer[1] * MAX_POT_SPEED/ 4096;  

  if(target_speed < MIN_POT_SPEED)
    target_speed = MIN_POT_SPEED;
   
  if(target_speed > (MAX_POT_SPEED/VAL_POT_SPEED_DIV)) 
    target_speed = (MAX_POT_SPEED/VAL_POT_SPEED_DIV);
}
/**
  * @} 
  */

/** @defgroup MC_SixStep_Speed_Potentiometer    MC_SixStep_Speed_Potentiometer
  *  @{
    * @brief Calculate the potentiometer value to set the Motor Speed
    * @retval None
*/

void MC_SixStep_Speed_Potentiometer()
{  
  uint16_t i=0;
  uint32_t sum = 0;
  uint16_t mean = 0;
  uint16_t max = 0;
  for (i = 0; i < HFBUFFERSIZE; i++)
  {
    uint16_t val = HFBuffer[i];
    sum += val;
    if (val > max)
    {
      max = val;
    }
  }
  sum -= max;
  mean = sum / (HFBUFFERSIZE - 1);  
  
  SIXSTEP_parameters.Speed_Ref_filtered = MC_Potentiometer_filter(mean);
  
}

/**
  * @} 
  */

/** @defgroup MC_Set_PI_param    MC_Set_PI_param
  *  @{
    * @brief Set all parameters for PI regulator
    * @param  PI_PARAM
    * @retval None
*/

void MC_Set_PI_param(SIXSTEP_PI_PARAM_InitTypeDef_t *PI_PARAM)
{
  if(SIXSTEP_parameters.CW_CCW == 0)
    PI_PARAM->Reference = target_speed;            
  else
    PI_PARAM->Reference = -target_speed;       
  
  PI_PARAM->Kp_Gain = SIXSTEP_parameters.KP;   
  PI_PARAM->Ki_Gain = SIXSTEP_parameters.KI;      

  PI_PARAM->Lower_Limit_Output = LOWER_OUT_LIMIT;       
  PI_PARAM->Upper_Limit_Output = UPPER_OUT_LIMIT;      
  PI_PARAM->Max_PID_Output =  FALSE; 
  PI_PARAM->Min_PID_Output =  FALSE;
}

/**
  * @} 
  */

/** @defgroup MC_PI_Controller    MC_PI_Controller
  *  @{
    * @brief Compute the PI output for the Current Reference
    * @param  PI_PARAM PI parameters structure
    * @param  speed_fdb motor_speed_value
    * @retval int16_t Currente reference 
*/

int16_t MC_PI_Controller(SIXSTEP_PI_PARAM_InitTypeDef_t *PI_PARAM, int16_t speed_fdb)
{
  int32_t wProportional_Term=0, wIntegral_Term=0, wOutput_32=0,wIntegral_sum_temp=0;
  int32_t Error =0;
    
  Error = (PI_PARAM->Reference - speed_fdb);
  
  /* Proportional term computation*/
  wProportional_Term = PI_PARAM->Kp_Gain * Error;
    
  /* Integral term computation */
  if (PI_PARAM->Ki_Gain == 0)
  {
    SIXSTEP_parameters.Integral_Term_sum = 0;
  }
  else
  { 
    wIntegral_Term = PI_PARAM->Ki_Gain * Error;
    wIntegral_sum_temp = SIXSTEP_parameters.Integral_Term_sum + wIntegral_Term;
    SIXSTEP_parameters.Integral_Term_sum = wIntegral_sum_temp;
  }
  
  if(SIXSTEP_parameters.Integral_Term_sum> KI_DIV * PI_PARAM->Upper_Limit_Output)
     SIXSTEP_parameters.Integral_Term_sum = KI_DIV* PI_PARAM->Upper_Limit_Output;
 
  if(SIXSTEP_parameters.Integral_Term_sum<-KI_DIV* PI_PARAM->Upper_Limit_Output)
     SIXSTEP_parameters.Integral_Term_sum = -KI_DIV* PI_PARAM->Upper_Limit_Output;
  
 /* WARNING: the below instruction is not MISRA compliant, user should verify
             that Cortex-M3 assembly instruction ASR (arithmetic shift right) 
             is used by the compiler to perform the shifts (instead of LSR 
             logical shift right)*/ 
             
  wOutput_32 = (wProportional_Term/KP_DIV) + (SIXSTEP_parameters.Integral_Term_sum/KI_DIV);
  
  if(PI_PARAM->Reference>0)
  {  
   if (wOutput_32 > PI_PARAM->Upper_Limit_Output)
    {
     wOutput_32 = PI_PARAM->Upper_Limit_Output;                                         
    }
   else if (wOutput_32 < PI_PARAM->Lower_Limit_Output)
    {
     wOutput_32 = PI_PARAM->Lower_Limit_Output;
    }
  }
  else 
{
   if (wOutput_32 < (- PI_PARAM->Upper_Limit_Output) )
    {
     wOutput_32 = - (PI_PARAM->Upper_Limit_Output);                                             
    }
   else if (wOutput_32 > (-PI_PARAM->Lower_Limit_Output))
    {
     wOutput_32 = (-PI_PARAM->Lower_Limit_Output);
    }    
  }
  return((int16_t)(wOutput_32));        
}

/**
  * @} 
  */


/** @defgroup MC_Task_Speed    MC_Task_Speed
  *  @{
    * @brief Main task: Speed Loop with PI regulator
    * @retval None
*/
void MC_Task_Speed()
{
 
 if(dac_status == TRUE)
 {  
   SET_DAC_value(SIXSTEP_parameters.speed_fdbk_filtered);   
 }
  
 if((SIXSTEP_parameters.speed_fdbk_filtered > (target_speed) || SIXSTEP_parameters.speed_fdbk_filtered < (-target_speed)) && SIXSTEP_parameters.VALIDATION_OK !=TRUE)
 { 
   SIXSTEP_parameters.STATUS = VALIDATION;   
   SIXSTEP_parameters.SPEED_VALIDATED = TRUE;
 } 
 
 if(SIXSTEP_parameters.SPEED_VALIDATED == TRUE && SIXSTEP_parameters.BEMF_OK == TRUE && SIXSTEP_parameters.CL_READY != TRUE)
 {
   SIXSTEP_parameters.CL_READY = TRUE;  
 }
   
 if(SIXSTEP_parameters.VALIDATION_OK == TRUE)
 {    
 /*****************************************************************************/ 
  SIXSTEP_parameters.STATUS = RUN; 
 /*****************************************************************************/ 
    
    if(PI_parameters.Reference>=0)
    {
      SIXSTEP_parameters.Current_Reference = (uint16_t)MC_PI_Controller(&PI_parameters,(int16_t)SIXSTEP_parameters.speed_fdbk_filtered);  
    }
    else
    {
      SIXSTEP_parameters.Current_Reference = (uint16_t)(-MC_PI_Controller(&PI_parameters,(int16_t)SIXSTEP_parameters.speed_fdbk_filtered));      
    }
                 
    MC_SixStep_Current_Reference_Setvalue(SIXSTEP_parameters.Current_Reference);
  // SIXSTEP_parameters.pulse_value=SIXSTEP_parameters.Current_Reference;

 }
 MC_Bemf_Delay();
}
     
/**
  * @} 
  */

/** @defgroup MC_Set_Speed    MC_Set_Speed
  *  @{
    * @brief Set the new motor speed value
    * @param  speed_value:  set new motor speed
    * @retval None
*/
void MC_Set_Speed(uint16_t speed_value)
{
 
#if (POTENTIOMETER == 1)
  uint8_t change_target_speed = 0;
  int16_t reference_tmp = 0;
  
  if (SIXSTEP_parameters.Speed_Ref_filtered > SIXSTEP_parameters.Speed_target_ramp)
  {
    if ((SIXSTEP_parameters.Speed_Ref_filtered - SIXSTEP_parameters.Speed_target_ramp) > ADC_SPEED_TH) 
    {
      change_target_speed = 1;
    }
    else
    {
      /* Not change target speed because less than threshold */
    }
  }
  else
  {
    if ((SIXSTEP_parameters.Speed_target_ramp - SIXSTEP_parameters.Speed_Ref_filtered) > ADC_SPEED_TH)
    {
      change_target_speed = 1;
    }
    else
    {
      /* Not change target speed because less than threshold */
    }
  }
  if (change_target_speed == 1)
  {
    SIXSTEP_parameters.Speed_target_ramp = SIXSTEP_parameters.Speed_Ref_filtered;
        
    if(SIXSTEP_parameters.CW_CCW == 0)
    {
      reference_tmp = SIXSTEP_parameters.Speed_Ref_filtered * MAX_POT_SPEED / 4096;
       if(reference_tmp <= MIN_POT_SPEED)
       {
         PI_parameters.Reference = MIN_POT_SPEED;
       }
       else 
       {
         PI_parameters.Reference =  reference_tmp;
       }
    }
    else
    {
      reference_tmp = -(SIXSTEP_parameters.Speed_Ref_filtered * MAX_POT_SPEED / 4096);
       if(reference_tmp >=- MIN_POT_SPEED)
       {
         PI_parameters.Reference = -MIN_POT_SPEED;
       }
       else 
       {
         PI_parameters.Reference=  reference_tmp;
       }      
    }

  }
#else
   if(speed_value != 0)
    PI_parameters.Reference = speed_value;
#endif
  
}

/**
  * @} 
  */


/** @defgroup MC_Bemf_Delay    MC_Bemf_Delay
  *  @{
    * @brief Take the delay time after each new 6-step commutation 
    * @retval None
*/
void MC_Bemf_Delay()
{
   Bemf_delay_calc(); 
} 
/**
  * @} 
  */

/** @defgroup MC_StartMotor    MC_StartMotor
  *  @{
    * @brief Start the Motor
    * @retval None
*/
void MC_StartMotor()
{ 
  uwTick = 0;
  SIXSTEP_parameters.STATUS = START;
  HAL_TIM_Base_Start_IT(&LF_TIMx);  
  HAL_ADC_Start_IT(&ADCx);
  SIXSTEP_parameters.RUN_Motor = 1;
  BSP_X_NUCLEO_FAULT_LED_ON();
  if(dac_status == TRUE)
  {
   START_DAC();
  }
}
/**
  * @} 
  */

/** @defgroup MC_StopMotor    MC_StopMotor
  *  @{
    * @brief Stop the Motor
    * @retval None  
*/
void MC_StopMotor()
{     
  uwTick = 0;  
  SIXSTEP_parameters.STATUS = STOP;
  SIXSTEP_parameters.RUN_Motor = 0;
  MC_SixStep_Stop_PWM_driving();
  HF_TIMx.Instance->CR1 &= ~(TIM_CR1_CEN);
  HF_TIMx.Instance->CNT = 0;  
  MC_SixStep_DisableInput_CH1_D_CH2_D_CH3_D();
  HAL_TIM_Base_Stop_IT(&LF_TIMx);  
  HAL_ADC_Stop_IT(&ADCx);
  MC_SixStep_Current_Reference_Stop();
  BSP_X_NUCLEO_FAULT_LED_OFF();
  MC_SixStep_RESET();
}

/**
  * @} 
  */

/** @defgroup MC_GetElSpeedHz    MC_GetElSpeedHz
  *  @{
    * @brief Get the Eletrical Motor Speed from ARR value of LF TIM
    * @retval int32_t Return the electrical motor speed
*/
int32_t MC_GetElSpeedHz()
{   
   if(__HAL_TIM_GetAutoreload(&LF_TIMx) != 0xFFFF)
   {
     uint16_t prsc = LF_TIMx.Instance->PSC;
     El_Speed_Hz = (int32_t)((SIXSTEP_parameters.SYSCLK_frequency)/(prsc))/(__HAL_TIM_GetAutoreload(&LF_TIMx)*6); 
   }
   else 
     El_Speed_Hz = 0;
   if(PI_parameters.Reference<0)
    return (-El_Speed_Hz);
   else 
    return (El_Speed_Hz);
}
/**
  * @} 
  */

/** @defgroup MC_GetMechSpeedRPM    MC_GetMechSpeedRPM
  *  @{
    * @brief Get the Mechanical Motor Speed (RPM)
    * @retval int32_t Return the mechanical motor speed (RPM
*/

int32_t MC_GetMechSpeedRPM()
{  
  Mech_Speed_RPM = (int32_t)(MC_GetElSpeedHz() *  60 / Rotor_poles_pairs);
 return (Mech_Speed_RPM);
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_Init_main_data    MC_SixStep_Init_main_data
  *  @{
    * @brief Init the main variables for motor driving from MC_SixStep_param.h
    * @retval None
*/

void MC_SixStep_Init_main_data()
{ 
  SIXSTEP_parameters.Ireference = STARTUP_CURRENT_REFERENCE;
  SIXSTEP_parameters.NUMPOLESPAIRS = NUM_POLE_PAIRS;
  SIXSTEP_parameters.ACCEL = ACC;
  SIXSTEP_parameters.KP = KP_GAIN;   
  SIXSTEP_parameters.KI = KI_GAIN;
  SIXSTEP_parameters.CW_CCW = DIRECTION;
  SIXSTEP_parameters.Potentiometer = POTENTIOMETER;  
}

/**
  * @} 
  */


/** @defgroup MC_SixStep_INIT    MC_SixStep_INIT
  *  @{
    * @brief Initialitation function for SixStep library
    * @retval None
*/

void MC_SixStep_INIT()
{
    MC_SixStep_Nucleo_Init();
    SIXSTEP_parameters.HF_TIMx_CCR  = HF_TIMx.Instance->HF_TIMx_CCR1;
    SIXSTEP_parameters.HF_TIMx_ARR  = HF_TIMx.Instance->ARR;
    SIXSTEP_parameters.HF_TIMx_PSC  = HF_TIMx.Instance->PSC;  
    SIXSTEP_parameters.LF_TIMx_ARR  = LF_TIMx.Instance->ARR;
    SIXSTEP_parameters.LF_TIMx_PSC  = LF_TIMx.Instance->PSC;
    
//    MC_SixStep_Current_Reference_Start(); 
    MC_SixStep_Current_Reference_Setvalue(SIXSTEP_parameters.Ireference); 

  #ifdef UART_COMM  
    SIXSTEP_parameters.Button_ready = FALSE;
    MC_UI_INIT();               /*!<  Start the UART Communication Task*/
  #endif    
    
    MC_SixStep_Init_main_data(); 

  #ifndef UART_COMM
    SIXSTEP_parameters.Button_ready = TRUE;
  #endif
    MC_SixStep_RESET();
}
      
/**
  * @} 
  */


/** @defgroup MC_TIMx_SixStep_timebase    MC_TIMx_SixStep_timebase
  *  @{
    * @brief Low Frequency Timer Callback - Call the next step and request the filtered speed value
    * @retval None
*/

void MC_TIMx_SixStep_timebase()
{
  MC_SixStep_NEXT_step();                                                       /*Change STEP number  */
  if(SIXSTEP_parameters.ARR_OK == 0) 
  {
    MC_SixStep_ARR_step();                                                       /*BASE TIMER - ARR modification for STEP frequency changing */ 
  }
  
  MC_Speed_Filter();                                                            /*Calculate SPEED filtered  */ 
}

/**
  * @} 
  */

/** @defgroup MC_Speed_Filter    MC_Speed_Filter
  *  @{
    * @brief Calculate the speed filtered
    * @retval None
*/

void MC_Speed_Filter()
{  
  if(array_completed == FALSE)
  {
     speed_tmp_array[index_array] = SIXSTEP_parameters.speed_fdbk;    
     speed_sum_sp_filt = 0;
     for(uint16_t i = 1; i <= index_array;i++)
     {
       speed_sum_sp_filt = speed_sum_sp_filt + speed_tmp_array[i];
     }
     SIXSTEP_parameters.speed_fdbk_filtered = speed_sum_sp_filt/index_array;
     index_array++;
     
      if(index_array >= FILTER_DEEP) 
       {
         index_array = 1;
         array_completed = TRUE;
       }
  }  
  else
  {
     index_array++;
     if(index_array >= FILTER_DEEP) 
      index_array = 1;
     
      speed_sum_sp_filt = 0;
      speed_tmp_array[index_array] = SIXSTEP_parameters.speed_fdbk;   
      for(uint16_t i = 1; i < FILTER_DEEP;i++)
       {
        speed_sum_sp_filt = speed_sum_sp_filt + speed_tmp_array[i];
       }      
      SIXSTEP_parameters.speed_fdbk_filtered = speed_sum_sp_filt/(FILTER_DEEP-1);
  }  
}

/**
  * @} 
  */

/** @defgroup MC_Potentiometer_filter    MC_Potentiometer_filter
  *  @{
    * @brief Calculate the filtered potentiometer value 
    * @retval uint16_t Return the filtered potentiometer value 
*/

uint16_t MC_Potentiometer_filter(uint16_t potentiometer_value)
{ 
  if(buffer_completed == FALSE)
  {
     speed_tmp_buffer[index_pot_filt] = potentiometer_value;    
     speed_sum_pot_filt = 0;
     for(uint16_t i = 1; i <= index_pot_filt;i++)
     {
       speed_sum_pot_filt = speed_sum_pot_filt + speed_tmp_buffer[i];
     }
     potent_filtered = speed_sum_pot_filt/index_pot_filt;
     index_pot_filt++;
     
      if(index_pot_filt >= FILTER_DEEP) 
       {
         index_pot_filt = 1;
         buffer_completed = TRUE;
       }
  }  
  else
  {
     index_pot_filt++;
     if(index_pot_filt >= FILTER_DEEP)
     {
      index_pot_filt = 1;
     }
     
     speed_sum_pot_filt = 0;
     speed_tmp_buffer[index_pot_filt] = potentiometer_value;
     uint16_t speed_max = 0;
     for(uint16_t i = 1; i < FILTER_DEEP;i++)
     {
       uint16_t val = speed_tmp_buffer[i];
       if (val > speed_max)
       {
         speed_max = val;
       }
       speed_sum_pot_filt += val;
     }
     speed_sum_pot_filt -= speed_max;
     potent_filtered = speed_sum_pot_filt/(FILTER_DEEP-2);
  }
  if(potent_filtered==0) potent_filtered = 1;
  
return(potent_filtered);
}

/**
  * @} 
  */

/** @defgroup MC_SysTick_SixStep_MediumFrequencyTask    MC_SysTick_SixStep_MediumFrequencyTask
  *  @{
    * @brief Systick Callback - Call the Speed loop
    * @retval None
*/

void MC_SysTick_SixStep_MediumFrequencyTask()
{
 if(SIXSTEP_parameters.ALIGNMENT == TRUE && SIXSTEP_parameters.ALIGN_OK == FALSE) 
 {         
   MC_SixStep_Alignment();
 }  
  
#ifdef UART_COMM  
 if(UART_FLAG_RECEIVE == TRUE)    UART_Communication_Task();
#endif
 
#ifdef DEMOMODE
  index_motor_run++;
  if(index_motor_run >= DEMO_START_TIME && test_motor_run == 0)
  {
    MC_StopMotor();
    index_motor_run=0;
    test_motor_run=1;
  }
  if(index_motor_run >= DEMO_STOP_TIME && test_motor_run == 1)
  {
    MC_StartMotor();
    test_motor_run = 0;
    index_motor_run=0;
  }    
#endif
  
 if(SIXSTEP_parameters.VALIDATION_OK == TRUE && SIXSTEP_parameters.Potentiometer  == TRUE)  
  {
    MC_SixStep_Speed_Potentiometer();
  }
 /* Push button delay time to avoid double command */ 
  if(HAL_GetTick() == BUTTON_DELAY && Enable_start_button != TRUE)
  {
    Enable_start_button = TRUE; 
  }
    
 /* SIXSTEP_parameters.Speed_Loop_Time x 1msec */ 
 if(Tick_cnt >= SIXSTEP_parameters.Speed_Loop_Time)
  { 
   if(SIXSTEP_parameters.STATUS != SPEEDFBKERROR) 
   {
     MC_Task_Speed();
   }     
   SIXSTEP_parameters.MediumFrequencyTask_flag = TRUE;       
   if(SIXSTEP_parameters.VALIDATION_OK == TRUE)
    {  
       MC_Set_Speed(0);
    }   
   Tick_cnt=0; 
  }
  else Tick_cnt++;
     
  if(startup_bemf_failure == 1)
  {
    SIXSTEP_parameters.ACCEL>>=1;
    if(SIXSTEP_parameters.ACCEL < MINIMUM_ACC)
    {
      SIXSTEP_parameters.ACCEL = MINIMUM_ACC;
    }  
    MC_StopMotor();
    cnt_bemf_event = 0;    
    SIXSTEP_parameters.STATUS = STARTUP_BEMF_FAILURE;  
  }
  
  if(speed_fdbk_error == 1)
  {  
    MC_StopMotor();
    SIXSTEP_parameters.STATUS = SPEEDFBKERROR; 
  }
}

/**
  * @} 
  */

/** @defgroup MC_SixStep_ARR_Bemf    MC_SixStep_ARR_Bemf
  *  @{
    * @brief Calculate the new Autoreload value (ARR) for Low Frequency timer
    * @retval None
*/

void MC_SixStep_ARR_Bemf(uint8_t up_bemf)
{

 if(SIXSTEP_parameters.status_prev != SIXSTEP_parameters.step_position)
  { 
   if(SIXSTEP_parameters.SPEED_VALIDATED == TRUE) 
   {
     if(GPIO_ZERO_CROSS == 1)
     {
       HAL_GPIO_TogglePin(GPIO_PORT_ZCR,GPIO_CH_ZCR);         
     } 
     if(cnt_bemf_event> BEMF_CNT_EVENT_MAX)
     {
       startup_bemf_failure = 1;  
     }
     
     if(up_bemf == 1 && SIXSTEP_parameters.BEMF_OK !=TRUE)
     {
       n_zcr_startup++;
       cnt_bemf_event = 0;      
     }
     else if(SIXSTEP_parameters.BEMF_OK !=TRUE)
     {
       cnt_bemf_event++;
     }
     
     if(n_zcr_startup>= NUMBER_ZCR && SIXSTEP_parameters.BEMF_OK !=TRUE )
     {
      SIXSTEP_parameters.BEMF_OK = TRUE;    
      n_zcr_startup = 0;
     }
   }
     SIXSTEP_parameters.status_prev = SIXSTEP_parameters.step_position;     
   
     if(SIXSTEP_parameters.VALIDATION_OK == 1)
     {                
      counter_ARR_Bemf = __HAL_TIM_GetCounter(&LF_TIMx);
      __HAL_TIM_SetAutoreload(&LF_TIMx,(counter_ARR_Bemf+ARR_LF/2));  
     }
  }

}

/**
  * @} 
  */

/** @defgroup MC_ADCx_SixStep_Bemf    MC_ADCx_SixStep_Bemf
  *  @{
    * @brief Compute the zero crossing detection
    * @retval None
*/

void MC_ADCx_SixStep_Bemf()
{  

  if(__HAL_TIM_DIRECTION_STATUS(&HF_TIMx)) 
  {   
  HAL_GPIO_WritePin(GPIO_PORT_COMM,GPIO_CH_COMM,GPIO_PIN_SET);      
     /* UP-counting direction started */
     /* GET the ADC value (PHASE CURRENT)*/
   if(SIXSTEP_parameters.STATUS != START && SIXSTEP_parameters.STATUS != ALIGNMENT)     
   {
     switch (SIXSTEP_parameters.step_position)
     { 
      case 6:
      {  
       if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {  
        SIXSTEP_parameters.ADC_BUFFER[1] = HAL_ADC_GetValue(&ADCx);  
        if(PI_parameters.Reference>=0)
        {  
         if(SIXSTEP_parameters.ADC_BUFFER[1]> SIXSTEP_parameters.ADC_BEMF_threshold_UP)
         {      
            MC_SixStep_ARR_Bemf(1);
            SIXSTEP_parameters.BEMF_Tdown_count = 0;   
         }
        }
        else
        {
         if(SIXSTEP_parameters.ADC_BUFFER[1]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN)    
         {
            MC_SixStep_ARR_Bemf(0);
         }
        }
       }
       else SIXSTEP_parameters.demagn_counter++;
      }  
      break;
      case 3:
      { 
      if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {  
        SIXSTEP_parameters.ADC_BUFFER[1] = HAL_ADC_GetValue(&ADCx);  
        if(PI_parameters.Reference>=0)
        {          
        if(SIXSTEP_parameters.ADC_BUFFER[1]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN) 
        { 
            MC_SixStep_ARR_Bemf(0);
        }
        }
        else
        {
         if(SIXSTEP_parameters.ADC_BUFFER[1]> SIXSTEP_parameters.ADC_BEMF_threshold_UP) 
         {   
          MC_SixStep_ARR_Bemf(1);
          SIXSTEP_parameters.BEMF_Tdown_count = 0;
        }
        }
       }
       else SIXSTEP_parameters.demagn_counter++;
      }  
      break;   
      case 5:
      {
       if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {          
        SIXSTEP_parameters.ADC_BUFFER[2] = HAL_ADC_GetValue(&ADCx); 
       if(PI_parameters.Reference>=0)
        {          
        if(SIXSTEP_parameters.ADC_BUFFER[2]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN) 
        {            
            MC_SixStep_ARR_Bemf(0);  
        }
        }
       else
       {
        if(SIXSTEP_parameters.ADC_BUFFER[2]> SIXSTEP_parameters.ADC_BEMF_threshold_UP)  
        {            
          MC_SixStep_ARR_Bemf(1);
          SIXSTEP_parameters.BEMF_Tdown_count = 0;
        }          
       }
       }
       else SIXSTEP_parameters.demagn_counter++;        
      }  
      break;     
      case 2:
      {  
       if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {          
        SIXSTEP_parameters.ADC_BUFFER[2] = HAL_ADC_GetValue(&ADCx);  
       if(PI_parameters.Reference>=0)
        {           
        if(SIXSTEP_parameters.ADC_BUFFER[2]> SIXSTEP_parameters.ADC_BEMF_threshold_UP) 
        {           
          MC_SixStep_ARR_Bemf(1);
          SIXSTEP_parameters.BEMF_Tdown_count = 0; 
        }
       }
       else
       {
        if(SIXSTEP_parameters.ADC_BUFFER[2]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN) 
        {            
              MC_SixStep_ARR_Bemf(0);
        }
       }
      }
      else SIXSTEP_parameters.demagn_counter++; 
      }  
      break; 
      case 4:
      {     
       if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {         
        SIXSTEP_parameters.ADC_BUFFER[3] = HAL_ADC_GetValue(&ADCx);  
       if(PI_parameters.Reference>=0)
        {            
        if(SIXSTEP_parameters.ADC_BUFFER[3]> SIXSTEP_parameters.ADC_BEMF_threshold_UP) 
        {                
          MC_SixStep_ARR_Bemf(1);
          SIXSTEP_parameters.BEMF_Tdown_count = 0;    
        }
        }
       else
       {
        if(SIXSTEP_parameters.ADC_BUFFER[3]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN) 
        {            
            MC_SixStep_ARR_Bemf(0);
        }           
       }
       }
       else SIXSTEP_parameters.demagn_counter++;             
      }  
      break; 
      case 1:
      {
      if(SIXSTEP_parameters.demagn_counter >= SIXSTEP_parameters.demagn_value) 
       {   
        SIXSTEP_parameters.ADC_BUFFER[3] = HAL_ADC_GetValue(&ADCx);  
       if(PI_parameters.Reference>=0)
        {           
        if(SIXSTEP_parameters.ADC_BUFFER[3]< SIXSTEP_parameters.ADC_BEMF_threshold_DOWN) 
        { 
            MC_SixStep_ARR_Bemf(0);
        }
        }
       else
       {
        if(SIXSTEP_parameters.ADC_BUFFER[3]> SIXSTEP_parameters.ADC_BEMF_threshold_UP)  
        { 
          MC_SixStep_ARR_Bemf(1);
          SIXSTEP_parameters.BEMF_Tdown_count = 0;        
        }
       }
       }
       else SIXSTEP_parameters.demagn_counter++; 
      }  
      break;     
     }
    }
    /******************* SET ADC CHANNEL FOR SPEED/CURRENT/VBUS *******************/
    /* Set the channel for next ADC Regular reading */ 
    MC_SixStep_ADC_Channel(SIXSTEP_parameters.ADC_SEQ_CHANNEL[index_adc_chn]);
    /******************************************************************************/ 
    HAL_GPIO_WritePin(GPIO_PORT_COMM,GPIO_CH_COMM,GPIO_PIN_RESET);   
   }  
   else 
   {  
     /* Down-counting direction started */
     /* Set the channel for next ADC Regular reading */
    
    SIXSTEP_parameters.ADC_Regular_Buffer[index_adc_chn] = HAL_ADC_GetValue(&ADCx);
    
    if (index_adc_chn == 1)
    {
      HFBuffer[HFBufferIndex++] = HAL_ADC_GetValue(&ADCx);
      if (HFBufferIndex >= HFBUFFERSIZE)
      {
        HFBufferIndex = 0;
      }
    }
    index_adc_chn++; 
    if(index_adc_chn>3) index_adc_chn = 0;       
     MC_SixStep_ADC_Channel(SIXSTEP_parameters.CurrentRegular_BEMF_ch);   
   }
 
}

/**
  * @} 
  */

/** @defgroup MC_EXT_button_SixStep    MC_EXT_button_SixStep
  *  @{
    * @brief GPIO EXT Callback - Start or Stop the motor through the Blue push button on STM32Nucleo
    * @retval None
*/

void MC_EXT_button_SixStep()
{
  if(Enable_start_button == TRUE)
  {
    if(SIXSTEP_parameters.RUN_Motor == 0 && SIXSTEP_parameters.Button_ready == TRUE) 
    {    
      MC_StartMotor();
      Enable_start_button = FALSE;      
    }
    else 
    {     
      MC_StopMotor(); 
      Enable_start_button = FALSE;
    }
  }
}

/**
  * @} 
  */

/**
  * @brief This function is called to increment  a global variable "uwTick"
  *        used as application time base.
  * @note In the default implementation, this variable is incremented each 1ms
  *       in Systick ISR.
  * @note This function is declared as __weak to be overwritten in case of other 
  *       implementations in user file.
  * @retval None
  */
void HAL_IncTick(void)
{
  uwTick++;
}

/**
  * @brief  Povides a tick value in millisecond.
  * @note   The function is declared as __Weak  to be overwritten  in case of other 
  *       implementations in user file.
  * @retval tick value
  */
uint32_t HAL_GetTick(void)
{
  return uwTick;
}


/**
  * @}  end MC_6-STEP_LIB 
  */

/**
  * @}  end MIDDLEWARES
  */