File content as of revision 1:bc265521eb00:

/**
  ******************************************************************************
  * @file    STSPIN820.cpp
  * @author  STM
  * @version V1.0.0
  * @date    August 7th, 2017
  * @brief   STSPIN820 driver (fully integrated microstepping motor driver)
  * @note    (C) COPYRIGHT 2017 STMicroelectronics
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT(c) 2017 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.
  *
  ******************************************************************************
  */


/* Generated with STM32CubeTOO -----------------------------------------------*/


/* Revision ------------------------------------------------------------------*/
/*
    Repository:       http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
    Branch/Trunk/Tag: trunk
    Based on:         X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.c
    Revision:         0
*/


/* Includes ------------------------------------------------------------------*/

#include "STSPIN820.h"

/* Private function prototypes -----------------------------------------------*/

/** @defgroup STSPIN820_Private_Functions STSPIN820 Private Functions
  * @{
  */  
/* Methods -------------------------------------------------------------------*/

/** @defgroup STSPIN820_Exported_Variables STSPIN820 Exported Variables
  * @{
  */

/** @defgroup STSPIN820_Library_Functions STSPIN820 Library Functions
  * @{
  */

/******************************************************//**
 * @brief Return motor handle (pointer to the STSPIN820 motor driver structure)
 * @retval Pointer to the MOTOR_vt_t structure
 **********************************************************/
MOTOR_vt_t* STSPIN820::STSPIN820_GetMotorHandle(void)
{
  return (&STSPIN820_drv);
}

/******************************************************//**
 * @brief Start the STSPIN820 library
 * @param[in] pInit pointer to the initialization data
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_Init(void* pInit)
{
  device_instance++;
  
  /* Initialise the GPIOs */
  STSPIN820_Board_GpioInit();

  /* Initialise the timer used for the step clock and ------------------------*/
  /* the PWM for the reference voltage generation ----------------------------*/
  STSPIN820_Board_TimStckInit();
  STSPIN820_Board_PwmRefInit();

  if (pInit == 0)
  {
    /* Set all context variables to the predefined values */
    /* from STSPIN820_config.h */
    STSPIN820_SetDeviceParamsToPredefinedValues();
  }
  else
  {
    STSPIN820_SetDeviceParamsToGivenValues((STSPIN820_init_t*) pInit);
  }
}

/******************************************************//**
 * @brief Read id
 * @retval Id of the STSPIN820 Driver Instance
 **********************************************************/
uint16_t STSPIN820::STSPIN820_ReadId(void)
{
  return (device_instance);
}

/******************************************************//**
 * @brief Attach a user callback to the error Handler.
 * The call back will be then called each time the library 
 * detects an error
 * @param[in] callback Name of the callback to attach 
 * to the error Hanlder
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_AttachErrorHandler(void (*callback)(uint16_t error))
{
  error_handler_callback = (void (*)(uint16_t error))callback;
}

/******************************************************//**
 * @brief Attach a user callback to the flag Interrupt
 * The call back will be then called each time the status 
 * flag pin will be pulled down due to the occurrence of 
 * a programmed alarms ( OCD, thermal pre-warning or 
 * shutdown, UVLO, wrong command, non-performable command)
 * @param[in] callback Name of the callback to attach 
 * to the Flag Interrupt
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_AttachFlagInterrupt(void (*callback)(void))
{
  flag_interrupt_callback = (void (*)(void))callback;
}

/******************************************************//**
 * @brief Check if STSPIN820 has a fault by reading EN pin position.
 * @retval One if STSPIN820 has EN pin down, otherwise zero
 **********************************************************/
uint8_t STSPIN820::STSPIN820_CheckStatusHw(void)
{
    if(!STSPIN820_Board_EN_AND_FAULT_PIN_GetState())
  {
    return 0x01;
  }
  else
  {
    return 0x00;
  }
}

/******************************************************//**
 * @brief Disable the power bridges (leave the output bridges HiZ)
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_Disable(uint8_t deviceId)
{
  STSPIN820_Board_Disable();
}

/******************************************************//**
 * @brief Enable the power bridges
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_Enable(uint8_t deviceId)
{
  STSPIN820_Board_Enable();
}

/******************************************************//**
 * @brief Error handler which calls the user callback (if defined)
 * @param[in] error Number of the error
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_ErrorHandler(uint16_t error)
{
  if (error_handler_callback != 0)
  {
    (void) error_handler_callback(error);
  }
  else   
  {
    while(1)
    {
      /* Infinite loop */
    }
  }
}

/******************************************************//**
 * @brief Exit STSPIN820 device from standby (low power consumption)
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_ExitDeviceFromStandby(uint8_t deviceId)
{
  uint32_t sequencerPosition = device_prm.sequencerPosition;
  
  /* Exit standby and set step mode */
  /* Disable step clock */
  if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
  }

  /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
  /* after the last step clock rising edge triggering the last step */
  STSPIN820_Board_Delay(DISABLE_DELAY);
    /* Set reference voltage to 0 */
  STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);

  /* Disable power bridges */
  STSPIN820_Board_Disable();
  
  device_prm.commandExecuted = NO_CMD;
  device_prm.stepsToTake = 0;  
  device_prm.speed = 0;

  /* Reset the microstepping sequencer position */
  device_prm.sequencerPosition = 0;

  /* Reset current and mark positions */
  device_prm.currentPosition = 0; 
  device_prm.markPosition = 0;
  
  STSPIN820_SetStepMode(deviceId, device_prm.stepMode);
  STSPIN820_Board_ReleaseReset();
  STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
  
  STSPIN820_SetHome(deviceId);
  STSPIN820_SetMark(deviceId);
  
  if (device_prm.sequencerPosition != 0)
  {
    /* Set direction to FORWARD to ensure the HW sequencer is increased at */
    /* each step clock rising edge */
    STSPIN820_SetDirection(0, FORWARD);
    /* Going out of standby */
    device_prm.motionState = STANDBYTOINACTIVE;
    /* Initialize the step clock timer */
    STSPIN820_Board_TimStckInit();
    /* Program the step clock */    
    STSPIN820_Board_TimStckCompareInit();
    STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
    toggle_odd = 0;
    STSPIN820_Board_TimStckStart();
    while (device_prm.sequencerPosition != 0);
    while (toggle_odd!=0);
    device_prm.sequencerPosition = sequencerPosition;    
  }
  
  device_prm.motionState = INACTIVE;
}

/******************************************************//**
 * @brief Return the acceleration of the specified device
 * @param[in] deviceId Unused parameter
 * @retval Acceleration in pps^2
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetAcceleration(int8_t deviceId)
{
  return (device_prm.acceleration);
}            

/******************************************************//**
 * @brief Return the current speed of the specified device
 * @param[in] deviceId Unused parameter
 * @retval Speed in pps
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetCurrentSpeed(int8_t deviceId)
{
  return device_prm.speed;
}

/******************************************************//**
 * @brief Return the decay mode of the specified device
 * @param[in] deviceId Unused parameter
 * @retval Decay Mode State (SLOW or MIXED)
 **********************************************************/
motor_decay_mode_t STSPIN820::STSPIN820_GetDecayMode(uint8_t deviceId)
{
  motor_decay_mode_t status;
  if (STSPIN820_Board_GetDecayGpio() != 0) 
  {
    status = SLOW_DECAY;
  }
  else
  {
    status = MIXED_DECAY;
  }
  
  return status;
}

/******************************************************//**
 * @brief Return the deceleration of the specified device
 * @param[in] deviceId Unused parameter
 * @retval Deceleration in pps^2
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetDeceleration(uint8_t deviceId)
{
  return (device_prm.deceleration);
}          

/******************************************************//**
 * @brief Return the device state
 * @param[in] deviceId Unused parameter
 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
 **********************************************************/
motor_state_t STSPIN820::STSPIN820_GetDeviceState(uint8_t deviceId)
{
  return device_prm.motionState;
}

/******************************************************//**
 * @brief Get the motor current direction
 * @param[in] deviceId Unused parameter
 * @retval direction
 **********************************************************/
motor_direction_t STSPIN820::STSPIN820_GetDirection(uint8_t deviceId)
{
  return device_prm.direction;
}

/******************************************************//**
 * @brief Return the FW version of the library
 * @retval Stspin220_FW_VERSION
 **********************************************************/
uint32_t STSPIN820::STSPIN820_GetFwVersion(void)
{
  return (STSPIN820_FW_VERSION);
}

/******************************************************//**
 * @brief Return the mark position of the specified device
 * @param[in] deviceId Unused parameter
 * @retval mark position value
 **********************************************************/
int32_t STSPIN820::STSPIN820_GetMark(uint8_t deviceId)
{
  return device_prm.markPosition;
}

/******************************************************//**
 * @brief Return the max speed of the specified device
 * @param[in] deviceId Unused parameter
 * @retval maxSpeed in pps
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetMaxSpeed(uint8_t deviceId)
{
  return (device_prm.maxSpeed);
}

/******************************************************//**
 * @brief Return the min speed of the specified device
 * @param[in] deviceId Unused parameter
 * @retval minSpeed in pps
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetMinSpeed(uint8_t deviceId)
{
  return (device_prm.minSpeed);
}                                                     

/******************************************************//**
 * @brief  Returns the number of devices
 * @retval number of devices
 **********************************************************/
uint8_t STSPIN820::STSPIN820_GetNbDevices(void)
{
  return (number_of_devices);
}

/******************************************************//**
 * @brief Return the current position value of the specified device
 * @param[in] deviceId Unused parameter
 * @retval current position value
 **********************************************************/
int32_t STSPIN820::STSPIN820_GetPosition(uint8_t deviceId)
{
  return device_prm.currentPosition;
}

/******************************************************//**
 * @brief Get the motor step mode
 * @param[in] deviceId Unused parameter
 * @retval step mode
 **********************************************************/
motor_step_mode_t STSPIN820::STSPIN820_GetStepMode(uint8_t deviceId)
{
  return device_prm.stepMode;
}

/******************************************************//**
 * @brief Get the selected stop mode
 * @param[in] deviceId Unused parameter
 * @retval the selected stop mode
 **********************************************************/
motor_stop_mode_t STSPIN820::STSPIN820_GetStopMode(uint8_t deviceId)
{
  return device_prm.stopMode;
}

/******************************************************//**
 * @brief Get the torque of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] torqueMode torque mode
 * @retval the torqueValue in % (from 0 to 100)
 * @note
 **********************************************************/
uint8_t STSPIN820::STSPIN820_GetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
{
  uint8_t torqueValue = 0;
  switch(torqueMode)
  {
    case ACC_TORQUE:
      torqueValue = device_prm.accelTorque;
      break;
    case DEC_TORQUE:
      torqueValue = device_prm.decelTorque;
      break;
    case RUN_TORQUE:
      torqueValue = device_prm.runTorque;
      break;
    case HOLD_TORQUE:
      torqueValue = device_prm.holdTorque;
      break;
    case CURRENT_TORQUE:
      torqueValue = device_prm.currentTorque;
      break;
    default:
      break;
  }
  return torqueValue;
}

/******************************************************//**
 * @brief Get the torque boost feature status
 * @param[in] deviceId Unused parameter
 * @retval TRUE if enabled, FALSE if disabled
 **********************************************************/
bool STSPIN820::STSPIN820_GetTorqueBoostEnable(uint8_t deviceId)
{
  return device_prm.torqueBoostEnable;
}

/******************************************************//**
 * @brief Get the torque boost threshold
 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
 * @retval the torque boost threshold above which the step mode is
 * changed to full step
 **********************************************************/
uint16_t STSPIN820::STSPIN820_GetTorqueBoostThreshold(uint8_t deviceId)
{
  return device_prm.torqueBoostSpeedThreshold;
}

/******************************************************//**
 * @brief Request the motor to move to the home position (ABS_POSITION = 0)
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_GoHome(uint8_t deviceId)
{
  deviceId = 0;
  
  STSPIN820_GoTo(deviceId, 0);
} 
  
/******************************************************//**
 * @brief Request the motor to move to the mark position 
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_GoMark(uint8_t deviceId)
{
  deviceId = 0;
  
    STSPIN820_GoTo(deviceId, device_prm.markPosition);
}

/******************************************************//**
 * @brief Request the motor to move to the specified position 
 * @param[in] deviceId Unused parameter
 * @param[in] targetPosition absolute position in steps
 * @retval None 
 * @note The position is at the resolution corresponding to the
 * selected step mode.
 * STEP_MODE_FULL                   : Full step
 * STEP_MODE_HALF                   : 1/2 step
 * STEP_MODE_1_4                    : 1/4 step
 * STEP_MODE_1_8                    : 1/8 step
 * STEP_MODE_1_16                   : 1/16 step
 * STEP_MODE_1_32                   : 1/32 step
 * STEP_MODE_1_128                  : 1/128 step
 * STEP_MODE_1_256                  : 1/256 step
 * @note The 1/64 step mode is not allowed
 **********************************************************/
void STSPIN820::STSPIN820_GoTo(uint8_t deviceId, int32_t targetPosition)
{
  motor_direction_t direction;
  deviceId = 0;
  
  /* Exit from standby if needed */
  if (device_prm.motionState == STANDBY)
  {
    STSPIN820_ExitDeviceFromStandby(deviceId);
  }
  /* Deactivate motor if needed */
  else if (device_prm.motionState != INACTIVE)
  { 
    STSPIN820_HardHiZ(deviceId);
  }
  
  if (targetPosition > device_prm.currentPosition)
  {
    device_prm.stepsToTake = targetPosition -\
                                      device_prm.currentPosition;
    if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
    {
      direction = FORWARD;
    }
    else
    {
      direction = BACKWARD;
      device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
                                        device_prm.stepsToTake;
    }
  }
  else
  {
    device_prm.stepsToTake = device_prm.currentPosition -\
                                      targetPosition;
    if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
    {
      direction = BACKWARD;
    }
    else
    {
      direction = FORWARD; 
      device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
                                        device_prm.stepsToTake;
    }
  }
  
  if (device_prm.stepsToTake != 0) 
  {
    device_prm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSPIN820_SetDirection(deviceId, direction);
    
    STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
    
    /* Motor activation */
    STSPIN820_StartMovement(deviceId);
  }
}

/******************************************************//**
 * @brief Move the motor to the absolute position
 * @param[in] deviceId Unused parameter
 * @param[in] direction FORWARD or BACKWARD
 * @param[in] targetPosition 32 bit signed value position
 * @retval None
 * @note The position is at the resolution corresponding to the
 * selected step mode.
 * STEP_MODE_FULL                   : step
 * STEP_MODE_HALF                   : 1/2 step
 * STEP_MODE_1_4                    : 1/4 step
 * STEP_MODE_1_8                    : 1/8 step
 * STEP_MODE_1_16                   : 1/16 step
 * STEP_MODE_1_32                   : 1/32 step
 * STEP_MODE_1_128                  : 1/128 step
 * STEP_MODE_1_256                  : 1/256 step
 * @note The 1/64 step mode is not allowed
 **********************************************************/
void STSPIN820::STSPIN820_GoToDir(uint8_t deviceId, motor_direction_t direction, int32_t targetPosition)
{
  deviceId = 0;
  
  /* Exit from standby if needed */
  if (device_prm.motionState == STANDBY)
  {
    STSPIN820_ExitDeviceFromStandby(deviceId);
  }
  /* Deactivate motor if needed */
  else if (device_prm.motionState != INACTIVE)
  { 
    STSPIN820_HardHiZ(deviceId);
  }
  
  if (direction != BACKWARD)
  {
    if (targetPosition > device_prm.currentPosition)
    {
      device_prm.stepsToTake = targetPosition -\
                                        device_prm.currentPosition;
    }
    else
    {
      device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
                                       (targetPosition -\
                                        device_prm.currentPosition);
    }
  }
  else
  {
    if (targetPosition > device_prm.currentPosition)
    {
      device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
                                        (device_prm.currentPosition -\
                                         targetPosition);
    }
    else
    {
      device_prm.stepsToTake = device_prm.currentPosition -\
                                        targetPosition;
    }
  }

  if (device_prm.stepsToTake != 0) 
  {
    device_prm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSPIN820_SetDirection(deviceId, direction);
    
    STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
    
    /* Motor activation */
    STSPIN820_StartMovement(deviceId);
  }  
}

/******************************************************//**
 * @brief  Immediatly stop the motor and disable the power bridge
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_HardHiZ(uint8_t deviceId) 
{
  /* Set inactive state */
  device_prm.motionState = INACTIVE;
  
  /* Disable step clock */
  if (STSPIN820_Board_TimStckStop(handle &toggle_odd) == 0)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
  }

  /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
  /* after the last step clock rising edge triggering the last step */
  STSPIN820_Board_Delay(DISABLE_DELAY);
  
  /* Set reference voltage to 0 */
  STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);

  /* Disable power bridges */
  STSPIN820_Board_Disable();
  
  /* Comeback to nominal step mode */
  if (device_prm.stepModeLatched != device_prm.stepMode)
  {
    motor_step_mode_t StepMode = device_prm.stepModeLatched;
    STSPIN820_SetStepMode(0, StepMode);
    device_prm.stepMode = device_prm.stepModeLatched;
  }

  device_prm.commandExecuted = NO_CMD;
  device_prm.stepsToTake = 0;  
  device_prm.speed = 0;
}

/******************************************************//**
 * @brief  Immediatly stop the motor
 * and either set holding torque when stop mode is HOLD_MODE,
 * or call STSPIN820_HardHiz function when stop mode is HIZ_MODE,
 * or call STSPIN820_PutDeviceInStandby function when stop mode is STANDBY_MODE
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_HardStop(uint8_t deviceId) 
{
  deviceId = 0;
  
  if (device_prm.stopMode == HOLD_MODE)
  {
    /* Set inactive state */
    device_prm.motionState = INACTIVE;

    /* Disable step clock */
    if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
    {
      STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
    }
    
    /* Set holding torque */
    STSPIN820_ApplyTorque(deviceId, HOLD_TORQUE);
 
    /* Comeback to nominal step mode */
    if (device_prm.stepModeLatched != device_prm.stepMode)
    {
      motor_step_mode_t StepMode = device_prm.stepModeLatched;
      STSPIN820_SetStepMode(0, StepMode);
      device_prm.stepMode = device_prm.stepModeLatched;
    }    
    
    device_prm.commandExecuted = NO_CMD;
    device_prm.stepsToTake = 0;  
    device_prm.speed = 0;
  }
  else if (device_prm.stopMode == HIZ_MODE)
  {
    STSPIN820_HardHiZ(deviceId);
  }
  else if (device_prm.stopMode == STANDBY_MODE)
  {
    STSPIN820_PutDeviceInStandby(deviceId);
  }
}

/******************************************************//**
 * @brief  Moves the motor of the specified number of steps
 * @param[in] deviceId Unused parameter
 * @param[in] direction FORWARD or BACKWARD
 * @param[in] stepCount Number of steps to perform
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_Move(uint8_t deviceId, motor_direction_t direction, uint32_t stepCount)
{
  deviceId = 0;
  
  /* Exit from standby if needed */
  if (device_prm.motionState == STANDBY)
  {
    STSPIN820_ExitDeviceFromStandby(deviceId);
  }
  /* Deactivate motor if needed */
  else if (device_prm.motionState != INACTIVE)
  {
    STSPIN820_HardHiZ(deviceId);
  }
  
  if (stepCount != 0) 
  {
    device_prm.stepsToTake = stepCount;    
    device_prm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSPIN820_SetDirection(deviceId, direction);
    
    STSPIN820_ComputeSpeedProfile(deviceId, stepCount);
    
    /* Motor activation */
    STSPIN820_StartMovement(deviceId);
  }  
}

/******************************************************//**
 * @brief Put STSPIN820 device in standby (low power consumption)
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_PutDeviceInStandby(uint8_t deviceId)
{
  /* Stop movement */
  STSPIN820_HardHiZ(deviceId);
  
  /* Enter standby */
    STSPIN820_Board_Reset();
  
  device_prm.motionState = STANDBY;
}

/******************************************************//**
 * @brief  Runs the motor. It will accelerate from the min 
 * speed up to the max speed by using the device acceleration.
 * @param[in] deviceId Unused parameter
 * @param[in] direction FORWARD or BACKWARD
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_Run(uint8_t deviceId, motor_direction_t direction)
{
  /* Exit from standby if needed */
  if (device_prm.motionState == STANDBY)
  {
    STSPIN820_ExitDeviceFromStandby(deviceId);
  }
  /* Deactivate motor if needed */
  else if (device_prm.motionState != INACTIVE)
  {
    STSPIN820_HardHiZ(deviceId);
  }
  
    /* Direction setup */
    STSPIN820_SetDirection(deviceId,direction);
    device_prm.commandExecuted = RUN_CMD;
    /* Motor activation */
    STSPIN820_StartMovement(deviceId); 
}

/******************************************************//**
 * @brief  Changes the acceleration of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] newAcc New acceleration to apply in pps^2
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed if the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command)
 **********************************************************/
bool STSPIN820::STSPIN820_SetAcceleration(uint8_t deviceId, uint16_t newAcc)
{
  bool cmdExecuted = FALSE;
  if ((newAcc != 0)&&
      (((device_prm.motionState & INACTIVE) == INACTIVE)||
       (device_prm.commandExecuted == RUN_CMD)))
  {
    device_prm.acceleration = newAcc;
    cmdExecuted = TRUE;
  }    
  return cmdExecuted;
}            

/******************************************************//**
 * @brief  Specifies the decay mode 
 * @param[in] deviceId Unused parameter
 * @param[in] decay SLOW_DECAY or MIXED_DECAY
 * @retval true if the command is successfully executed, else false
 **********************************************************/
void STSPIN820::STSPIN820_SetDecayMode(uint8_t deviceId, motor_decay_mode_t decay)
{ 
  if (decay == SLOW_DECAY)
  {
    STSPIN820_Board_SetDecayGpio(1);
  }
  else if (decay == MIXED_DECAY)
  {
    STSPIN820_Board_SetDecayGpio(0);
  }
}

/******************************************************//**
 * @brief  Changes the deceleration of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] newDec New deceleration to apply in pps^2
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed if the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command)
 **********************************************************/
bool STSPIN820::STSPIN820_SetDeceleration(uint8_t deviceId, uint16_t newDec)
{
  bool cmdExecuted = FALSE;
  if ((newDec != 0)&& 
      (((device_prm.motionState & INACTIVE) == INACTIVE)||
       (device_prm.commandExecuted == RUN_CMD)))
  {
    device_prm.deceleration = newDec;
    cmdExecuted = TRUE;
  }      
  return cmdExecuted;
}

/******************************************************//**
 * @brief  Specifies the direction 
 * @param[in] deviceId Unused parameter
 * @param[in] dir FORWARD or BACKWARD
 * @note The direction change is applied if the device 
 * is in INACTIVE or STANDBY state or if the device is 
 * executing a run command
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetDirection(uint8_t deviceId, motor_direction_t dir)
{
  if ((device_prm.motionState == INACTIVE)||\
      (device_prm.motionState == STANDBY))
  {
    device_prm.direction = dir;
    STSPIN820_Board_SetDirectionGpio(dir);
  }
  else if ((device_prm.commandExecuted&RUN_CMD)!=0)
  {
    device_prm.commandExecuted|=STSPIN820_DIR_CHANGE_BIT_MASK;
  }
}

/******************************************************//**
 * @brief  Set current position to be the Home position
 * (current position set to 0)
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetHome(uint8_t deviceId)
{
  device_prm.currentPosition = 0;
}
 
/******************************************************//**
 * @brief  Set current position to be the Mark position 
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetMark(uint8_t deviceId)
{
  device_prm.markPosition = device_prm.currentPosition;
}

/******************************************************//**
 * @brief  Changes the max speed of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] newMaxSpeed New max speed  to apply in pps
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command).
 **********************************************************/
bool STSPIN820::STSPIN820_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed)
{
  bool cmdExecuted = FALSE;
  if ((newMaxSpeed >= STSPIN820_MIN_STCK_FREQ)&&\
      ((newMaxSpeed <= STSPIN820_MAX_STCK_FREQ)||\
       ((device_prm.torqueBoostEnable != FALSE)&&\
        ((newMaxSpeed>>STSPIN820_GetStepMode(deviceId))<= STSPIN820_MAX_STCK_FREQ)))&&\
      (device_prm.minSpeed <= newMaxSpeed) &&\
      (((device_prm.motionState & INACTIVE) == INACTIVE)||\
      (device_prm.commandExecuted == RUN_CMD)))
  {
    device_prm.maxSpeed = newMaxSpeed;
    cmdExecuted = TRUE;
  }
  return cmdExecuted;
}                                                     

/******************************************************//**
 * @brief  Changes the min speed of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] newMinSpeed New min speed  to apply in pps
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command).
 **********************************************************/
bool STSPIN820::STSPIN820_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed)
{
  bool cmdExecuted = FALSE;
  if ((newMinSpeed >= STSPIN820_MIN_STCK_FREQ)&&
      (newMinSpeed <= STSPIN820_MAX_STCK_FREQ) &&
      (newMinSpeed <= device_prm.maxSpeed) && 
      (((device_prm.motionState & INACTIVE) == INACTIVE)||
       (device_prm.commandExecuted == RUN_CMD)))
  {
    device_prm.minSpeed = newMinSpeed;
    cmdExecuted = TRUE;
  }  
  return cmdExecuted;
}

/******************************************************//**
 * @brief  Sets the number of devices to be used 
 * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES)
 * @retval TRUE if successfull, FALSE if failure, attempt to set a number of 
 * devices greater than MAX_NUMBER_OF_DEVICES
 **********************************************************/
bool STSPIN820::STSPIN820_SetNbDevices(uint8_t nbDevices)
{
  if (nbDevices <= MAX_NUMBER_OF_DEVICES)
  {
    number_of_devices = nbDevices;
    return TRUE;
  }
  else
  {
    return FALSE;
  }
}

/******************************************************//**
 * @brief Set the stepping mode 
 * @param[in] deviceId Unused parameter
 * @param[in] stepMode from full step to 1/256 microstep
 * as specified in enum motor_step_mode_t
 * 1/64 microstep mode not supported by STSPIN820
 * @retval true if the command is successfully executed, else false
 **********************************************************/
bool STSPIN820::STSPIN820_SetStepMode(uint8_t deviceId, motor_step_mode_t stepMode)
{
  /* Store step mode */
  device_prm.stepMode = stepMode;
  device_prm.stepModeLatched = stepMode;
  
  /* Set the mode pins to the levels corresponding to the selected step mode */
  switch (stepMode)
  {
    case STEP_MODE_FULL:
      STSPIN820_Board_SetFullStep();
      break;
    case STEP_MODE_HALF:
      STSPIN820_Board_SetModePins(1, 0, 0);
      break;    
    case STEP_MODE_1_4:
      STSPIN820_Board_SetModePins(0, 1, 0);
      break;        
    case STEP_MODE_1_8:
      STSPIN820_Board_SetModePins(1, 1, 0);
      break;
    case STEP_MODE_1_16:
      STSPIN820_Board_SetModePins(0, 0, 1);
      break;   
    case STEP_MODE_1_32:
      STSPIN820_Board_SetModePins(1, 0, 1);
      break;   
    case STEP_MODE_1_128:
      STSPIN820_Board_SetModePins(0, 1, 1);
      break;  
    case STEP_MODE_1_256:
      STSPIN820_Board_SetModePins(1, 1, 1);
      break;
    default:
      return FALSE;
  }

  return TRUE;
  
}

/******************************************************//**
 * @brief Select the mode to stop the motor.
 * @param[in] deviceId Unused parameter
 * @param[in] stopMode HOLD_MODE to let power bridge enabled
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetStopMode(uint8_t deviceId, motor_stop_mode_t stopMode)
{
  device_prm.stopMode = stopMode;
}

/******************************************************//**
 * @brief Set the torque of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] torqueMode Torque mode as specified in enum motor_torque_mode_t
 * @param[in] torqueValue in % (from 0 to 100)
 * @retval None
 * @note
 **********************************************************/
void STSPIN820::STSPIN820_SetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode, uint8_t torqueValue)
{
  device_prm.updateTorque = TRUE;
  if (torqueValue>100)
  {
    torqueValue = 100;
  }
  switch(torqueMode)
  {
    case ACC_TORQUE:
      device_prm.accelTorque = torqueValue;
      break;
    case DEC_TORQUE:
      device_prm.decelTorque = torqueValue;
      break;
    case RUN_TORQUE:
      device_prm.runTorque = torqueValue;
      break;
    case HOLD_TORQUE:
      device_prm.holdTorque = torqueValue;
      if (device_prm.motionState != INACTIVE)
      {
        break;
      }
    case CURRENT_TORQUE:
      device_prm.currentTorque = torqueValue;
      STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
    default:
      device_prm.updateTorque = FALSE;
      break; //ignore error
  }
}

/******************************************************//**
 * @brief Enable or disable the torque boost feature
 * @param[in] deviceId Unused parameter
 * @param[in] enable true to enable torque boost, false to disable
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetTorqueBoostEnable(uint8_t deviceId, bool enable)
{
  device_prm.torqueBoostEnable = enable;
}

/******************************************************//**
 * @brief Set the torque boost threshold
 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
 * @param[in] speedThreshold speed threshold above which the step mode is
 * changed to full step
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetTorqueBoostThreshold(uint8_t deviceId, uint16_t speedThreshold)
{
  device_prm.torqueBoostSpeedThreshold = speedThreshold;
}

/******************************************************//**
 * @brief  Stops the motor by using the device deceleration
 * @param[in] deviceId Unused parameter
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed if the device is in INACTIVE,
 * STANDBYTOINACTIVE or STANDBY state.
 **********************************************************/
bool STSPIN820::STSPIN820_SoftStop(uint8_t deviceId)
{
  bool cmdExecuted = FALSE;
  if ((device_prm.motionState & INACTIVE) != INACTIVE)
  {
    device_prm.commandExecuted |= STSPIN820_SOFT_STOP_BIT_MASK;
    cmdExecuted = TRUE;
  }
  return (cmdExecuted);
}

/******************************************************//**
 * @brief Get the frequency of REF PWM of the specified device
 * @param[in] deviceId Unused parameter
 * @retval the frequency of REF PWM in Hz
 * @note
 **********************************************************/
uint32_t STSPIN820::STSPIN820_VrefPwmGetFreq(uint8_t deviceId)
{
  return device_prm.refPwmFreq;
}

/******************************************************//**
 * @brief Set the frequency of REF PWM of the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] newFreq in Hz
 * @retval None
 * @note
 **********************************************************/
void STSPIN820::STSPIN820_VrefPwmSetFreq(uint8_t deviceId, uint32_t newFreq)
{
  device_prm.refPwmFreq = newFreq;
  STSPIN820_Board_PwmRefSetFreqAndDutyCycle(newFreq,device_prm.currentTorque);
}

/******************************************************//**
 * @brief  Locks until the device state becomes Inactive
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_WaitWhileActive(uint8_t deviceId)
{
  /* Wait while motor is running */
  while (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)!=INACTIVE)||\
   (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)==INACTIVE)&&(toggle_odd!=0)));
}

/******************************************************//**
 * @brief Updates the current speed of the device
 * @param[in] deviceId Unused parameter
 * @param[in] newSpeed in pps
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_ApplySpeed(uint8_t deviceId, uint16_t newSpeed)
{
  if (device_prm.torqueBoostEnable != FALSE)
  {
    if (device_prm.stepMode > STEP_MODE_1_256)
    {
      STSPIN820_ErrorHandler(STSPIN820_ERROR_APPLY_SPEED);
    }
    if (device_prm.stepMode != STEP_MODE_FULL)
    {
      if (((newSpeed>>device_prm.stepModeLatched)>\
           device_prm.torqueBoostSpeedThreshold)&&\
          (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) != MOVE_CMD) ||\
           ((device_prm.stepsToTake-device_prm.relativePos)>=\
            (1<<device_prm.stepModeLatched))))
      {         
        if ((device_prm.sequencerPosition & 0xFF) == 0x80)
       {
          STSPIN820_Board_SetFullStep();
          device_prm.stepMode = STEP_MODE_FULL;
          device_prm.accu >>= device_prm.stepModeLatched;
          newSpeed >>= device_prm.stepModeLatched;
       }
      }
    }
    else if (((newSpeed <= device_prm.torqueBoostSpeedThreshold) &&\
              (device_prm.stepModeLatched != STEP_MODE_FULL))||\
             (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) == MOVE_CMD)&&\
               ((device_prm.stepsToTake-device_prm.relativePos)<=\
                (1<<device_prm.stepModeLatched))))
    {
      STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
      device_prm.stepMode = device_prm.stepModeLatched;
      device_prm.accu <<= device_prm.stepModeLatched;
      newSpeed <<= device_prm.stepModeLatched;
    }
  }
  else if (device_prm.stepMode != device_prm.stepModeLatched)
  {
    //torqueBoostEnable has just been disabled
    STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
    device_prm.stepMode = device_prm.stepModeLatched;
    device_prm.accu <<= device_prm.stepModeLatched;
    newSpeed <<= device_prm.stepModeLatched;
  }
  
  if (newSpeed < STSPIN820_MIN_STCK_FREQ)
  {
    newSpeed = STSPIN820_MIN_STCK_FREQ;  
  }
  if (newSpeed > STSPIN820_MAX_STCK_FREQ)
  {
    newSpeed = STSPIN820_MAX_STCK_FREQ;
  }
  
  device_prm.speed = newSpeed;
  STSPIN820_Board_TimStckSetFreq(newSpeed);

}

/******************************************************//**
 * @brief Apply the set torque to the specified device
 * @param[in] deviceId Unused parameter
 * @param[in] torqueMode torque mode
 * @retval None
 * @note
 **********************************************************/
void STSPIN820::STSPIN820_ApplyTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
{
  uint8_t torqueValue = 0;
  device_prm.updateTorque = FALSE;
  switch(torqueMode)
  {
    case ACC_TORQUE:
      device_prm.currentTorque = device_prm.accelTorque;
      break;
    case DEC_TORQUE:
      device_prm.currentTorque = device_prm.decelTorque;
      break;
    case RUN_TORQUE:
      device_prm.currentTorque = device_prm.runTorque;
      break;
    case HOLD_TORQUE:
      device_prm.currentTorque = device_prm.holdTorque;
      break;
    case CURRENT_TORQUE:
      break;
    default:
      return; //ignore error
  }
  torqueValue = device_prm.currentTorque;
  STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
}

/******************************************************//**
 * @brief  Computes the speed profile according to the number of steps to move
 * @param[in] deviceId Unused parameter
 * @param[in] nbSteps number of steps to perform
 * @retval None
 * @note Using the acceleration and deceleration of the device,
 * this function determines the duration in steps of the acceleration,
 * steady and deceleration phases.
 * If the total number of steps to perform is big enough, a trapezoidal move
 * is performed (i.e. there is a steady phase where the motor runs at the maximum
 * speed.
 * Else, a triangular move is performed (no steady phase: the maximum speed is never
 * reached.
 **********************************************************/
void STSPIN820::STSPIN820_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps)
{
  uint32_t reqAccSteps; 
    uint32_t reqDecSteps;
   
  /* compute the number of steps to get the targeted speed */
  uint16_t minSpeed = device_prm.minSpeed;
  reqAccSteps = (device_prm.maxSpeed - minSpeed);
  reqAccSteps *= (device_prm.maxSpeed + minSpeed);
  reqDecSteps = reqAccSteps;
  reqAccSteps /= (uint32_t)device_prm.acceleration;
  reqAccSteps /= 2;

  /* compute the number of steps to stop */
  reqDecSteps /= (uint32_t)device_prm.deceleration;
  reqDecSteps /= 2;

    if(( reqAccSteps + reqDecSteps ) > nbSteps)
    {   
    /* Triangular move  */
    /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
    uint32_t dec = device_prm.deceleration;
    uint32_t acc = device_prm.acceleration;
    
    reqDecSteps =  ((uint32_t) dec * nbSteps) / (acc + dec);
    if (reqDecSteps > 1)
    {
      reqAccSteps = reqDecSteps - 1;
      if(reqAccSteps == 0)
      {
        reqAccSteps = 1;
      }      
    }
    else
    {
      reqAccSteps = 0;
    }
    device_prm.endAccPos = reqAccSteps;
    device_prm.startDecPos = reqDecSteps;
    }
    else
    {    
    /* Trapezoidal move */
    /* accelerating phase to endAccPos */
    /* steady phase from  endAccPos to startDecPos */
    /* decelerating from startDecPos to stepsToTake*/
    device_prm.endAccPos = reqAccSteps;
    device_prm.startDecPos = nbSteps - reqDecSteps - 1;
    }
}

/******************************************************//**
 * @brief  Handlers of the flag interrupt which calls the user callback (if defined)
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_FlagInterruptHandler(void)
{
  if (flag_interrupt_callback != 0)
  {
    flag_interrupt_callback();
  }
}

/******************************************************//**
 * @brief  Set the parameters of the device whose values are not defined in
 * STSPIN820_config.h
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetDeviceParamsOtherValues(void)
{
  uint16_t tmp;

  device_prm.accu = 0;
  device_prm.currentPosition = 0;
  device_prm.sequencerPosition = 0;
  device_prm.endAccPos = 0;
  device_prm.relativePos = 0;
  device_prm.startDecPos = 0;
  device_prm.stepsToTake = 0;
  device_prm.updateTorque = FALSE;
  device_prm.speed = 0;
  device_prm.commandExecuted = NO_CMD;
  device_prm.direction = FORWARD;
  tmp = device_prm.minSpeed;
  if (((device_prm.torqueBoostEnable != FALSE)&&\
       (device_prm.torqueBoostSpeedThreshold>STSPIN820_MAX_STCK_FREQ))||\
      (tmp>device_prm.maxSpeed))
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_INIT);
  }
}

/******************************************************//**
 * @brief  Set the parameters of the device to values of initDevicePrm structure
 * @param pInitDevicePrm structure containing values to initialize the device 
 * parameters
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* pInitDevicePrm)
{
  device_prm.motionState = STANDBY;;

  if (STSPIN820_SetAcceleration(0,pInitDevicePrm->acceleration)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
  }
  if (STSPIN820_SetDeceleration(0,pInitDevicePrm->deceleration)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
  }
  if (STSPIN820_SetMaxSpeed(0,pInitDevicePrm->maxSpeed)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
  }
  if (STSPIN820_SetMinSpeed(0,pInitDevicePrm->minSpeed)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
  }
 
  STSPIN820_VrefPwmSetFreq(0,pInitDevicePrm->vrefPwmFreq);
  STSPIN820_SetTorque(0,ACC_TORQUE,pInitDevicePrm->accelTorque);
  STSPIN820_SetTorque(0,DEC_TORQUE,pInitDevicePrm->decelTorque);
  STSPIN820_SetTorque(0,RUN_TORQUE,pInitDevicePrm->runTorque);
  STSPIN820_SetTorque(0,HOLD_TORQUE,pInitDevicePrm->holdTorque);
  device_prm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
  device_prm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
  STSPIN820_SetStopMode(0,pInitDevicePrm->stopMode);

  STSPIN820_SetDeviceParamsOtherValues();
  
  /* Eventually deactivate motor */
  if ((device_prm.motionState != INACTIVE)&&\
      (device_prm.motionState != STANDBY))
  {
    STSPIN820_HardHiZ(0);
  }

  /* Enter standby */
  STSPIN820_Board_Reset();
  
  /* Reset the microstepping sequencer position */
  device_prm.sequencerPosition = 0;

  /* Reset current and mark positions */
  device_prm.currentPosition = 0; 
  device_prm.markPosition = 0;
  
  /* Set predefined step mode */
  STSPIN820_SetStepMode(0, pInitDevicePrm->stepMode);
  
  /* Wait */
  STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
  
  /* Exit standby */
  STSPIN820_Board_ReleaseReset();
  
    /* Let a delay after reset release*/
  STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
  
}

/******************************************************//**
 * @brief  Sets the parameters of the device to predefined values
 * from STSPIN820_config.h
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_SetDeviceParamsToPredefinedValues(void)
{
  device_prm.motionState = STANDBY;

  if (STSPIN820_SetAcceleration(0,STSPIN820_CONF_PARAM_ACC)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
  }
  if (STSPIN820_SetDeceleration(0,STSPIN820_CONF_PARAM_DEC)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
  }
  if (STSPIN820_SetMaxSpeed(0,STSPIN820_CONF_PARAM_RUNNING_SPEED)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
  }
  if (STSPIN820_SetMinSpeed(0,STSPIN820_CONF_PARAM_MIN_SPEED)==FALSE)
  {
    STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
  }

  STSPIN820_VrefPwmSetFreq(0,STSPIN820_CONF_PARAM_REF_PWM_FREQUENCY);
  STSPIN820_SetTorque(0,ACC_TORQUE,STSPIN820_CONF_PARAM_ACC_TORQUE);
  STSPIN820_SetTorque(0,DEC_TORQUE,STSPIN820_CONF_PARAM_DEC_TORQUE);
  STSPIN820_SetTorque(0,RUN_TORQUE,STSPIN820_CONF_PARAM_RUNNING_TORQUE);
  STSPIN820_SetTorque(0,HOLD_TORQUE,STSPIN820_CONF_PARAM_HOLDING_TORQUE);
  device_prm.torqueBoostEnable = STSPIN820_CONF_PARAM_TORQUE_BOOST_EN;
  device_prm.torqueBoostSpeedThreshold = STSPIN820_CONF_PARAM_TORQUE_BOOST_TH;
  STSPIN820_SetStopMode(0,STSPIN820_CONF_PARAM_AUTO_HIZ_STOP);

  STSPIN820_SetDeviceParamsOtherValues();
  
    /* Eventually deactivate motor */
  if ((device_prm.motionState != INACTIVE)&&\
      (device_prm.motionState != STANDBY))
  {
    STSPIN820_HardHiZ(0);
  }

  /* Enter standby */
  STSPIN820_Board_Reset();
  
  /* Reset the microstepping sequencer position */
  device_prm.sequencerPosition = 0;

  /* Reset current and mark positions */
  device_prm.currentPosition = 0; 
  device_prm.markPosition = 0;
  
  /* Set predefined step mode */
  STSPIN820_SetStepMode(0, STSPIN820_CONF_PARAM_STEP_MODE);
    
  /* Wait */
  STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
  
  /* Exit standby */
  STSPIN820_Board_ReleaseReset();
  
  /* Let a delay after reset release*/
  STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
}

/******************************************************//**
 * @brief Initialises the bridge parameters to start the movement
 * and enable the power bridge
 * @param[in] deviceId Unused parameter
 * @retval None
 **********************************************************/
void STSPIN820::STSPIN820_StartMovement(uint8_t deviceId)  
{
  deviceId = 0;
  
  /* Enable STSPIN820 powerstage */
  STSPIN820_Enable(deviceId);
  toggle_odd = 0;
  device_prm.accu = 0;
  device_prm.relativePos = 0;  
  if ((device_prm.endAccPos == 0)&&\
      (device_prm.commandExecuted != RUN_CMD))
  {
    device_prm.motionState = DECELERATING;
    STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
  }
  else
  {
    device_prm.motionState = ACCELERATING;
    STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
  }
  STSPIN820_Board_PwmRefStart();
  /* Initialize the step clock timer */
  STSPIN820_Board_TimStckInit();
  /* Program the step clock */
  STSPIN820_Board_TimStckCompareInit();
  STSPIN820_ApplySpeed(deviceId, device_prm.minSpeed);
  STSPIN820_Board_TimStckStart();
}

/******************************************************//**
 * @brief  Handles the device state machine at each pulse
 * @param[in] deviceId Unused parameter
 * @retval None
 * @note Must only be called by the timer ISR
 **********************************************************/
void STSPIN820::STSPIN820_StepClockHandler(uint8_t deviceId)
{
  uint32_t stepModeShift = device_prm.stepModeLatched - device_prm.stepMode;
  uint16_t tmp;
  deviceId = 0;
  
  if (device_prm.motionState == STANDBYTOINACTIVE)
  {
    if (toggle_odd != 0)
    {
      toggle_odd = 0;
      if (device_prm.sequencerPosition == 0)
      {
        if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
        {
          STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
        }
        return;
      }      
    }
    else
    {
      toggle_odd = 1;
      tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
      device_prm.sequencerPosition -= tmp;
    }
    STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
    return;
  }  
  
  if (toggle_odd == 0)
  {
    toggle_odd = 1;
  }
  else
  {
    toggle_odd = 0;
    /* Incrementation of the relative position */
    device_prm.relativePos += (1 << stepModeShift);

    /* Incrementation of the current position */
    if (device_prm.direction != BACKWARD)
    {
      device_prm.currentPosition += (1 << stepModeShift);
      tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
      device_prm.sequencerPosition += tmp;
      if (device_prm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
      {
        device_prm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
      }
    }
    else
    {
      device_prm.currentPosition -= (1 << stepModeShift);
      tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
      device_prm.sequencerPosition -= tmp;
      if (device_prm.sequencerPosition < 0)
      {
        device_prm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
      }
    }

    switch (device_prm.motionState) 
    {
      case ACCELERATING: 
      {
          uint32_t relPos = device_prm.relativePos;
          uint32_t endAccPos = device_prm.endAccPos;
          uint16_t speed = device_prm.speed;
          uint32_t acc = ((uint32_t)device_prm.acceleration << 16)>>stepModeShift;
        
          if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
              ((device_prm.commandExecuted==MOVE_CMD)&&(relPos>=device_prm.startDecPos)))
          {
            device_prm.motionState = DECELERATING;
            device_prm.accu = 0;
            /* Apply decelerating torque */
            STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
          }
          else if ((speed>=(device_prm.maxSpeed>>stepModeShift))||\
                   ((device_prm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
          {
            device_prm.motionState = STEADY;
            /* Apply running torque */
            STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
          }
          else
          {
            bool speedUpdated = FALSE;
            /* Go on accelerating */
            if (speed==0)
            {
              speed =1;
            }
            device_prm.accu += acc / speed;
            while (device_prm.accu>=(0X10000L))
            {
              device_prm.accu -= (0X10000L);
              speed +=1;
              speedUpdated = TRUE;
            }
          
            if (speedUpdated)
            {
              if (speed>(device_prm.maxSpeed>>stepModeShift))
              {
                speed = device_prm.maxSpeed>>stepModeShift;
              }    
              device_prm.speed = speed;
            }
            
            if (device_prm.updateTorque!=FALSE)
            {
              /* Apply accelerating torque */
              STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);              
            }
          }
          break;
      }
      case STEADY: 
      {
        uint16_t maxSpeed = device_prm.maxSpeed>>stepModeShift;
        uint32_t relativePos = device_prm.relativePos;
        if (device_prm.updateTorque!=FALSE)
        {
          /* Apply accelerating torque */
          STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
        }
        if  (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
             ((device_prm.commandExecuted==MOVE_CMD)&&\
              (relativePos>=(device_prm.startDecPos)))||\
             ((device_prm.commandExecuted==RUN_CMD)&&\
              (device_prm.speed>maxSpeed)))
        {
          device_prm.motionState = DECELERATING;
          device_prm.accu = 0;
          /* Apply decelerating torque */
          STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
        }
        else if ((device_prm.commandExecuted==RUN_CMD)&&(device_prm.speed<maxSpeed))
        {
          device_prm.motionState = ACCELERATING;
          device_prm.accu = 0;
          /* Apply accelerating torque */
          STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
        }
        break;
      }
      case DECELERATING: 
      {
        uint32_t relativePos = device_prm.relativePos;
        uint16_t speed = device_prm.speed;
        uint32_t dec = ((uint32_t)device_prm.deceleration << 16)>>stepModeShift;
        if ((((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)&&\
             (speed<=(device_prm.minSpeed>>stepModeShift)))||\
            ((device_prm.commandExecuted==MOVE_CMD)&&(relativePos>=device_prm.stepsToTake)))
        {
          /* Motion process complete */
          if ((device_prm.commandExecuted&STSPIN820_DIR_CHANGE_BIT_MASK)!=0)
          {
            device_prm.commandExecuted&=~STSPIN820_DIR_CHANGE_BIT_MASK;
            if (device_prm.direction==BACKWARD)
            {
              device_prm.direction=FORWARD;
            }
            else device_prm.direction=BACKWARD;
            STSPIN820_Board_SetDirectionGpio(device_prm.direction);
            if ((device_prm.commandExecuted&STSPIN820_SOFT_STOP_BIT_MASK)==0)
            {
              device_prm.motionState = ACCELERATING;
              device_prm.accu = 0;
              /* Apply accelerating torque */
              STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
              break;
            }
          }
          if (device_prm.stopMode==HOLD_MODE)
          {
            STSPIN820_HardStop(deviceId);
          }
          else if (device_prm.stopMode==STANDBY_MODE)
          {
            STSPIN820_PutDeviceInStandby(deviceId);
          }
          else
          {
            STSPIN820_HardHiZ(deviceId);
          }
        }
        else if ((device_prm.commandExecuted==RUN_CMD)&&
                 (speed<=(device_prm.maxSpeed>>stepModeShift)))
        {
          device_prm.motionState = STEADY;
          /* Apply running torque */
          STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
        }
        else
        {
          /* Go on decelerating */
          if (speed>(device_prm.minSpeed>>stepModeShift))
          {
            bool speedUpdated = FALSE;
            if (speed==0)
            {
              speed = 1;
            }
            device_prm.accu += dec / speed;
            while (device_prm.accu>=(0X10000L))
            {
              device_prm.accu -= (0X10000L);
              if (speed>1)
              {  
                speed -=1;
              }
              speedUpdated = TRUE;
            }
          
            if (speedUpdated)
            {
              if (speed<(device_prm.minSpeed>>stepModeShift))
              {
                speed = device_prm.minSpeed>>stepModeShift;
              }  
              device_prm.speed = speed;
            }
            
            if (device_prm.updateTorque!=FALSE)
            {
              /* Apply decelerating torque */
              STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
            }
          }
        }
        break;
      }
      default: 
      {
        break;
      }
    }
  }
  if ((device_prm.motionState & INACTIVE) != INACTIVE)
  {
    STSPIN820_ApplySpeed(deviceId, device_prm.speed);
  }
  else
  {
    if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
    {
      STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
    }
  }
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/