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
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:
- 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>© 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****/