Diff: Components/stspin220/STSpin220.cpp

Revision:

3:a132aa6d66e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Components/stspin220/STSpin220.cpp	Fri Mar 24 10:59:19 2017 +0100
@@ -0,0 +1,1528 @@
+/**
+  ******************************************************************************
+  * @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_get_step_mode(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_get_step_mode())<= 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****/