ST
Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.
Dependencies: ST_INTERFACES
Dependents: HelloWorld_IHM06A1
Fork of
X-NUCLEO-IHM06A1
by 
ST Expansion SW Team
Motor Control Library
Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.
It features the:
- Read and write of the device parameters; GPIO, PWM and IRQ configuration; microstepping, direction position, speed, acceleration, deceleration and torque controls
- Automatic full-step switch management; high impedance or hold stop mode selection; enable and standby management
- Fault interrupts handling (over current, short-circuit and over temperature)
- Command locking until the device completes movement
The API allows to easily:
- perform various positioning, moves and stops
- get/set or monitor the motor positions
- set the home position and mark another position
- get/set the minimum and maximum speed
- get the current speed
- get/set the acceleration and deceleration
- get/set the stop mode (hold, hiz or standby)
- get/set the torque
- get/set the torque boost
- get/set the step mode (up to 1/256)
Platform compatibility
Compatible platforms have been tested with the configurations provided by the HelloWorld_IHM06A1 example.
Diff: Components/stspin220/STSpin220.cpp
- Revision:
- 4:265c30b9112a
- Parent:
- 3:a132aa6d66e4
- Child:
- 5:fd1315beea32
--- a/Components/stspin220/STSpin220.cpp Fri Mar 24 10:59:19 2017 +0100
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,1528 +0,0 @@
-/**
- ******************************************************************************
- * @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>© 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****/
X-NUCLEO-IHM06A1 Low Voltage Stepper Motor Driver