File content as of revision 5:fd1315beea32:

/**
  ******************************************************************************
  * @file    STSpin220.cpp
  * @author  IPC Rennes
  * @version V1.0.0
  * @date    July 27th, 2016
  * @brief   STSpin220 product related routines
  * @note    (C) COPYRIGHT 2016 STMicroelectronics
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT(c) 2016 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.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "STSpin220.h"

/* Definitions ---------------------------------------------------------------*/

/* Variables  ----------------------------------------------------------------*/
/* Number of devices. */
uint8_t STSpin220::numberOfDevices = 0;

/* Methods -------------------------------------------------------------------*/
/******************************************************//**
 * @brief Start the STSpin220 library
 * @param[in] pInit pointer to the initialization data
 * @retval COMPONENT_OK in case of success.
 **********************************************************/
status_t STSpin220::STSpin220_Init(void* pInit)
{ 
  STSpin220_Disable();
  if (pInit == NULL)
  {
    /* Set context variables to the predefined values from STSpin220_target_config.h */
    /* Set GPIO according to these values */
    STSpin220_SetDeviceParamsToPredefinedValues();
  }
  else
  {
    STSpin220_SetDeviceParamsToGivenValues((STSpin220_init_t*) pInit);
  }
  STSpin220_Board_TimStckInit(false);
  return COMPONENT_OK;
}

/**********************************************************
 * @brief Read id
 * @param id pointer to the identifier to be read.
 * @retval COMPONENT_OK in case of success.
 **********************************************************/
status_t STSpin220::STSpin220_ReadID(uint8_t *id)
{
  *id = deviceInstance;

  return COMPONENT_OK;
}

/**********************************************************
 * @brief  Attaches 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 STSpin220::STSpin220_AttachErrorHandler(void (*callback)(uint16_t error))
{
  errorHandlerCallback = (void (*)(uint16_t error)) callback;
}

/******************************************************//**
 * @brief Apply the set torque
 * @param[in] torqueMode torque mode
 * @retval None
 * @note
 **********************************************************/
void STSpin220::STSpin220_ApplyTorque(motorTorqueMode_t torqueMode)
{
  uint8_t torqueValue = 0;
  devicePrm.updateTorque = false;
  switch(torqueMode)
  {
    case ACC_TORQUE:
      devicePrm.currentTorque = devicePrm.accelTorque;
      break;
    case DEC_TORQUE:
      devicePrm.currentTorque = devicePrm.decelTorque;
      break;
    case RUN_TORQUE:
      devicePrm.currentTorque = devicePrm.runTorque;
      break;
    case HOLD_TORQUE:
      devicePrm.currentTorque = devicePrm.holdTorque;
      break;
    case CURRENT_TORQUE:
      break;
    default:
      return; //ignore error
  }
  torqueValue = devicePrm.currentTorque;
  STSpin220_Board_PwmRefSetDutyCycle(torqueValue);
}

/******************************************************//**
 * @brief Disable the power bridges (leave the output bridges HiZ)
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_Disable(void)
{
  STSpin220_Board_Disable();
}

/******************************************************//**
 * @brief Enable the power bridges
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_Enable(void)
{
  STSpin220_Board_Enable();
}

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

/******************************************************//**
 * @brief Exit STSpin220 device from standby (low power consumption)
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_ExitDeviceFromStandby(void)
{
  uint32_t sequencerPosition = devicePrm.sequencerPosition;
  
  /* Exit standby and set step mode */
  STSpin220_SetStepModeWithoutReset(devicePrm.stepMode);
  
  if (devicePrm.sequencerPosition != 0)
  {
    /* Set direction to FORWARD to ensure the HW sequencer is increased at */
    /* each step clock rising edge */
    STSpin220_SetDirection(FORWARD);
    /* Going out of standby */
    devicePrm.motionState = STANDBYTOINACTIVE;
    /* Program the step clock */
    STSpin220_Board_TimStckInit(true);
    STSpin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ);
    toggleOdd = 0;
    while (devicePrm.sequencerPosition != 0);
    while (toggleOdd!=0);
    devicePrm.sequencerPosition = sequencerPosition;    
  }
  
  devicePrm.motionState = INACTIVE;
}

/******************************************************//**
 * @brief Return the acceleration
 * @retval Acceleration in pps^2
 **********************************************************/
uint16_t STSpin220::STSpin220_GetAcceleration(void)
{                                                  
  return (devicePrm.acceleration);
}

/******************************************************//**
 * @brief Return the current speed
 * @retval Speed in pps
 **********************************************************/
uint16_t STSpin220::STSpin220_GetCurrentSpeed(void)
{
  return devicePrm.speed;
}

/******************************************************//**
 * @brief Return the deceleration
 * @retval Deceleration in pps^2
 **********************************************************/
uint16_t STSpin220::STSpin220_GetDeceleration(void)
{
  return (devicePrm.deceleration);
}

/******************************************************//**
 * @brief Return the device state
 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
 **********************************************************/
motorState_t STSpin220::STSpin220_get_device_state(void)
{
  return devicePrm.motionState;
}

/******************************************************//**
 * @brief Get the motor current direction
 * @retval direction
 **********************************************************/
motorDir_t STSpin220::STSpin220_GetDirection(void)
{
  return devicePrm.direction;
}

/******************************************************//**
 * @brief Return the FW version.
 * @retval FW version
 **********************************************************/
uint32_t STSpin220::STSpin220_GetFwVersion(void)
{
  return (STSPIN220_FW_VERSION);
}

/******************************************************//**
 * @brief Get the mark position (32b signed) 
 * @retval mark position
 **********************************************************/
int32_t STSpin220::STSpin220_GetMark(void)
{
  return devicePrm.markPosition;
}

/******************************************************//**
 * @brief Return the max speed
 * @retval maxSpeed in pps
 **********************************************************/
uint16_t STSpin220::STSpin220_GetMaxSpeed(void)
{
  return (devicePrm.maxSpeed);
}

/******************************************************//**
 * @brief Get the min speed
 * in step/s for full, half and wave modes
 * in microsteps/s for microstep modes
 * @retval return the min speed in step/s or microstep/s
 * @note
 **********************************************************/
uint16_t STSpin220::STSpin220_GetMinSpeed(void)
{
  return (devicePrm.minSpeed);
}

/******************************************************//**
 * @brief Get the current position (32b signed) 
 * @retval current position value
 **********************************************************/
int32_t STSpin220::STSpin220_GetPosition(void)
{
  return devicePrm.currentPosition;
}

/******************************************************//**
 * @brief Get the motor step mode
 * @retval step mode
 **********************************************************/
motorStepMode_t STSpin220::STSpin220_GetStepMode(void)
{
  return devicePrm.stepMode;
}

/******************************************************//**
 * @brief Get the selected stop mode
 * @retval the selected stop mode
 **********************************************************/
motorStopMode_t STSpin220::STSpin220_GetStopMode(void)
{
  return devicePrm.stopMode;
}

/******************************************************//**
 * @brief Get the torque
 * @param[in] torqueMode torque mode
 * @retval the torqueValue in % (from 0 to 100)
 * @note
 **********************************************************/
uint8_t STSpin220::STSpin220_GetTorque(motorTorqueMode_t torqueMode)
{
  uint8_t torqueValue = 0;
  switch(torqueMode)
  {
    case ACC_TORQUE:
      torqueValue = devicePrm.accelTorque;
      break;
    case DEC_TORQUE:
      torqueValue = devicePrm.decelTorque;
      break;
    case RUN_TORQUE:
      torqueValue = devicePrm.runTorque;
      break;
    case HOLD_TORQUE:
      torqueValue = devicePrm.holdTorque;
      break;
    case CURRENT_TORQUE:
      torqueValue = devicePrm.currentTorque;
      break;
    default:
      break;
  }
  return torqueValue;
}

/******************************************************//**
 * @brief Get the torque boost feature status
 * @retval TRUE if enabled, FALSE if disabled
 **********************************************************/
bool STSpin220::STSpin220_GetTorqueBoostEnable(void)
{
  return devicePrm.torqueBoostEnable;
}

/******************************************************//**
 * @brief Get the torque boost threshold
 * @retval The torque boost threshold above which the step mode is
 * changed to full step
 **********************************************************/
uint16_t STSpin220::STSpin220_GetTorqueBoostThreshold(void)
{
  return devicePrm.torqueBoostSpeedThreshold;
}

/******************************************************//**
 * @brief Go to the home position
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_GoHome(void)
{
  STSpin220_GoTo(0);
}

/******************************************************//**
 * @brief Go to the Mark position
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_GoMark(void)
{
  STSpin220_GoTo(devicePrm.markPosition);
}

/******************************************************//**
 * @brief Request the motor to move to the specified position 
 * @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                   : 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_64                   : 1/64 step
 * STEP_MODE_1_128                  : 1/128 step
 * STEP_MODE_1_256                  : 1/256 step
 **********************************************************/
void STSpin220::STSpin220_GoTo(int32_t targetPosition)
{
  motorDir_t direction;
  
  /* Exit from standby if needed */
  if (devicePrm.motionState == STANDBY)
  {
    STSpin220_ExitDeviceFromStandby();
  }
  /* Deactivate motor if needed */
  else
  {
    if (devicePrm.motionState != INACTIVE)
    { 
      STSpin220_HardHiZ();
    }
  }

  if (targetPosition > devicePrm.currentPosition)
  {
    devicePrm.stepsToTake = targetPosition -\
                                      devicePrm.currentPosition;
    if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
    {
      direction = FORWARD;
    }
    else
    {
      direction = BACKWARD;
      devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
                                        devicePrm.stepsToTake;
    }
  }
  else
  {
    devicePrm.stepsToTake = devicePrm.currentPosition -\
                                      targetPosition;
    if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
    {
      direction = BACKWARD;
    }
    else
    {
      direction = FORWARD; 
      devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
                                        devicePrm.stepsToTake;
    }
  }
  
  if (devicePrm.stepsToTake != 0) 
  {
    devicePrm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSpin220_SetDirection(direction);
    
    STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake);
    
    /* Motor activation */
    STSpin220_StartMovement();
  }
}

/******************************************************//**
 * @brief move the motor to the absolute position
 * @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_64                   : 1/64 step
 * STEP_MODE_1_128                  : 1/128 step
 * STEP_MODE_1_256                  : 1/256 step
 **********************************************************/
void STSpin220::STSpin220_GoToDir(motorDir_t direction, int32_t targetPosition)
{
  /* Exit from standby if needed */
  if (devicePrm.motionState == STANDBY)
  {
    STSpin220_ExitDeviceFromStandby();
  }
  /* Deactivate motor if needed */
  else
  {
    if (devicePrm.motionState != INACTIVE)
    { 
      STSpin220_HardHiZ();
    }
  }
  
  if (direction != BACKWARD)
  {
    if (targetPosition > devicePrm.currentPosition)
    {
      devicePrm.stepsToTake = targetPosition -\
                                        devicePrm.currentPosition;
    }
    else
    {
      devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
                                       (targetPosition -\
                                        devicePrm.currentPosition);
    }
  }
  else
  {
    if (targetPosition > devicePrm.currentPosition)
    {
      devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
                                        (devicePrm.currentPosition -\
                                         targetPosition);
    }
    else
    {
      devicePrm.stepsToTake = devicePrm.currentPosition -\
                                        targetPosition;
    }
  }

  if (devicePrm.stepsToTake != 0) 
  {
    devicePrm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSpin220_SetDirection(direction);
    
    STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake);
    
    /* Motor activation */
    STSpin220_StartMovement();
  }  
}

/******************************************************//**
 * @brief Immediately stop the motor and disables the power bridges
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_HardHiZ(void)
{
  /* Set inactive state */
  devicePrm.motionState = INACTIVE;
  
  /* Disable step clock */
  if (STSpin220_Board_TimStckStop(&toggleOdd) == 0)
  {
    STSpin220_ErrorHandler(STSPIN220_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 */
  STSpin220_Board_Delay(DISABLE_DELAY);
  
  /* Set reference voltage to 0 */
  STSpin220_SetTorque(CURRENT_TORQUE, 0);

  /* Disable power bridges */
  STSpin220_Board_Disable();
  
  /* Comeback to nominal step mode */
  if (devicePrm.stepModeLatched != devicePrm.stepMode)
  {
    STSpin220_Board_UnsetFullStep();
    devicePrm.stepMode = devicePrm.stepModeLatched;
  }

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

/******************************************************//**
 * @brief  Immediatly stop the motor
 * and either set holding torque when stop mode is HOLD_MODE,
 * or call STSpin220_HardHiz function when stop mode is HIZ_MODE,
 * or call STSpin220_PutDeviceInStandby function when stop mode is STANDBY_MODE
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_HardStop(void) 
{
  if (devicePrm.stopMode == HOLD_MODE)
  {
    /* Set inactive state */
    devicePrm.motionState = INACTIVE;

    /* Disable step clock */
    if (STSpin220_Board_TimStckStop(&toggleOdd) == 0)
    {
      STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
    }
    
    /* Set holding torque */
    STSpin220_ApplyTorque(HOLD_TORQUE);
 
    /* Comeback to nominal step mode */
    if (devicePrm.stepModeLatched != devicePrm.stepMode)
    {
      STSpin220_Board_UnsetFullStep();
      devicePrm.stepMode = devicePrm.stepModeLatched;
    }    
    
    devicePrm.commandExecuted = NO_CMD;
    devicePrm.stepsToTake = 0;  
    devicePrm.speed = 0;
  }
  else if (devicePrm.stopMode == HIZ_MODE)
  {
    STSpin220_HardHiZ();
  }
  else if (devicePrm.stopMode == STANDBY_MODE)
  {
    STSpin220_PutDeviceInStandby();
  }
}

/******************************************************//**
 * @brief  Moves the motor of the specified number of steps
 * @param[in] direction FORWARD or BACKWARD
 * @param[in] stepCount Number of steps to perform
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_Move(motorDir_t direction, uint32_t stepCount)
{
  /* Exit from standby if needed */
  if (devicePrm.motionState == STANDBY)
  {
    STSpin220_ExitDeviceFromStandby();
  }
  /* Deactivate motor if needed */
  else
  {
    if (devicePrm.motionState != INACTIVE)
    {
      STSpin220_HardHiZ();
    }
  }
  
  if (stepCount != 0) 
  {
    devicePrm.stepsToTake = stepCount;    
    devicePrm.commandExecuted = MOVE_CMD;
    
    /* Direction setup */
    STSpin220_SetDirection(direction);
    
    STSpin220_ComputeSpeedProfile(stepCount);
    
    /* Motor activation */
    STSpin220_StartMovement();
  }  
}

/******************************************************//**
 * @brief Put STSpin220 device in standby (low power consumption)
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_PutDeviceInStandby(void)
{
  /* Stop movement */
  STSpin220_HardHiZ();
  
  /* Enter standby */
  STSpin220_Board_Reset();
  
  devicePrm.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] direction FORWARD or BACKWARD
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_Run(motorDir_t direction)
{
  /* Exit from standby if needed */
  if (devicePrm.motionState == STANDBY)
  {
    STSpin220_ExitDeviceFromStandby();
  }
  /* Deactivate motor if needed */
  else
  {
    if (devicePrm.motionState != INACTIVE)
    {
      STSpin220_HardHiZ();
    }
  }
  
  /* Direction setup */
  STSpin220_SetDirection(direction);
  devicePrm.commandExecuted = RUN_CMD;
  /* Motor activation */
  STSpin220_StartMovement(); 
}

/******************************************************//**
 * @brief  Changes the acceleration
 * @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 STSpin220::STSpin220_SetAcceleration(uint16_t newAcc)
{                                                  
  bool cmdExecuted = false;
  if ((newAcc != 0)&&
      (((devicePrm.motionState & INACTIVE) == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.acceleration = newAcc;
    cmdExecuted = true;
  }    
  return cmdExecuted;
}

/******************************************************//**
 * @brief  Changes the deceleration
 * @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 STSpin220::STSpin220_SetDeceleration(uint16_t newDec)
{                                                  
  bool cmdExecuted = false;
  if ((newDec != 0)&& 
      (((devicePrm.motionState & INACTIVE) == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.deceleration = newDec;
    cmdExecuted = true;
  }      
  return cmdExecuted;
}

/******************************************************//**
 * @brief  Specifies the direction 
 * @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 STSpin220::STSpin220_SetDirection(motorDir_t dir)
{
  if ((devicePrm.motionState == INACTIVE)||\
      (devicePrm.motionState == STANDBY))
  {
    devicePrm.direction = dir;
    STSpin220_Board_SetDirectionGpio(dir);
  }
  else
  {
    if ((devicePrm.commandExecuted&RUN_CMD)!=0)
    {
      devicePrm.commandExecuted=(deviceCommand_t)
        (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted);
    }
  }
}

/******************************************************//**
 * @brief  Set current position to be the Home position
 * (current position set to 0)
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetHome(void)
{
  devicePrm.currentPosition = 0;
}

/******************************************************//**
 * @brief  Set current position to be the Mark position 
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetMark(void)
{
  devicePrm.markPosition = devicePrm.currentPosition;
}

/******************************************************//**
 * @brief  Changes the max speed
 * @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 STSpin220::STSpin220_SetMaxSpeed(uint16_t newMaxSpeed)
{
  bool cmdExecuted = false;
  if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\
      ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\
       ((devicePrm.torqueBoostEnable != false)&&\
        ((newMaxSpeed>>STSpin220_GetStepMode())<= STSPIN220_MAX_STCK_FREQ)))&&\
      (devicePrm.minSpeed <= newMaxSpeed) &&\
      (((devicePrm.motionState & INACTIVE) == INACTIVE)||\
      (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.maxSpeed = newMaxSpeed;
    cmdExecuted = true;
  }
  return cmdExecuted;
}

/******************************************************//**
 * @brief  Changes the min speed
 * @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 STSpin220::STSpin220_SetMinSpeed(uint16_t newMinSpeed)
{                                                  
  bool cmdExecuted = false;
  if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&&
      (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) &&
      (newMinSpeed <= devicePrm.maxSpeed) && 
      (((devicePrm.motionState & INACTIVE) == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.minSpeed = newMinSpeed;
    cmdExecuted = true;
  }  
  return cmdExecuted;
}

/******************************************************//**
 * @brief Set the stepping mode 
 * @param[in] stepMode from full step to 1/256 microstep
 * as specified in enum motorStepMode_t
 * @retval true if the command is successfully executed, else false
 **********************************************************/
bool STSpin220::STSpin220_SetStepMode(motorStepMode_t stepMode)
{
  /* Eventually deactivate motor */
  if ((devicePrm.motionState != INACTIVE)&&\
      (devicePrm.motionState != STANDBY))
  {
    STSpin220_HardHiZ();
  }

  /* Enter standby */
  STSpin220_Board_Reset();
  
  /* Reset the microstepping sequencer position */
  devicePrm.sequencerPosition = 0;
  
  /* Reset current and mark positions */
  devicePrm.currentPosition = 0; 
  devicePrm.markPosition = 0;
  
  /* Set the step mode */
  return (STSpin220_SetStepModeWithoutReset(stepMode));
}

/******************************************************//**
 * @brief Select the mode to stop the motor.
 * @param[in] stopMode HOLD_MODE to let power bridge enabled
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetStopMode(motorStopMode_t stopMode)
{
  devicePrm.stopMode = stopMode;
}

/******************************************************//**
 * @brief Set the torque
 * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t
 * @param[in] torqueValue in % (from 0 to 100)
 * @retval None
 * @note
 **********************************************************/
void STSpin220::STSpin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue)
{
  devicePrm.updateTorque = true;
  if (torqueValue>100) torqueValue = 100;
  switch(torqueMode)
  {
    case ACC_TORQUE:
      devicePrm.accelTorque = torqueValue;
      break;
    case DEC_TORQUE:
      devicePrm.decelTorque = torqueValue;
      break;
    case RUN_TORQUE:
      devicePrm.runTorque = torqueValue;
      break;
    case HOLD_TORQUE:
      devicePrm.holdTorque = torqueValue;
      if (devicePrm.motionState != INACTIVE)
      {
        break;
      }
    case CURRENT_TORQUE:
      devicePrm.currentTorque = torqueValue;
      STSpin220_Board_PwmRefSetDutyCycle(torqueValue);
    default:
      devicePrm.updateTorque = false;
      break; //ignore error
  }
}

/******************************************************//**
 * @brief Enable or disable the torque boost feature
 * @param[in] enable true to enable torque boost, false to disable
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetTorqueBoostEnable(bool enable)
{
  devicePrm.torqueBoostEnable = enable;
}

/******************************************************//**
 * @brief Set the torque boost threshold
 * @param[in] speedThreshold speed threshold above which the step mode is
 * changed to full step
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetTorqueBoostThreshold(uint16_t speedThreshold)
{
  devicePrm.torqueBoostSpeedThreshold = speedThreshold;
}

/******************************************************//**
 * @brief  Stops the motor by using the device deceleration
 * @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 STSpin220::STSpin220_SoftStop(void)
{   
  bool cmdExecuted = false;
  if ((devicePrm.motionState & INACTIVE) != INACTIVE)
  {
    devicePrm.commandExecuted=(deviceCommand_t)
      (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted);
    cmdExecuted = true;
  }
  return (cmdExecuted);
}

/******************************************************//**
 * @brief Get the frequency of REF PWM
 * @retval the frequency of REF PWM in Hz
 * @note
 **********************************************************/
uint32_t STSpin220::STSpin220_VrefPwmGetFreq(void)
{
  return devicePrm.refPwmFreq;
}

/******************************************************//**
 * @brief Set the frequency of REF PWM
 * @param[in] newFreq in Hz
 * @retval None
 * @note
 **********************************************************/
void STSpin220::STSpin220_VrefPwmSetFreq(uint32_t newFreq)
{ 
  devicePrm.refPwmFreq = newFreq;
  STSpin220_Board_PwmRefSetFreq(newFreq);
}

/******************************************************//**
 * @brief  Locks until the device state becomes Inactive
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_WaitWhileActive(void)
{
  /* Wait while motor is running */
  while (((STSpin220_get_device_state()&INACTIVE)!=INACTIVE)||\
   (((STSpin220_get_device_state()&INACTIVE)==INACTIVE)&&(toggleOdd!=0)));
}

/* ------------------------------------------------------------------------- */
/* Internal functions ------------------------------------------------------ */
/* ------------------------------------------------------------------------- */
/******************************************************//**
 * @brief Updates the current speed of the device
 * @param[in] newSpeed in pps
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_ApplySpeed(uint16_t newSpeed)
{
  if (devicePrm.torqueBoostEnable != false)
  {
    if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256)
    {
      STSpin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED);
    }
    if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL)
    {
      if (((newSpeed>>devicePrm.stepModeLatched)>\
           devicePrm.torqueBoostSpeedThreshold)&&\
          (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\
           ((devicePrm.stepsToTake-devicePrm.relativePos)>=\
            (1<<devicePrm.stepModeLatched))))
      {
        if ((devicePrm.sequencerPosition & 0xFF) == 0X80)
        {
          STSpin220_Board_SetFullStep();
          devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL;
          devicePrm.accu >>= devicePrm.stepModeLatched;
          newSpeed >>= devicePrm.stepModeLatched;
        }
      }
    }
    else
    {
      if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\
          (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\
           (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) == MOVE_CMD)&&\
            ((devicePrm.stepsToTake-devicePrm.relativePos)<=\
             (1<<devicePrm.stepModeLatched))))
      {
        STSpin220_Board_UnsetFullStep();
        devicePrm.stepMode = devicePrm.stepModeLatched;
        devicePrm.accu <<= devicePrm.stepModeLatched;
        newSpeed <<= devicePrm.stepModeLatched;
      }
    }
  }
  else
  {
    if (devicePrm.stepMode != devicePrm.stepModeLatched)
    {
      //torqueBoostEnable has just been disabled
      STSpin220_Board_UnsetFullStep();
      devicePrm.stepMode = devicePrm.stepModeLatched;
      devicePrm.accu <<= devicePrm.stepModeLatched;
      newSpeed <<= devicePrm.stepModeLatched;
    }
  }
  
  if (newSpeed < STSPIN220_MIN_STCK_FREQ)
  {
    newSpeed = STSPIN220_MIN_STCK_FREQ;  
  }
  if (newSpeed > STSPIN220_MAX_STCK_FREQ)
  {
    newSpeed = STSPIN220_MAX_STCK_FREQ;
  }
  
  devicePrm.speed = newSpeed;
  STSpin220_Board_TimStckSetFreq(newSpeed);
}

/******************************************************//**
 * @brief  Computes the speed profile according to the number of steps to move
 * @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 STSpin220::STSpin220_ComputeSpeedProfile(uint32_t nbSteps)
{
  uint32_t reqAccSteps;
  uint32_t reqDecSteps;
   
  /* compute the number of steps to get the targeted speed */
  uint16_t minSpeed = devicePrm.minSpeed;
  reqAccSteps = (devicePrm.maxSpeed - minSpeed);
  reqAccSteps *= (devicePrm.maxSpeed + minSpeed);
  reqDecSteps = reqAccSteps;
  reqAccSteps /= (uint32_t)devicePrm.acceleration;
  reqAccSteps /= 2;

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

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

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

  devicePrm.accu = 0;
  devicePrm.currentPosition = 0;
  devicePrm.sequencerPosition = 0;
  devicePrm.endAccPos = 0;
  devicePrm.relativePos = 0;
  devicePrm.startDecPos = 0;
  devicePrm.stepsToTake = 0;
  devicePrm.updateTorque = false;
  devicePrm.speed = 0;
  devicePrm.commandExecuted = NO_CMD;
  devicePrm.direction = FORWARD;
  tmp = devicePrm.minSpeed;
  if (((devicePrm.torqueBoostEnable != false)&&\
       (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\
      (tmp>devicePrm.maxSpeed))
  {
    STSpin220_ErrorHandler(STSPIN220_ERROR_INIT);
  }
}

/******************************************************//**
 * @brief  Set the parameters of the device to values of the structure pointed
 * by pInitDevicePrm. Set GPIO according to these values.
 * @param pInitDevicePrm pointer onto the structure containing values to
 * initialize the device parameters.
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetDeviceParamsToGivenValues(STSpin220_init_t* pInitDevicePrm)
{
  devicePrm.motionState = STANDBY;

  if (STSpin220_SetAcceleration(pInitDevicePrm->acceleration)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
  }
  if (STSpin220_SetDeceleration(pInitDevicePrm->deceleration)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
  }
  if (STSpin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
  }
  if (STSpin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
  }

  STSpin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq);
  STSpin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque);
  STSpin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque);
  STSpin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque);
  STSpin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque);
  devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
  devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
  STSpin220_SetStopMode(pInitDevicePrm->stopMode);

  STSpin220_SetDeviceParamsOtherValues();
  
  /* Set predefined step mode */
  /* Standby-reset deactivation included to latch the MODEX inputs */
  STSpin220_SetStepMode(pInitDevicePrm->stepMode);
}

/******************************************************//**
 * @brief  Sets the parameters of the device to predefined values
 * from stspin220_target_config.h
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_SetDeviceParamsToPredefinedValues(void)
{
  devicePrm.motionState = STANDBY;

  if (STSpin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
  }
  if (STSpin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
  }
  if (STSpin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
  }
  if (STSpin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false)
  {
      STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
  }

  STSpin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY);  
  STSpin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE);
  STSpin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE);
  STSpin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE);
  STSpin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE);
  devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN;
  devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH;
  STSpin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP);

  STSpin220_SetDeviceParamsOtherValues();
  
  /* Set predefined step mode */
  /* Standby-reset deactivation included to latch the MODEX inputs */
  STSpin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE);
}

/******************************************************//**
 * @brief Set the stepping mode without reset
 * @param[in] stepMode from full step to 1/256 microstep
 * as specified in enum motorStepMode_t
 * @retval true if the command is successfully executed, else false
 **********************************************************/
bool STSpin220::STSpin220_SetStepModeWithoutReset(motorStepMode_t stepMode)
{
  /* Store step mode */
  devicePrm.stepMode = stepMode;
  devicePrm.stepModeLatched = stepMode;
  
  /* Set the mode pins to the levels corresponding to the selected step mode */
  switch (stepMode)
  {
    case STEP_MODE_FULL:
      STSpin220_Board_SetFullStep();
      break;
    case STEP_MODE_HALF:
      STSpin220_Board_SetModePins(1, 0, 1, 0);
      break;    
    case STEP_MODE_1_4:
      STSpin220_Board_SetModePins(0, 1, 0, 1);
      break;        
    case STEP_MODE_1_8:
      STSpin220_Board_SetModePins(1, 1, 1, 0);
      break;
    case STEP_MODE_1_16:
      STSpin220_Board_SetModePins(1, 1, 1, 1);
      break;   
    case STEP_MODE_1_32:
      STSpin220_Board_SetModePins(0, 0, 0, 1);
      break;   
    case STEP_MODE_1_64:
      STSpin220_Board_SetModePins(1, 1, 0, 1);
      break;
    case STEP_MODE_1_128:
      STSpin220_Board_SetModePins(0, 0, 1, 0);
      break;  
    case STEP_MODE_1_256:
      STSpin220_Board_SetModePins(1, 1, 0, 0);
      break;
    default:
      return false;
  }

  /* Wait */
  STSpin220_Board_Delay(SELECT_STEP_MODE_DELAY);
  
  /* Exit standby, selected step mode is latched */
  STSpin220_Board_ReleaseReset();
  
  /* Let a delay after reset release and step mode latching*/
  STSpin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
  
  /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */
  /* after the device quit the standby condition */
  if (stepMode!=(motorStepMode_t)STEP_MODE_FULL)
  {
    STSpin220_Board_UnsetFullStep();
  }
  
  return true;
}

/******************************************************//**
 * @brief Initialises the bridge parameters to start the movement
 * and enable the power bridge
 * @retval None
 **********************************************************/
void STSpin220::STSpin220_StartMovement(void)
{
  /* Enable STSpin220 powerstage */
  STSpin220_Enable();
  toggleOdd = 0;
  devicePrm.accu = 0;
  devicePrm.relativePos = 0;
  STSpin220_Board_TimStckInit(true);  
  if ((devicePrm.endAccPos == 0)&&\
      (devicePrm.commandExecuted != RUN_CMD))
  {
    devicePrm.motionState = DECELERATING;
    STSpin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE);
  }
  else
  {
    devicePrm.motionState = ACCELERATING;
    STSpin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE);
  }
  /* Program the step clock */
  STSpin220_ApplySpeed(devicePrm.minSpeed);
}

/******************************************************//**
 * @brief  Handles the device state machine at each pulse
 * @retval None
 * @note Must only be called by the timer ISR
 **********************************************************/
void STSpin220::STSpin220_StepClockHandler(void)
{
  uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode;
  uint16_t tmp;
  STSpin220_Board_Monitor_Set();
  if (devicePrm.motionState == STANDBYTOINACTIVE)
  {
    if (toggleOdd != 0)
    {
      STSpin220_Board_StckMode3_Reset();
      toggleOdd = 0;
      if (devicePrm.sequencerPosition == 0)
      {
        if (STSpin220_Board_TimStckStop(&toggleOdd) == 0)
        {
          STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
        }
        return;
      }      
    }
    else
    {
      STSpin220_Board_StckMode3_Set();
      toggleOdd = 1;
      tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
      devicePrm.sequencerPosition -= tmp;
    }
    STSpin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ);
    return;
  }  
  
  if (toggleOdd == 0)
  {
    STSpin220_Board_StckMode3_Set();
    toggleOdd = 1;
  }
  else
  {
    STSpin220_Board_StckMode3_Reset();
    toggleOdd = 0;
    /* Incrementation of the relative position */
    devicePrm.relativePos += (1 << stepModeShift);

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

    switch (devicePrm.motionState) 
    {
      case ACCELERATING: 
      {
          uint32_t relPos = devicePrm.relativePos;
          uint32_t endAccPos = devicePrm.endAccPos;
          uint16_t speed = devicePrm.speed;
          uint32_t acc = ((uint32_t)devicePrm.acceleration << 16)>>stepModeShift;
        
          if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
              ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos)))
          {
            devicePrm.motionState = DECELERATING;
            devicePrm.accu = 0;
            /* Apply decelerating torque */
            STSpin220_ApplyTorque(DEC_TORQUE);
          }
          else
          {
            if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\
                ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
            {
              devicePrm.motionState = STEADY;
              /* Apply running torque */
              STSpin220_ApplyTorque(RUN_TORQUE);
            }
            else
            {
              bool speedUpdated = false;
              /* Go on accelerating */
              if (speed==0)
              {
                speed =1;
              }
              devicePrm.accu += acc / speed;
              while (devicePrm.accu>=(0X10000L))
              {
                devicePrm.accu -= (0X10000L);
                speed +=1;
                speedUpdated = true;
              }
          
              if (speedUpdated)
              {
                if (speed>(devicePrm.maxSpeed>>stepModeShift))
                {
                  speed = devicePrm.maxSpeed>>stepModeShift;
                }    
                devicePrm.speed = speed;
              }
            
              if (devicePrm.updateTorque!=false)
              {
                /* Apply accelerating torque */
                STSpin220_ApplyTorque(ACC_TORQUE);              
              }
            }
          }
          break;
      }
      case STEADY: 
      {
        uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift;
        uint32_t relativePos = devicePrm.relativePos;
        if (devicePrm.updateTorque!=false)
        {
          /* Apply accelerating torque */
          STSpin220_ApplyTorque(RUN_TORQUE);
        }
        if  (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
             ((devicePrm.commandExecuted==MOVE_CMD)&&\
              (relativePos>=(devicePrm.startDecPos)))||\
             ((devicePrm.commandExecuted==RUN_CMD)&&\
              (devicePrm.speed>maxSpeed)))
        {
          devicePrm.motionState = DECELERATING;
          devicePrm.accu = 0;
          /* Apply decelerating torque */
          STSpin220_ApplyTorque(DEC_TORQUE);
        }
        else
        {
          if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed))
          {
            devicePrm.motionState = ACCELERATING;
             devicePrm.accu = 0;
            /* Apply accelerating torque */
            STSpin220_ApplyTorque(ACC_TORQUE);
          }
        }
        break;
      }
      case DECELERATING: 
      {
        uint32_t relativePos = devicePrm.relativePos;
        uint16_t speed = devicePrm.speed;
        uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift;
        if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\
             (speed<=(devicePrm.minSpeed>>stepModeShift)))||\
            ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake)))
        {
          /* Motion process complete */
          if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0)
          {
            devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted);
            if (devicePrm.direction==BACKWARD)
            {
                devicePrm.direction=FORWARD;
            }
            else
            {
                devicePrm.direction=BACKWARD;
            }
            STSpin220_Board_SetDirectionGpio(devicePrm.direction);
            if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0)
            {
              devicePrm.motionState = ACCELERATING;
              devicePrm.accu = 0;
              /* Apply accelerating torque */
              STSpin220_ApplyTorque(ACC_TORQUE);
              break;
            }
          }
          if (devicePrm.stopMode==HOLD_MODE)
          {
            STSpin220_HardStop();
          }
          else if (devicePrm.stopMode==STANDBY_MODE)
          {
            STSpin220_PutDeviceInStandby();
          }
          else
          {
            STSpin220_HardHiZ();
          }
        }
        else
        {
          if ((devicePrm.commandExecuted==RUN_CMD)&&
              (speed<=(devicePrm.maxSpeed>>stepModeShift)))
          {
            devicePrm.motionState = STEADY;
            /* Apply running torque */
            STSpin220_ApplyTorque(RUN_TORQUE);
          }
          else
          {
            /* Go on decelerating */
            if (speed>(devicePrm.minSpeed>>stepModeShift))
            {
              bool speedUpdated = false;
              if (speed==0)
              {
                  speed =1;
              }
              devicePrm.accu += dec / speed;
              while (devicePrm.accu>=(0X10000L))
              {
                devicePrm.accu -= (0X10000L);
                if (speed>1)
                {  
                  speed -=1;
                }
                speedUpdated = true;
              }
          
              if (speedUpdated)
              {
                if (speed<(devicePrm.minSpeed>>stepModeShift))
                {
                  speed = devicePrm.minSpeed>>stepModeShift;
                }  
                devicePrm.speed = speed;
              }
            
              if (devicePrm.updateTorque!=false)
              {
                /* Apply decelerating torque */
                STSpin220_ApplyTorque(DEC_TORQUE);
              }
            }
          }
        }
        break;
      }
      default: 
      {
        break;
      }
    }
  }
  if ((devicePrm.motionState & INACTIVE) != INACTIVE)
  {
    STSpin220_ApplySpeed(devicePrm.speed);
  }
  else
  {
    if (STSpin220_Board_TimStckStop(&toggleOdd) == 0)
    {
      STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
    }
  }
  STSpin220_Board_Monitor_Reset();
}

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/