Davide Aliprandi
/
X_NUCLEO_IHM14A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: Components/STSPIN820/STSPIN820.cpp
- Revision:
- 1:bc265521eb00
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Components/STSPIN820/STSPIN820.cpp Fri Apr 27 17:02:08 2018 +0000
@@ -0,0 +1,1799 @@
+/**
+ ******************************************************************************
+ * @file STSPIN820.cpp
+ * @author STM
+ * @version V1.0.0
+ * @date August 7th, 2017
+ * @brief STSPIN820 driver (fully integrated microstepping motor driver)
+ * @note (C) COPYRIGHT 2017 STMicroelectronics
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+
+/* Generated with STM32CubeTOO -----------------------------------------------*/
+
+
+/* Revision ------------------------------------------------------------------*/
+/*
+ Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
+ Branch/Trunk/Tag: trunk
+ Based on: X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.c
+ Revision: 0
+*/
+
+
+/* Includes ------------------------------------------------------------------*/
+
+#include "STSPIN820.h"
+
+/* Private function prototypes -----------------------------------------------*/
+
+/** @defgroup STSPIN820_Private_Functions STSPIN820 Private Functions
+ * @{
+ */
+/* Methods -------------------------------------------------------------------*/
+
+/** @defgroup STSPIN820_Exported_Variables STSPIN820 Exported Variables
+ * @{
+ */
+
+/** @defgroup STSPIN820_Library_Functions STSPIN820 Library Functions
+ * @{
+ */
+
+/******************************************************//**
+ * @brief Return motor handle (pointer to the STSPIN820 motor driver structure)
+ * @retval Pointer to the MOTOR_vt_t structure
+ **********************************************************/
+MOTOR_vt_t* STSPIN820::STSPIN820_GetMotorHandle(void)
+{
+ return (&STSPIN820_drv);
+}
+
+/******************************************************//**
+ * @brief Start the STSPIN820 library
+ * @param[in] pInit pointer to the initialization data
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_Init(void* pInit)
+{
+ device_instance++;
+
+ /* Initialise the GPIOs */
+ STSPIN820_Board_GpioInit();
+
+ /* Initialise the timer used for the step clock and ------------------------*/
+ /* the PWM for the reference voltage generation ----------------------------*/
+ STSPIN820_Board_TimStckInit();
+ STSPIN820_Board_PwmRefInit();
+
+ if (pInit == 0)
+ {
+ /* Set all context variables to the predefined values */
+ /* from STSPIN820_config.h */
+ STSPIN820_SetDeviceParamsToPredefinedValues();
+ }
+ else
+ {
+ STSPIN820_SetDeviceParamsToGivenValues((STSPIN820_init_t*) pInit);
+ }
+}
+
+/******************************************************//**
+ * @brief Read id
+ * @retval Id of the STSPIN820 Driver Instance
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_ReadId(void)
+{
+ return (device_instance);
+}
+
+/******************************************************//**
+ * @brief Attach a user callback to the error Handler.
+ * The call back will be then called each time the library
+ * detects an error
+ * @param[in] callback Name of the callback to attach
+ * to the error Hanlder
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_AttachErrorHandler(void (*callback)(uint16_t error))
+{
+ error_handler_callback = (void (*)(uint16_t error))callback;
+}
+
+/******************************************************//**
+ * @brief Attach a user callback to the flag Interrupt
+ * The call back will be then called each time the status
+ * flag pin will be pulled down due to the occurrence of
+ * a programmed alarms ( OCD, thermal pre-warning or
+ * shutdown, UVLO, wrong command, non-performable command)
+ * @param[in] callback Name of the callback to attach
+ * to the Flag Interrupt
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_AttachFlagInterrupt(void (*callback)(void))
+{
+ flag_interrupt_callback = (void (*)(void))callback;
+}
+
+/******************************************************//**
+ * @brief Check if STSPIN820 has a fault by reading EN pin position.
+ * @retval One if STSPIN820 has EN pin down, otherwise zero
+ **********************************************************/
+uint8_t STSPIN820::STSPIN820_CheckStatusHw(void)
+{
+ if(!STSPIN820_Board_EN_AND_FAULT_PIN_GetState())
+ {
+ return 0x01;
+ }
+ else
+ {
+ return 0x00;
+ }
+}
+
+/******************************************************//**
+ * @brief Disable the power bridges (leave the output bridges HiZ)
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_Disable(uint8_t deviceId)
+{
+ STSPIN820_Board_Disable();
+}
+
+/******************************************************//**
+ * @brief Enable the power bridges
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_Enable(uint8_t deviceId)
+{
+ STSPIN820_Board_Enable();
+}
+
+/******************************************************//**
+ * @brief Error handler which calls the user callback (if defined)
+ * @param[in] error Number of the error
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_ErrorHandler(uint16_t error)
+{
+ if (error_handler_callback != 0)
+ {
+ (void) error_handler_callback(error);
+ }
+ else
+ {
+ while(1)
+ {
+ /* Infinite loop */
+ }
+ }
+}
+
+/******************************************************//**
+ * @brief Exit STSPIN820 device from standby (low power consumption)
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_ExitDeviceFromStandby(uint8_t deviceId)
+{
+ uint32_t sequencerPosition = device_prm.sequencerPosition;
+
+ /* Exit standby and set step mode */
+ /* Disable step clock */
+ if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
+ }
+
+ /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
+ /* after the last step clock rising edge triggering the last step */
+ STSPIN820_Board_Delay(DISABLE_DELAY);
+ /* Set reference voltage to 0 */
+ STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);
+
+ /* Disable power bridges */
+ STSPIN820_Board_Disable();
+
+ device_prm.commandExecuted = NO_CMD;
+ device_prm.stepsToTake = 0;
+ device_prm.speed = 0;
+
+ /* Reset the microstepping sequencer position */
+ device_prm.sequencerPosition = 0;
+
+ /* Reset current and mark positions */
+ device_prm.currentPosition = 0;
+ device_prm.markPosition = 0;
+
+ STSPIN820_SetStepMode(deviceId, device_prm.stepMode);
+ STSPIN820_Board_ReleaseReset();
+ STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
+
+ STSPIN820_SetHome(deviceId);
+ STSPIN820_SetMark(deviceId);
+
+ if (device_prm.sequencerPosition != 0)
+ {
+ /* Set direction to FORWARD to ensure the HW sequencer is increased at */
+ /* each step clock rising edge */
+ STSPIN820_SetDirection(0, FORWARD);
+ /* Going out of standby */
+ device_prm.motionState = STANDBYTOINACTIVE;
+ /* Initialize the step clock timer */
+ STSPIN820_Board_TimStckInit();
+ /* Program the step clock */
+ STSPIN820_Board_TimStckCompareInit();
+ STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
+ toggle_odd = 0;
+ STSPIN820_Board_TimStckStart();
+ while (device_prm.sequencerPosition != 0);
+ while (toggle_odd!=0);
+ device_prm.sequencerPosition = sequencerPosition;
+ }
+
+ device_prm.motionState = INACTIVE;
+}
+
+/******************************************************//**
+ * @brief Return the acceleration of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval Acceleration in pps^2
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetAcceleration(int8_t deviceId)
+{
+ return (device_prm.acceleration);
+}
+
+/******************************************************//**
+ * @brief Return the current speed of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval Speed in pps
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetCurrentSpeed(int8_t deviceId)
+{
+ return device_prm.speed;
+}
+
+/******************************************************//**
+ * @brief Return the decay mode of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval Decay Mode State (SLOW or MIXED)
+ **********************************************************/
+motor_decay_mode_t STSPIN820::STSPIN820_GetDecayMode(uint8_t deviceId)
+{
+ motor_decay_mode_t status;
+ if (STSPIN820_Board_GetDecayGpio() != 0)
+ {
+ status = SLOW_DECAY;
+ }
+ else
+ {
+ status = MIXED_DECAY;
+ }
+
+ return status;
+}
+
+/******************************************************//**
+ * @brief Return the deceleration of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval Deceleration in pps^2
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetDeceleration(uint8_t deviceId)
+{
+ return (device_prm.deceleration);
+}
+
+/******************************************************//**
+ * @brief Return the device state
+ * @param[in] deviceId Unused parameter
+ * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
+ **********************************************************/
+motor_state_t STSPIN820::STSPIN820_GetDeviceState(uint8_t deviceId)
+{
+ return device_prm.motionState;
+}
+
+/******************************************************//**
+ * @brief Get the motor current direction
+ * @param[in] deviceId Unused parameter
+ * @retval direction
+ **********************************************************/
+motor_direction_t STSPIN820::STSPIN820_GetDirection(uint8_t deviceId)
+{
+ return device_prm.direction;
+}
+
+/******************************************************//**
+ * @brief Return the FW version of the library
+ * @retval Stspin220_FW_VERSION
+ **********************************************************/
+uint32_t STSPIN820::STSPIN820_GetFwVersion(void)
+{
+ return (STSPIN820_FW_VERSION);
+}
+
+/******************************************************//**
+ * @brief Return the mark position of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval mark position value
+ **********************************************************/
+int32_t STSPIN820::STSPIN820_GetMark(uint8_t deviceId)
+{
+ return device_prm.markPosition;
+}
+
+/******************************************************//**
+ * @brief Return the max speed of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval maxSpeed in pps
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetMaxSpeed(uint8_t deviceId)
+{
+ return (device_prm.maxSpeed);
+}
+
+/******************************************************//**
+ * @brief Return the min speed of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval minSpeed in pps
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetMinSpeed(uint8_t deviceId)
+{
+ return (device_prm.minSpeed);
+}
+
+/******************************************************//**
+ * @brief Returns the number of devices
+ * @retval number of devices
+ **********************************************************/
+uint8_t STSPIN820::STSPIN820_GetNbDevices(void)
+{
+ return (number_of_devices);
+}
+
+/******************************************************//**
+ * @brief Return the current position value of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval current position value
+ **********************************************************/
+int32_t STSPIN820::STSPIN820_GetPosition(uint8_t deviceId)
+{
+ return device_prm.currentPosition;
+}
+
+/******************************************************//**
+ * @brief Get the motor step mode
+ * @param[in] deviceId Unused parameter
+ * @retval step mode
+ **********************************************************/
+motor_step_mode_t STSPIN820::STSPIN820_GetStepMode(uint8_t deviceId)
+{
+ return device_prm.stepMode;
+}
+
+/******************************************************//**
+ * @brief Get the selected stop mode
+ * @param[in] deviceId Unused parameter
+ * @retval the selected stop mode
+ **********************************************************/
+motor_stop_mode_t STSPIN820::STSPIN820_GetStopMode(uint8_t deviceId)
+{
+ return device_prm.stopMode;
+}
+
+/******************************************************//**
+ * @brief Get the torque of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] torqueMode torque mode
+ * @retval the torqueValue in % (from 0 to 100)
+ * @note
+ **********************************************************/
+uint8_t STSPIN820::STSPIN820_GetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
+{
+ uint8_t torqueValue = 0;
+ switch(torqueMode)
+ {
+ case ACC_TORQUE:
+ torqueValue = device_prm.accelTorque;
+ break;
+ case DEC_TORQUE:
+ torqueValue = device_prm.decelTorque;
+ break;
+ case RUN_TORQUE:
+ torqueValue = device_prm.runTorque;
+ break;
+ case HOLD_TORQUE:
+ torqueValue = device_prm.holdTorque;
+ break;
+ case CURRENT_TORQUE:
+ torqueValue = device_prm.currentTorque;
+ break;
+ default:
+ break;
+ }
+ return torqueValue;
+}
+
+/******************************************************//**
+ * @brief Get the torque boost feature status
+ * @param[in] deviceId Unused parameter
+ * @retval TRUE if enabled, FALSE if disabled
+ **********************************************************/
+bool STSPIN820::STSPIN820_GetTorqueBoostEnable(uint8_t deviceId)
+{
+ return device_prm.torqueBoostEnable;
+}
+
+/******************************************************//**
+ * @brief Get the torque boost threshold
+ * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
+ * @retval the torque boost threshold above which the step mode is
+ * changed to full step
+ **********************************************************/
+uint16_t STSPIN820::STSPIN820_GetTorqueBoostThreshold(uint8_t deviceId)
+{
+ return device_prm.torqueBoostSpeedThreshold;
+}
+
+/******************************************************//**
+ * @brief Request the motor to move to the home position (ABS_POSITION = 0)
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_GoHome(uint8_t deviceId)
+{
+ deviceId = 0;
+
+ STSPIN820_GoTo(deviceId, 0);
+}
+
+/******************************************************//**
+ * @brief Request the motor to move to the mark position
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_GoMark(uint8_t deviceId)
+{
+ deviceId = 0;
+
+ STSPIN820_GoTo(deviceId, device_prm.markPosition);
+}
+
+/******************************************************//**
+ * @brief Request the motor to move to the specified position
+ * @param[in] deviceId Unused parameter
+ * @param[in] targetPosition absolute position in steps
+ * @retval None
+ * @note The position is at the resolution corresponding to the
+ * selected step mode.
+ * STEP_MODE_FULL : Full step
+ * STEP_MODE_HALF : 1/2 step
+ * STEP_MODE_1_4 : 1/4 step
+ * STEP_MODE_1_8 : 1/8 step
+ * STEP_MODE_1_16 : 1/16 step
+ * STEP_MODE_1_32 : 1/32 step
+ * STEP_MODE_1_128 : 1/128 step
+ * STEP_MODE_1_256 : 1/256 step
+ * @note The 1/64 step mode is not allowed
+ **********************************************************/
+void STSPIN820::STSPIN820_GoTo(uint8_t deviceId, int32_t targetPosition)
+{
+ motor_direction_t direction;
+ deviceId = 0;
+
+ /* Exit from standby if needed */
+ if (device_prm.motionState == STANDBY)
+ {
+ STSPIN820_ExitDeviceFromStandby(deviceId);
+ }
+ /* Deactivate motor if needed */
+ else if (device_prm.motionState != INACTIVE)
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+
+ if (targetPosition > device_prm.currentPosition)
+ {
+ device_prm.stepsToTake = targetPosition -\
+ device_prm.currentPosition;
+ if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
+ {
+ direction = FORWARD;
+ }
+ else
+ {
+ direction = BACKWARD;
+ device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
+ device_prm.stepsToTake;
+ }
+ }
+ else
+ {
+ device_prm.stepsToTake = device_prm.currentPosition -\
+ targetPosition;
+ if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
+ {
+ direction = BACKWARD;
+ }
+ else
+ {
+ direction = FORWARD;
+ device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
+ device_prm.stepsToTake;
+ }
+ }
+
+ if (device_prm.stepsToTake != 0)
+ {
+ device_prm.commandExecuted = MOVE_CMD;
+
+ /* Direction setup */
+ STSPIN820_SetDirection(deviceId, direction);
+
+ STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
+
+ /* Motor activation */
+ STSPIN820_StartMovement(deviceId);
+ }
+}
+
+/******************************************************//**
+ * @brief Move the motor to the absolute position
+ * @param[in] deviceId Unused parameter
+ * @param[in] direction FORWARD or BACKWARD
+ * @param[in] targetPosition 32 bit signed value position
+ * @retval None
+ * @note The position is at the resolution corresponding to the
+ * selected step mode.
+ * STEP_MODE_FULL : step
+ * STEP_MODE_HALF : 1/2 step
+ * STEP_MODE_1_4 : 1/4 step
+ * STEP_MODE_1_8 : 1/8 step
+ * STEP_MODE_1_16 : 1/16 step
+ * STEP_MODE_1_32 : 1/32 step
+ * STEP_MODE_1_128 : 1/128 step
+ * STEP_MODE_1_256 : 1/256 step
+ * @note The 1/64 step mode is not allowed
+ **********************************************************/
+void STSPIN820::STSPIN820_GoToDir(uint8_t deviceId, motor_direction_t direction, int32_t targetPosition)
+{
+ deviceId = 0;
+
+ /* Exit from standby if needed */
+ if (device_prm.motionState == STANDBY)
+ {
+ STSPIN820_ExitDeviceFromStandby(deviceId);
+ }
+ /* Deactivate motor if needed */
+ else if (device_prm.motionState != INACTIVE)
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+
+ if (direction != BACKWARD)
+ {
+ if (targetPosition > device_prm.currentPosition)
+ {
+ device_prm.stepsToTake = targetPosition -\
+ device_prm.currentPosition;
+ }
+ else
+ {
+ device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
+ (targetPosition -\
+ device_prm.currentPosition);
+ }
+ }
+ else
+ {
+ if (targetPosition > device_prm.currentPosition)
+ {
+ device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
+ (device_prm.currentPosition -\
+ targetPosition);
+ }
+ else
+ {
+ device_prm.stepsToTake = device_prm.currentPosition -\
+ targetPosition;
+ }
+ }
+
+ if (device_prm.stepsToTake != 0)
+ {
+ device_prm.commandExecuted = MOVE_CMD;
+
+ /* Direction setup */
+ STSPIN820_SetDirection(deviceId, direction);
+
+ STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
+
+ /* Motor activation */
+ STSPIN820_StartMovement(deviceId);
+ }
+}
+
+/******************************************************//**
+ * @brief Immediatly stop the motor and disable the power bridge
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_HardHiZ(uint8_t deviceId)
+{
+ /* Set inactive state */
+ device_prm.motionState = INACTIVE;
+
+ /* Disable step clock */
+ if (STSPIN820_Board_TimStckStop(handle &toggle_odd) == 0)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
+ }
+
+ /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
+ /* after the last step clock rising edge triggering the last step */
+ STSPIN820_Board_Delay(DISABLE_DELAY);
+
+ /* Set reference voltage to 0 */
+ STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);
+
+ /* Disable power bridges */
+ STSPIN820_Board_Disable();
+
+ /* Comeback to nominal step mode */
+ if (device_prm.stepModeLatched != device_prm.stepMode)
+ {
+ motor_step_mode_t StepMode = device_prm.stepModeLatched;
+ STSPIN820_SetStepMode(0, StepMode);
+ device_prm.stepMode = device_prm.stepModeLatched;
+ }
+
+ device_prm.commandExecuted = NO_CMD;
+ device_prm.stepsToTake = 0;
+ device_prm.speed = 0;
+}
+
+/******************************************************//**
+ * @brief Immediatly stop the motor
+ * and either set holding torque when stop mode is HOLD_MODE,
+ * or call STSPIN820_HardHiz function when stop mode is HIZ_MODE,
+ * or call STSPIN820_PutDeviceInStandby function when stop mode is STANDBY_MODE
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_HardStop(uint8_t deviceId)
+{
+ deviceId = 0;
+
+ if (device_prm.stopMode == HOLD_MODE)
+ {
+ /* Set inactive state */
+ device_prm.motionState = INACTIVE;
+
+ /* Disable step clock */
+ if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
+ }
+
+ /* Set holding torque */
+ STSPIN820_ApplyTorque(deviceId, HOLD_TORQUE);
+
+ /* Comeback to nominal step mode */
+ if (device_prm.stepModeLatched != device_prm.stepMode)
+ {
+ motor_step_mode_t StepMode = device_prm.stepModeLatched;
+ STSPIN820_SetStepMode(0, StepMode);
+ device_prm.stepMode = device_prm.stepModeLatched;
+ }
+
+ device_prm.commandExecuted = NO_CMD;
+ device_prm.stepsToTake = 0;
+ device_prm.speed = 0;
+ }
+ else if (device_prm.stopMode == HIZ_MODE)
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+ else if (device_prm.stopMode == STANDBY_MODE)
+ {
+ STSPIN820_PutDeviceInStandby(deviceId);
+ }
+}
+
+/******************************************************//**
+ * @brief Moves the motor of the specified number of steps
+ * @param[in] deviceId Unused parameter
+ * @param[in] direction FORWARD or BACKWARD
+ * @param[in] stepCount Number of steps to perform
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_Move(uint8_t deviceId, motor_direction_t direction, uint32_t stepCount)
+{
+ deviceId = 0;
+
+ /* Exit from standby if needed */
+ if (device_prm.motionState == STANDBY)
+ {
+ STSPIN820_ExitDeviceFromStandby(deviceId);
+ }
+ /* Deactivate motor if needed */
+ else if (device_prm.motionState != INACTIVE)
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+
+ if (stepCount != 0)
+ {
+ device_prm.stepsToTake = stepCount;
+ device_prm.commandExecuted = MOVE_CMD;
+
+ /* Direction setup */
+ STSPIN820_SetDirection(deviceId, direction);
+
+ STSPIN820_ComputeSpeedProfile(deviceId, stepCount);
+
+ /* Motor activation */
+ STSPIN820_StartMovement(deviceId);
+ }
+}
+
+/******************************************************//**
+ * @brief Put STSPIN820 device in standby (low power consumption)
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_PutDeviceInStandby(uint8_t deviceId)
+{
+ /* Stop movement */
+ STSPIN820_HardHiZ(deviceId);
+
+ /* Enter standby */
+ STSPIN820_Board_Reset();
+
+ device_prm.motionState = STANDBY;
+}
+
+/******************************************************//**
+ * @brief Runs the motor. It will accelerate from the min
+ * speed up to the max speed by using the device acceleration.
+ * @param[in] deviceId Unused parameter
+ * @param[in] direction FORWARD or BACKWARD
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_Run(uint8_t deviceId, motor_direction_t direction)
+{
+ /* Exit from standby if needed */
+ if (device_prm.motionState == STANDBY)
+ {
+ STSPIN820_ExitDeviceFromStandby(deviceId);
+ }
+ /* Deactivate motor if needed */
+ else if (device_prm.motionState != INACTIVE)
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+
+ /* Direction setup */
+ STSPIN820_SetDirection(deviceId,direction);
+ device_prm.commandExecuted = RUN_CMD;
+ /* Motor activation */
+ STSPIN820_StartMovement(deviceId);
+}
+
+/******************************************************//**
+ * @brief Changes the acceleration of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newAcc New acceleration to apply in pps^2
+ * @retval true if the command is successfully executed, else false
+ * @note The command is not performed if the device is executing
+ * a MOVE or GOTO command (but it can be used during a RUN command)
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetAcceleration(uint8_t deviceId, uint16_t newAcc)
+{
+ bool cmdExecuted = FALSE;
+ if ((newAcc != 0)&&
+ (((device_prm.motionState & INACTIVE) == INACTIVE)||
+ (device_prm.commandExecuted == RUN_CMD)))
+ {
+ device_prm.acceleration = newAcc;
+ cmdExecuted = TRUE;
+ }
+ return cmdExecuted;
+}
+
+/******************************************************//**
+ * @brief Specifies the decay mode
+ * @param[in] deviceId Unused parameter
+ * @param[in] decay SLOW_DECAY or MIXED_DECAY
+ * @retval true if the command is successfully executed, else false
+ **********************************************************/
+void STSPIN820::STSPIN820_SetDecayMode(uint8_t deviceId, motor_decay_mode_t decay)
+{
+ if (decay == SLOW_DECAY)
+ {
+ STSPIN820_Board_SetDecayGpio(1);
+ }
+ else if (decay == MIXED_DECAY)
+ {
+ STSPIN820_Board_SetDecayGpio(0);
+ }
+}
+
+/******************************************************//**
+ * @brief Changes the deceleration of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newDec New deceleration to apply in pps^2
+ * @retval true if the command is successfully executed, else false
+ * @note The command is not performed if the device is executing
+ * a MOVE or GOTO command (but it can be used during a RUN command)
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetDeceleration(uint8_t deviceId, uint16_t newDec)
+{
+ bool cmdExecuted = FALSE;
+ if ((newDec != 0)&&
+ (((device_prm.motionState & INACTIVE) == INACTIVE)||
+ (device_prm.commandExecuted == RUN_CMD)))
+ {
+ device_prm.deceleration = newDec;
+ cmdExecuted = TRUE;
+ }
+ return cmdExecuted;
+}
+
+/******************************************************//**
+ * @brief Specifies the direction
+ * @param[in] deviceId Unused parameter
+ * @param[in] dir FORWARD or BACKWARD
+ * @note The direction change is applied if the device
+ * is in INACTIVE or STANDBY state or if the device is
+ * executing a run command
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetDirection(uint8_t deviceId, motor_direction_t dir)
+{
+ if ((device_prm.motionState == INACTIVE)||\
+ (device_prm.motionState == STANDBY))
+ {
+ device_prm.direction = dir;
+ STSPIN820_Board_SetDirectionGpio(dir);
+ }
+ else if ((device_prm.commandExecuted&RUN_CMD)!=0)
+ {
+ device_prm.commandExecuted|=STSPIN820_DIR_CHANGE_BIT_MASK;
+ }
+}
+
+/******************************************************//**
+ * @brief Set current position to be the Home position
+ * (current position set to 0)
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetHome(uint8_t deviceId)
+{
+ device_prm.currentPosition = 0;
+}
+
+/******************************************************//**
+ * @brief Set current position to be the Mark position
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetMark(uint8_t deviceId)
+{
+ device_prm.markPosition = device_prm.currentPosition;
+}
+
+/******************************************************//**
+ * @brief Changes the max speed of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newMaxSpeed New max speed to apply in pps
+ * @retval true if the command is successfully executed, else false
+ * @note The command is not performed is the device is executing
+ * a MOVE or GOTO command (but it can be used during a RUN command).
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed)
+{
+ bool cmdExecuted = FALSE;
+ if ((newMaxSpeed >= STSPIN820_MIN_STCK_FREQ)&&\
+ ((newMaxSpeed <= STSPIN820_MAX_STCK_FREQ)||\
+ ((device_prm.torqueBoostEnable != FALSE)&&\
+ ((newMaxSpeed>>STSPIN820_GetStepMode(deviceId))<= STSPIN820_MAX_STCK_FREQ)))&&\
+ (device_prm.minSpeed <= newMaxSpeed) &&\
+ (((device_prm.motionState & INACTIVE) == INACTIVE)||\
+ (device_prm.commandExecuted == RUN_CMD)))
+ {
+ device_prm.maxSpeed = newMaxSpeed;
+ cmdExecuted = TRUE;
+ }
+ return cmdExecuted;
+}
+
+/******************************************************//**
+ * @brief Changes the min speed of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newMinSpeed New min speed to apply in pps
+ * @retval true if the command is successfully executed, else false
+ * @note The command is not performed is the device is executing
+ * a MOVE or GOTO command (but it can be used during a RUN command).
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed)
+{
+ bool cmdExecuted = FALSE;
+ if ((newMinSpeed >= STSPIN820_MIN_STCK_FREQ)&&
+ (newMinSpeed <= STSPIN820_MAX_STCK_FREQ) &&
+ (newMinSpeed <= device_prm.maxSpeed) &&
+ (((device_prm.motionState & INACTIVE) == INACTIVE)||
+ (device_prm.commandExecuted == RUN_CMD)))
+ {
+ device_prm.minSpeed = newMinSpeed;
+ cmdExecuted = TRUE;
+ }
+ return cmdExecuted;
+}
+
+/******************************************************//**
+ * @brief Sets the number of devices to be used
+ * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES)
+ * @retval TRUE if successfull, FALSE if failure, attempt to set a number of
+ * devices greater than MAX_NUMBER_OF_DEVICES
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetNbDevices(uint8_t nbDevices)
+{
+ if (nbDevices <= MAX_NUMBER_OF_DEVICES)
+ {
+ number_of_devices = nbDevices;
+ return TRUE;
+ }
+ else
+ {
+ return FALSE;
+ }
+}
+
+/******************************************************//**
+ * @brief Set the stepping mode
+ * @param[in] deviceId Unused parameter
+ * @param[in] stepMode from full step to 1/256 microstep
+ * as specified in enum motor_step_mode_t
+ * 1/64 microstep mode not supported by STSPIN820
+ * @retval true if the command is successfully executed, else false
+ **********************************************************/
+bool STSPIN820::STSPIN820_SetStepMode(uint8_t deviceId, motor_step_mode_t stepMode)
+{
+ /* Store step mode */
+ device_prm.stepMode = stepMode;
+ device_prm.stepModeLatched = stepMode;
+
+ /* Set the mode pins to the levels corresponding to the selected step mode */
+ switch (stepMode)
+ {
+ case STEP_MODE_FULL:
+ STSPIN820_Board_SetFullStep();
+ break;
+ case STEP_MODE_HALF:
+ STSPIN820_Board_SetModePins(1, 0, 0);
+ break;
+ case STEP_MODE_1_4:
+ STSPIN820_Board_SetModePins(0, 1, 0);
+ break;
+ case STEP_MODE_1_8:
+ STSPIN820_Board_SetModePins(1, 1, 0);
+ break;
+ case STEP_MODE_1_16:
+ STSPIN820_Board_SetModePins(0, 0, 1);
+ break;
+ case STEP_MODE_1_32:
+ STSPIN820_Board_SetModePins(1, 0, 1);
+ break;
+ case STEP_MODE_1_128:
+ STSPIN820_Board_SetModePins(0, 1, 1);
+ break;
+ case STEP_MODE_1_256:
+ STSPIN820_Board_SetModePins(1, 1, 1);
+ break;
+ default:
+ return FALSE;
+ }
+
+ return TRUE;
+
+}
+
+/******************************************************//**
+ * @brief Select the mode to stop the motor.
+ * @param[in] deviceId Unused parameter
+ * @param[in] stopMode HOLD_MODE to let power bridge enabled
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetStopMode(uint8_t deviceId, motor_stop_mode_t stopMode)
+{
+ device_prm.stopMode = stopMode;
+}
+
+/******************************************************//**
+ * @brief Set the torque of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] torqueMode Torque mode as specified in enum motor_torque_mode_t
+ * @param[in] torqueValue in % (from 0 to 100)
+ * @retval None
+ * @note
+ **********************************************************/
+void STSPIN820::STSPIN820_SetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode, uint8_t torqueValue)
+{
+ device_prm.updateTorque = TRUE;
+ if (torqueValue>100)
+ {
+ torqueValue = 100;
+ }
+ switch(torqueMode)
+ {
+ case ACC_TORQUE:
+ device_prm.accelTorque = torqueValue;
+ break;
+ case DEC_TORQUE:
+ device_prm.decelTorque = torqueValue;
+ break;
+ case RUN_TORQUE:
+ device_prm.runTorque = torqueValue;
+ break;
+ case HOLD_TORQUE:
+ device_prm.holdTorque = torqueValue;
+ if (device_prm.motionState != INACTIVE)
+ {
+ break;
+ }
+ case CURRENT_TORQUE:
+ device_prm.currentTorque = torqueValue;
+ STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
+ default:
+ device_prm.updateTorque = FALSE;
+ break; //ignore error
+ }
+}
+
+/******************************************************//**
+ * @brief Enable or disable the torque boost feature
+ * @param[in] deviceId Unused parameter
+ * @param[in] enable true to enable torque boost, false to disable
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetTorqueBoostEnable(uint8_t deviceId, bool enable)
+{
+ device_prm.torqueBoostEnable = enable;
+}
+
+/******************************************************//**
+ * @brief Set the torque boost threshold
+ * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
+ * @param[in] speedThreshold speed threshold above which the step mode is
+ * changed to full step
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetTorqueBoostThreshold(uint8_t deviceId, uint16_t speedThreshold)
+{
+ device_prm.torqueBoostSpeedThreshold = speedThreshold;
+}
+
+/******************************************************//**
+ * @brief Stops the motor by using the device deceleration
+ * @param[in] deviceId Unused parameter
+ * @retval true if the command is successfully executed, else false
+ * @note The command is not performed if the device is in INACTIVE,
+ * STANDBYTOINACTIVE or STANDBY state.
+ **********************************************************/
+bool STSPIN820::STSPIN820_SoftStop(uint8_t deviceId)
+{
+ bool cmdExecuted = FALSE;
+ if ((device_prm.motionState & INACTIVE) != INACTIVE)
+ {
+ device_prm.commandExecuted |= STSPIN820_SOFT_STOP_BIT_MASK;
+ cmdExecuted = TRUE;
+ }
+ return (cmdExecuted);
+}
+
+/******************************************************//**
+ * @brief Get the frequency of REF PWM of the specified device
+ * @param[in] deviceId Unused parameter
+ * @retval the frequency of REF PWM in Hz
+ * @note
+ **********************************************************/
+uint32_t STSPIN820::STSPIN820_VrefPwmGetFreq(uint8_t deviceId)
+{
+ return device_prm.refPwmFreq;
+}
+
+/******************************************************//**
+ * @brief Set the frequency of REF PWM of the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newFreq in Hz
+ * @retval None
+ * @note
+ **********************************************************/
+void STSPIN820::STSPIN820_VrefPwmSetFreq(uint8_t deviceId, uint32_t newFreq)
+{
+ device_prm.refPwmFreq = newFreq;
+ STSPIN820_Board_PwmRefSetFreqAndDutyCycle(newFreq,device_prm.currentTorque);
+}
+
+/******************************************************//**
+ * @brief Locks until the device state becomes Inactive
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_WaitWhileActive(uint8_t deviceId)
+{
+ /* Wait while motor is running */
+ while (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)!=INACTIVE)||\
+ (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)==INACTIVE)&&(toggle_odd!=0)));
+}
+
+/******************************************************//**
+ * @brief Updates the current speed of the device
+ * @param[in] deviceId Unused parameter
+ * @param[in] newSpeed in pps
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_ApplySpeed(uint8_t deviceId, uint16_t newSpeed)
+{
+ if (device_prm.torqueBoostEnable != FALSE)
+ {
+ if (device_prm.stepMode > STEP_MODE_1_256)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_APPLY_SPEED);
+ }
+ if (device_prm.stepMode != STEP_MODE_FULL)
+ {
+ if (((newSpeed>>device_prm.stepModeLatched)>\
+ device_prm.torqueBoostSpeedThreshold)&&\
+ (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) != MOVE_CMD) ||\
+ ((device_prm.stepsToTake-device_prm.relativePos)>=\
+ (1<<device_prm.stepModeLatched))))
+ {
+ if ((device_prm.sequencerPosition & 0xFF) == 0x80)
+ {
+ STSPIN820_Board_SetFullStep();
+ device_prm.stepMode = STEP_MODE_FULL;
+ device_prm.accu >>= device_prm.stepModeLatched;
+ newSpeed >>= device_prm.stepModeLatched;
+ }
+ }
+ }
+ else if (((newSpeed <= device_prm.torqueBoostSpeedThreshold) &&\
+ (device_prm.stepModeLatched != STEP_MODE_FULL))||\
+ (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) == MOVE_CMD)&&\
+ ((device_prm.stepsToTake-device_prm.relativePos)<=\
+ (1<<device_prm.stepModeLatched))))
+ {
+ STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
+ device_prm.stepMode = device_prm.stepModeLatched;
+ device_prm.accu <<= device_prm.stepModeLatched;
+ newSpeed <<= device_prm.stepModeLatched;
+ }
+ }
+ else if (device_prm.stepMode != device_prm.stepModeLatched)
+ {
+ //torqueBoostEnable has just been disabled
+ STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
+ device_prm.stepMode = device_prm.stepModeLatched;
+ device_prm.accu <<= device_prm.stepModeLatched;
+ newSpeed <<= device_prm.stepModeLatched;
+ }
+
+ if (newSpeed < STSPIN820_MIN_STCK_FREQ)
+ {
+ newSpeed = STSPIN820_MIN_STCK_FREQ;
+ }
+ if (newSpeed > STSPIN820_MAX_STCK_FREQ)
+ {
+ newSpeed = STSPIN820_MAX_STCK_FREQ;
+ }
+
+ device_prm.speed = newSpeed;
+ STSPIN820_Board_TimStckSetFreq(newSpeed);
+
+}
+
+/******************************************************//**
+ * @brief Apply the set torque to the specified device
+ * @param[in] deviceId Unused parameter
+ * @param[in] torqueMode torque mode
+ * @retval None
+ * @note
+ **********************************************************/
+void STSPIN820::STSPIN820_ApplyTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
+{
+ uint8_t torqueValue = 0;
+ device_prm.updateTorque = FALSE;
+ switch(torqueMode)
+ {
+ case ACC_TORQUE:
+ device_prm.currentTorque = device_prm.accelTorque;
+ break;
+ case DEC_TORQUE:
+ device_prm.currentTorque = device_prm.decelTorque;
+ break;
+ case RUN_TORQUE:
+ device_prm.currentTorque = device_prm.runTorque;
+ break;
+ case HOLD_TORQUE:
+ device_prm.currentTorque = device_prm.holdTorque;
+ break;
+ case CURRENT_TORQUE:
+ break;
+ default:
+ return; //ignore error
+ }
+ torqueValue = device_prm.currentTorque;
+ STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
+}
+
+/******************************************************//**
+ * @brief Computes the speed profile according to the number of steps to move
+ * @param[in] deviceId Unused parameter
+ * @param[in] nbSteps number of steps to perform
+ * @retval None
+ * @note Using the acceleration and deceleration of the device,
+ * this function determines the duration in steps of the acceleration,
+ * steady and deceleration phases.
+ * If the total number of steps to perform is big enough, a trapezoidal move
+ * is performed (i.e. there is a steady phase where the motor runs at the maximum
+ * speed.
+ * Else, a triangular move is performed (no steady phase: the maximum speed is never
+ * reached.
+ **********************************************************/
+void STSPIN820::STSPIN820_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps)
+{
+ uint32_t reqAccSteps;
+ uint32_t reqDecSteps;
+
+ /* compute the number of steps to get the targeted speed */
+ uint16_t minSpeed = device_prm.minSpeed;
+ reqAccSteps = (device_prm.maxSpeed - minSpeed);
+ reqAccSteps *= (device_prm.maxSpeed + minSpeed);
+ reqDecSteps = reqAccSteps;
+ reqAccSteps /= (uint32_t)device_prm.acceleration;
+ reqAccSteps /= 2;
+
+ /* compute the number of steps to stop */
+ reqDecSteps /= (uint32_t)device_prm.deceleration;
+ reqDecSteps /= 2;
+
+ if(( reqAccSteps + reqDecSteps ) > nbSteps)
+ {
+ /* Triangular move */
+ /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
+ uint32_t dec = device_prm.deceleration;
+ uint32_t acc = device_prm.acceleration;
+
+ reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec);
+ if (reqDecSteps > 1)
+ {
+ reqAccSteps = reqDecSteps - 1;
+ if(reqAccSteps == 0)
+ {
+ reqAccSteps = 1;
+ }
+ }
+ else
+ {
+ reqAccSteps = 0;
+ }
+ device_prm.endAccPos = reqAccSteps;
+ device_prm.startDecPos = reqDecSteps;
+ }
+ else
+ {
+ /* Trapezoidal move */
+ /* accelerating phase to endAccPos */
+ /* steady phase from endAccPos to startDecPos */
+ /* decelerating from startDecPos to stepsToTake*/
+ device_prm.endAccPos = reqAccSteps;
+ device_prm.startDecPos = nbSteps - reqDecSteps - 1;
+ }
+}
+
+/******************************************************//**
+ * @brief Handlers of the flag interrupt which calls the user callback (if defined)
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_FlagInterruptHandler(void)
+{
+ if (flag_interrupt_callback != 0)
+ {
+ flag_interrupt_callback();
+ }
+}
+
+/******************************************************//**
+ * @brief Set the parameters of the device whose values are not defined in
+ * STSPIN820_config.h
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetDeviceParamsOtherValues(void)
+{
+ uint16_t tmp;
+
+ device_prm.accu = 0;
+ device_prm.currentPosition = 0;
+ device_prm.sequencerPosition = 0;
+ device_prm.endAccPos = 0;
+ device_prm.relativePos = 0;
+ device_prm.startDecPos = 0;
+ device_prm.stepsToTake = 0;
+ device_prm.updateTorque = FALSE;
+ device_prm.speed = 0;
+ device_prm.commandExecuted = NO_CMD;
+ device_prm.direction = FORWARD;
+ tmp = device_prm.minSpeed;
+ if (((device_prm.torqueBoostEnable != FALSE)&&\
+ (device_prm.torqueBoostSpeedThreshold>STSPIN820_MAX_STCK_FREQ))||\
+ (tmp>device_prm.maxSpeed))
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_INIT);
+ }
+}
+
+/******************************************************//**
+ * @brief Set the parameters of the device to values of initDevicePrm structure
+ * @param pInitDevicePrm structure containing values to initialize the device
+ * parameters
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* pInitDevicePrm)
+{
+ device_prm.motionState = STANDBY;;
+
+ if (STSPIN820_SetAcceleration(0,pInitDevicePrm->acceleration)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
+ }
+ if (STSPIN820_SetDeceleration(0,pInitDevicePrm->deceleration)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
+ }
+ if (STSPIN820_SetMaxSpeed(0,pInitDevicePrm->maxSpeed)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
+ }
+ if (STSPIN820_SetMinSpeed(0,pInitDevicePrm->minSpeed)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
+ }
+
+ STSPIN820_VrefPwmSetFreq(0,pInitDevicePrm->vrefPwmFreq);
+ STSPIN820_SetTorque(0,ACC_TORQUE,pInitDevicePrm->accelTorque);
+ STSPIN820_SetTorque(0,DEC_TORQUE,pInitDevicePrm->decelTorque);
+ STSPIN820_SetTorque(0,RUN_TORQUE,pInitDevicePrm->runTorque);
+ STSPIN820_SetTorque(0,HOLD_TORQUE,pInitDevicePrm->holdTorque);
+ device_prm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
+ device_prm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
+ STSPIN820_SetStopMode(0,pInitDevicePrm->stopMode);
+
+ STSPIN820_SetDeviceParamsOtherValues();
+
+ /* Eventually deactivate motor */
+ if ((device_prm.motionState != INACTIVE)&&\
+ (device_prm.motionState != STANDBY))
+ {
+ STSPIN820_HardHiZ(0);
+ }
+
+ /* Enter standby */
+ STSPIN820_Board_Reset();
+
+ /* Reset the microstepping sequencer position */
+ device_prm.sequencerPosition = 0;
+
+ /* Reset current and mark positions */
+ device_prm.currentPosition = 0;
+ device_prm.markPosition = 0;
+
+ /* Set predefined step mode */
+ STSPIN820_SetStepMode(0, pInitDevicePrm->stepMode);
+
+ /* Wait */
+ STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
+
+ /* Exit standby */
+ STSPIN820_Board_ReleaseReset();
+
+ /* Let a delay after reset release*/
+ STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
+
+}
+
+/******************************************************//**
+ * @brief Sets the parameters of the device to predefined values
+ * from STSPIN820_config.h
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_SetDeviceParamsToPredefinedValues(void)
+{
+ device_prm.motionState = STANDBY;
+
+ if (STSPIN820_SetAcceleration(0,STSPIN820_CONF_PARAM_ACC)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
+ }
+ if (STSPIN820_SetDeceleration(0,STSPIN820_CONF_PARAM_DEC)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
+ }
+ if (STSPIN820_SetMaxSpeed(0,STSPIN820_CONF_PARAM_RUNNING_SPEED)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
+ }
+ if (STSPIN820_SetMinSpeed(0,STSPIN820_CONF_PARAM_MIN_SPEED)==FALSE)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
+ }
+
+ STSPIN820_VrefPwmSetFreq(0,STSPIN820_CONF_PARAM_REF_PWM_FREQUENCY);
+ STSPIN820_SetTorque(0,ACC_TORQUE,STSPIN820_CONF_PARAM_ACC_TORQUE);
+ STSPIN820_SetTorque(0,DEC_TORQUE,STSPIN820_CONF_PARAM_DEC_TORQUE);
+ STSPIN820_SetTorque(0,RUN_TORQUE,STSPIN820_CONF_PARAM_RUNNING_TORQUE);
+ STSPIN820_SetTorque(0,HOLD_TORQUE,STSPIN820_CONF_PARAM_HOLDING_TORQUE);
+ device_prm.torqueBoostEnable = STSPIN820_CONF_PARAM_TORQUE_BOOST_EN;
+ device_prm.torqueBoostSpeedThreshold = STSPIN820_CONF_PARAM_TORQUE_BOOST_TH;
+ STSPIN820_SetStopMode(0,STSPIN820_CONF_PARAM_AUTO_HIZ_STOP);
+
+ STSPIN820_SetDeviceParamsOtherValues();
+
+ /* Eventually deactivate motor */
+ if ((device_prm.motionState != INACTIVE)&&\
+ (device_prm.motionState != STANDBY))
+ {
+ STSPIN820_HardHiZ(0);
+ }
+
+ /* Enter standby */
+ STSPIN820_Board_Reset();
+
+ /* Reset the microstepping sequencer position */
+ device_prm.sequencerPosition = 0;
+
+ /* Reset current and mark positions */
+ device_prm.currentPosition = 0;
+ device_prm.markPosition = 0;
+
+ /* Set predefined step mode */
+ STSPIN820_SetStepMode(0, STSPIN820_CONF_PARAM_STEP_MODE);
+
+ /* Wait */
+ STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
+
+ /* Exit standby */
+ STSPIN820_Board_ReleaseReset();
+
+ /* Let a delay after reset release*/
+ STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
+}
+
+/******************************************************//**
+ * @brief Initialises the bridge parameters to start the movement
+ * and enable the power bridge
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ **********************************************************/
+void STSPIN820::STSPIN820_StartMovement(uint8_t deviceId)
+{
+ deviceId = 0;
+
+ /* Enable STSPIN820 powerstage */
+ STSPIN820_Enable(deviceId);
+ toggle_odd = 0;
+ device_prm.accu = 0;
+ device_prm.relativePos = 0;
+ if ((device_prm.endAccPos == 0)&&\
+ (device_prm.commandExecuted != RUN_CMD))
+ {
+ device_prm.motionState = DECELERATING;
+ STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
+ }
+ else
+ {
+ device_prm.motionState = ACCELERATING;
+ STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
+ }
+ STSPIN820_Board_PwmRefStart();
+ /* Initialize the step clock timer */
+ STSPIN820_Board_TimStckInit();
+ /* Program the step clock */
+ STSPIN820_Board_TimStckCompareInit();
+ STSPIN820_ApplySpeed(deviceId, device_prm.minSpeed);
+ STSPIN820_Board_TimStckStart();
+}
+
+/******************************************************//**
+ * @brief Handles the device state machine at each pulse
+ * @param[in] deviceId Unused parameter
+ * @retval None
+ * @note Must only be called by the timer ISR
+ **********************************************************/
+void STSPIN820::STSPIN820_StepClockHandler(uint8_t deviceId)
+{
+ uint32_t stepModeShift = device_prm.stepModeLatched - device_prm.stepMode;
+ uint16_t tmp;
+ deviceId = 0;
+
+ if (device_prm.motionState == STANDBYTOINACTIVE)
+ {
+ if (toggle_odd != 0)
+ {
+ toggle_odd = 0;
+ if (device_prm.sequencerPosition == 0)
+ {
+ if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
+ }
+ return;
+ }
+ }
+ else
+ {
+ toggle_odd = 1;
+ tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
+ device_prm.sequencerPosition -= tmp;
+ }
+ STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
+ return;
+ }
+
+ if (toggle_odd == 0)
+ {
+ toggle_odd = 1;
+ }
+ else
+ {
+ toggle_odd = 0;
+ /* Incrementation of the relative position */
+ device_prm.relativePos += (1 << stepModeShift);
+
+ /* Incrementation of the current position */
+ if (device_prm.direction != BACKWARD)
+ {
+ device_prm.currentPosition += (1 << stepModeShift);
+ tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
+ device_prm.sequencerPosition += tmp;
+ if (device_prm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
+ {
+ device_prm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
+ }
+ }
+ else
+ {
+ device_prm.currentPosition -= (1 << stepModeShift);
+ tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
+ device_prm.sequencerPosition -= tmp;
+ if (device_prm.sequencerPosition < 0)
+ {
+ device_prm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
+ }
+ }
+
+ switch (device_prm.motionState)
+ {
+ case ACCELERATING:
+ {
+ uint32_t relPos = device_prm.relativePos;
+ uint32_t endAccPos = device_prm.endAccPos;
+ uint16_t speed = device_prm.speed;
+ uint32_t acc = ((uint32_t)device_prm.acceleration << 16)>>stepModeShift;
+
+ if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
+ ((device_prm.commandExecuted==MOVE_CMD)&&(relPos>=device_prm.startDecPos)))
+ {
+ device_prm.motionState = DECELERATING;
+ device_prm.accu = 0;
+ /* Apply decelerating torque */
+ STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
+ }
+ else if ((speed>=(device_prm.maxSpeed>>stepModeShift))||\
+ ((device_prm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
+ {
+ device_prm.motionState = STEADY;
+ /* Apply running torque */
+ STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
+ }
+ else
+ {
+ bool speedUpdated = FALSE;
+ /* Go on accelerating */
+ if (speed==0)
+ {
+ speed =1;
+ }
+ device_prm.accu += acc / speed;
+ while (device_prm.accu>=(0X10000L))
+ {
+ device_prm.accu -= (0X10000L);
+ speed +=1;
+ speedUpdated = TRUE;
+ }
+
+ if (speedUpdated)
+ {
+ if (speed>(device_prm.maxSpeed>>stepModeShift))
+ {
+ speed = device_prm.maxSpeed>>stepModeShift;
+ }
+ device_prm.speed = speed;
+ }
+
+ if (device_prm.updateTorque!=FALSE)
+ {
+ /* Apply accelerating torque */
+ STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
+ }
+ }
+ break;
+ }
+ case STEADY:
+ {
+ uint16_t maxSpeed = device_prm.maxSpeed>>stepModeShift;
+ uint32_t relativePos = device_prm.relativePos;
+ if (device_prm.updateTorque!=FALSE)
+ {
+ /* Apply accelerating torque */
+ STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
+ }
+ if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
+ ((device_prm.commandExecuted==MOVE_CMD)&&\
+ (relativePos>=(device_prm.startDecPos)))||\
+ ((device_prm.commandExecuted==RUN_CMD)&&\
+ (device_prm.speed>maxSpeed)))
+ {
+ device_prm.motionState = DECELERATING;
+ device_prm.accu = 0;
+ /* Apply decelerating torque */
+ STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
+ }
+ else if ((device_prm.commandExecuted==RUN_CMD)&&(device_prm.speed<maxSpeed))
+ {
+ device_prm.motionState = ACCELERATING;
+ device_prm.accu = 0;
+ /* Apply accelerating torque */
+ STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
+ }
+ break;
+ }
+ case DECELERATING:
+ {
+ uint32_t relativePos = device_prm.relativePos;
+ uint16_t speed = device_prm.speed;
+ uint32_t dec = ((uint32_t)device_prm.deceleration << 16)>>stepModeShift;
+ if ((((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)&&\
+ (speed<=(device_prm.minSpeed>>stepModeShift)))||\
+ ((device_prm.commandExecuted==MOVE_CMD)&&(relativePos>=device_prm.stepsToTake)))
+ {
+ /* Motion process complete */
+ if ((device_prm.commandExecuted&STSPIN820_DIR_CHANGE_BIT_MASK)!=0)
+ {
+ device_prm.commandExecuted&=~STSPIN820_DIR_CHANGE_BIT_MASK;
+ if (device_prm.direction==BACKWARD)
+ {
+ device_prm.direction=FORWARD;
+ }
+ else device_prm.direction=BACKWARD;
+ STSPIN820_Board_SetDirectionGpio(device_prm.direction);
+ if ((device_prm.commandExecuted&STSPIN820_SOFT_STOP_BIT_MASK)==0)
+ {
+ device_prm.motionState = ACCELERATING;
+ device_prm.accu = 0;
+ /* Apply accelerating torque */
+ STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
+ break;
+ }
+ }
+ if (device_prm.stopMode==HOLD_MODE)
+ {
+ STSPIN820_HardStop(deviceId);
+ }
+ else if (device_prm.stopMode==STANDBY_MODE)
+ {
+ STSPIN820_PutDeviceInStandby(deviceId);
+ }
+ else
+ {
+ STSPIN820_HardHiZ(deviceId);
+ }
+ }
+ else if ((device_prm.commandExecuted==RUN_CMD)&&
+ (speed<=(device_prm.maxSpeed>>stepModeShift)))
+ {
+ device_prm.motionState = STEADY;
+ /* Apply running torque */
+ STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
+ }
+ else
+ {
+ /* Go on decelerating */
+ if (speed>(device_prm.minSpeed>>stepModeShift))
+ {
+ bool speedUpdated = FALSE;
+ if (speed==0)
+ {
+ speed = 1;
+ }
+ device_prm.accu += dec / speed;
+ while (device_prm.accu>=(0X10000L))
+ {
+ device_prm.accu -= (0X10000L);
+ if (speed>1)
+ {
+ speed -=1;
+ }
+ speedUpdated = TRUE;
+ }
+
+ if (speedUpdated)
+ {
+ if (speed<(device_prm.minSpeed>>stepModeShift))
+ {
+ speed = device_prm.minSpeed>>stepModeShift;
+ }
+ device_prm.speed = speed;
+ }
+
+ if (device_prm.updateTorque!=FALSE)
+ {
+ /* Apply decelerating torque */
+ STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
+ }
+ }
+ }
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+ }
+ if ((device_prm.motionState & INACTIVE) != INACTIVE)
+ {
+ STSPIN820_ApplySpeed(deviceId, device_prm.speed);
+ }
+ else
+ {
+ if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
+ {
+ STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
+ }
+ }
+}
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/