ST Expansion SW Team / 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.

Dependencies:   ST_INTERFACES

Dependents:   HelloWorld_IHM14A1

Fork of X_NUCLEO_IHM14A1 by Davide Aliprandi

Components/STSPIN820/STSPIN820.cpp@1:bc265521eb00, 2018-04-27 (annotated)

Committer:
Davidroid
Date:
Fri Apr 27 17:02:08 2018 +0000
Revision:
1:bc265521eb00
Child:
2:4fd08b67958c
Populate

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 1:bc265521eb00 1 /**
Davidroid 1:bc265521eb00 2 ******************************************************************************
Davidroid 1:bc265521eb00 3 * @file STSPIN820.cpp
Davidroid 1:bc265521eb00 4 * @author STM
Davidroid 1:bc265521eb00 5 * @version V1.0.0
Davidroid 1:bc265521eb00 6 * @date August 7th, 2017
Davidroid 1:bc265521eb00 7 * @brief STSPIN820 driver (fully integrated microstepping motor driver)
Davidroid 1:bc265521eb00 8 * @note (C) COPYRIGHT 2017 STMicroelectronics
Davidroid 1:bc265521eb00 9 ******************************************************************************
Davidroid 1:bc265521eb00 10 * @attention
Davidroid 1:bc265521eb00 11 *
Davidroid 1:bc265521eb00 12 * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
Davidroid 1:bc265521eb00 13 *
Davidroid 1:bc265521eb00 14 * Redistribution and use in source and binary forms, with or without modification,
Davidroid 1:bc265521eb00 15 * are permitted provided that the following conditions are met:
Davidroid 1:bc265521eb00 16 * 1. Redistributions of source code must retain the above copyright notice,
Davidroid 1:bc265521eb00 17 * this list of conditions and the following disclaimer.
Davidroid 1:bc265521eb00 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid 1:bc265521eb00 19 * this list of conditions and the following disclaimer in the documentation
Davidroid 1:bc265521eb00 20 * and/or other materials provided with the distribution.
Davidroid 1:bc265521eb00 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid 1:bc265521eb00 22 * may be used to endorse or promote products derived from this software
Davidroid 1:bc265521eb00 23 * without specific prior written permission.
Davidroid 1:bc265521eb00 24 *
Davidroid 1:bc265521eb00 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid 1:bc265521eb00 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid 1:bc265521eb00 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid 1:bc265521eb00 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid 1:bc265521eb00 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid 1:bc265521eb00 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid 1:bc265521eb00 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid 1:bc265521eb00 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid 1:bc265521eb00 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid 1:bc265521eb00 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid 1:bc265521eb00 35 *
Davidroid 1:bc265521eb00 36 ******************************************************************************
Davidroid 1:bc265521eb00 37 */
Davidroid 1:bc265521eb00 38
Davidroid 1:bc265521eb00 39
Davidroid 1:bc265521eb00 40 /* Generated with STM32CubeTOO -----------------------------------------------*/
Davidroid 1:bc265521eb00 41
Davidroid 1:bc265521eb00 42
Davidroid 1:bc265521eb00 43 /* Revision ------------------------------------------------------------------*/
Davidroid 1:bc265521eb00 44 /*
Davidroid 1:bc265521eb00 45 Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
Davidroid 1:bc265521eb00 46 Branch/Trunk/Tag: trunk
Davidroid 1:bc265521eb00 47 Based on: X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.c
Davidroid 1:bc265521eb00 48 Revision: 0
Davidroid 1:bc265521eb00 49 */
Davidroid 1:bc265521eb00 50
Davidroid 1:bc265521eb00 51
Davidroid 1:bc265521eb00 52 /* Includes ------------------------------------------------------------------*/
Davidroid 1:bc265521eb00 53
Davidroid 1:bc265521eb00 54 #include "STSPIN820.h"
Davidroid 1:bc265521eb00 55
Davidroid 1:bc265521eb00 56 /* Private function prototypes -----------------------------------------------*/
Davidroid 1:bc265521eb00 57
Davidroid 1:bc265521eb00 58 /** @defgroup STSPIN820_Private_Functions STSPIN820 Private Functions
Davidroid 1:bc265521eb00 59 * @{
Davidroid 1:bc265521eb00 60 */
Davidroid 1:bc265521eb00 61 /* Methods -------------------------------------------------------------------*/
Davidroid 1:bc265521eb00 62
Davidroid 1:bc265521eb00 63 /** @defgroup STSPIN820_Exported_Variables STSPIN820 Exported Variables
Davidroid 1:bc265521eb00 64 * @{
Davidroid 1:bc265521eb00 65 */
Davidroid 1:bc265521eb00 66
Davidroid 1:bc265521eb00 67 /** @defgroup STSPIN820_Library_Functions STSPIN820 Library Functions
Davidroid 1:bc265521eb00 68 * @{
Davidroid 1:bc265521eb00 69 */
Davidroid 1:bc265521eb00 70
Davidroid 1:bc265521eb00 71 /******************************************************//**
Davidroid 1:bc265521eb00 72 * @brief Return motor handle (pointer to the STSPIN820 motor driver structure)
Davidroid 1:bc265521eb00 73 * @retval Pointer to the MOTOR_vt_t structure
Davidroid 1:bc265521eb00 74 **********************************************************/
Davidroid 1:bc265521eb00 75 MOTOR_vt_t* STSPIN820::STSPIN820_GetMotorHandle(void)
Davidroid 1:bc265521eb00 76 {
Davidroid 1:bc265521eb00 77 return (&STSPIN820_drv);
Davidroid 1:bc265521eb00 78 }
Davidroid 1:bc265521eb00 79
Davidroid 1:bc265521eb00 80 /******************************************************//**
Davidroid 1:bc265521eb00 81 * @brief Start the STSPIN820 library
Davidroid 1:bc265521eb00 82 * @param[in] pInit pointer to the initialization data
Davidroid 1:bc265521eb00 83 * @retval None
Davidroid 1:bc265521eb00 84 **********************************************************/
Davidroid 1:bc265521eb00 85 void STSPIN820::STSPIN820_Init(void* pInit)
Davidroid 1:bc265521eb00 86 {
Davidroid 1:bc265521eb00 87 device_instance++;
Davidroid 1:bc265521eb00 88
Davidroid 1:bc265521eb00 89 /* Initialise the GPIOs */
Davidroid 1:bc265521eb00 90 STSPIN820_Board_GpioInit();
Davidroid 1:bc265521eb00 91
Davidroid 1:bc265521eb00 92 /* Initialise the timer used for the step clock and ------------------------*/
Davidroid 1:bc265521eb00 93 /* the PWM for the reference voltage generation ----------------------------*/
Davidroid 1:bc265521eb00 94 STSPIN820_Board_TimStckInit();
Davidroid 1:bc265521eb00 95 STSPIN820_Board_PwmRefInit();
Davidroid 1:bc265521eb00 96
Davidroid 1:bc265521eb00 97 if (pInit == 0)
Davidroid 1:bc265521eb00 98 {
Davidroid 1:bc265521eb00 99 /* Set all context variables to the predefined values */
Davidroid 1:bc265521eb00 100 /* from STSPIN820_config.h */
Davidroid 1:bc265521eb00 101 STSPIN820_SetDeviceParamsToPredefinedValues();
Davidroid 1:bc265521eb00 102 }
Davidroid 1:bc265521eb00 103 else
Davidroid 1:bc265521eb00 104 {
Davidroid 1:bc265521eb00 105 STSPIN820_SetDeviceParamsToGivenValues((STSPIN820_init_t*) pInit);
Davidroid 1:bc265521eb00 106 }
Davidroid 1:bc265521eb00 107 }
Davidroid 1:bc265521eb00 108
Davidroid 1:bc265521eb00 109 /******************************************************//**
Davidroid 1:bc265521eb00 110 * @brief Read id
Davidroid 1:bc265521eb00 111 * @retval Id of the STSPIN820 Driver Instance
Davidroid 1:bc265521eb00 112 **********************************************************/
Davidroid 1:bc265521eb00 113 uint16_t STSPIN820::STSPIN820_ReadId(void)
Davidroid 1:bc265521eb00 114 {
Davidroid 1:bc265521eb00 115 return (device_instance);
Davidroid 1:bc265521eb00 116 }
Davidroid 1:bc265521eb00 117
Davidroid 1:bc265521eb00 118 /******************************************************//**
Davidroid 1:bc265521eb00 119 * @brief Attach a user callback to the error Handler.
Davidroid 1:bc265521eb00 120 * The call back will be then called each time the library
Davidroid 1:bc265521eb00 121 * detects an error
Davidroid 1:bc265521eb00 122 * @param[in] callback Name of the callback to attach
Davidroid 1:bc265521eb00 123 * to the error Hanlder
Davidroid 1:bc265521eb00 124 * @retval None
Davidroid 1:bc265521eb00 125 **********************************************************/
Davidroid 1:bc265521eb00 126 void STSPIN820::STSPIN820_AttachErrorHandler(void (*callback)(uint16_t error))
Davidroid 1:bc265521eb00 127 {
Davidroid 1:bc265521eb00 128 error_handler_callback = (void (*)(uint16_t error))callback;
Davidroid 1:bc265521eb00 129 }
Davidroid 1:bc265521eb00 130
Davidroid 1:bc265521eb00 131 /******************************************************//**
Davidroid 1:bc265521eb00 132 * @brief Attach a user callback to the flag Interrupt
Davidroid 1:bc265521eb00 133 * The call back will be then called each time the status
Davidroid 1:bc265521eb00 134 * flag pin will be pulled down due to the occurrence of
Davidroid 1:bc265521eb00 135 * a programmed alarms ( OCD, thermal pre-warning or
Davidroid 1:bc265521eb00 136 * shutdown, UVLO, wrong command, non-performable command)
Davidroid 1:bc265521eb00 137 * @param[in] callback Name of the callback to attach
Davidroid 1:bc265521eb00 138 * to the Flag Interrupt
Davidroid 1:bc265521eb00 139 * @retval None
Davidroid 1:bc265521eb00 140 **********************************************************/
Davidroid 1:bc265521eb00 141 void STSPIN820::STSPIN820_AttachFlagInterrupt(void (*callback)(void))
Davidroid 1:bc265521eb00 142 {
Davidroid 1:bc265521eb00 143 flag_interrupt_callback = (void (*)(void))callback;
Davidroid 1:bc265521eb00 144 }
Davidroid 1:bc265521eb00 145
Davidroid 1:bc265521eb00 146 /******************************************************//**
Davidroid 1:bc265521eb00 147 * @brief Check if STSPIN820 has a fault by reading EN pin position.
Davidroid 1:bc265521eb00 148 * @retval One if STSPIN820 has EN pin down, otherwise zero
Davidroid 1:bc265521eb00 149 **********************************************************/
Davidroid 1:bc265521eb00 150 uint8_t STSPIN820::STSPIN820_CheckStatusHw(void)
Davidroid 1:bc265521eb00 151 {
Davidroid 1:bc265521eb00 152 if(!STSPIN820_Board_EN_AND_FAULT_PIN_GetState())
Davidroid 1:bc265521eb00 153 {
Davidroid 1:bc265521eb00 154 return 0x01;
Davidroid 1:bc265521eb00 155 }
Davidroid 1:bc265521eb00 156 else
Davidroid 1:bc265521eb00 157 {
Davidroid 1:bc265521eb00 158 return 0x00;
Davidroid 1:bc265521eb00 159 }
Davidroid 1:bc265521eb00 160 }
Davidroid 1:bc265521eb00 161
Davidroid 1:bc265521eb00 162 /******************************************************//**
Davidroid 1:bc265521eb00 163 * @brief Disable the power bridges (leave the output bridges HiZ)
Davidroid 1:bc265521eb00 164 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 165 * @retval None
Davidroid 1:bc265521eb00 166 **********************************************************/
Davidroid 1:bc265521eb00 167 void STSPIN820::STSPIN820_Disable(uint8_t deviceId)
Davidroid 1:bc265521eb00 168 {
Davidroid 1:bc265521eb00 169 STSPIN820_Board_Disable();
Davidroid 1:bc265521eb00 170 }
Davidroid 1:bc265521eb00 171
Davidroid 1:bc265521eb00 172 /******************************************************//**
Davidroid 1:bc265521eb00 173 * @brief Enable the power bridges
Davidroid 1:bc265521eb00 174 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 175 * @retval None
Davidroid 1:bc265521eb00 176 **********************************************************/
Davidroid 1:bc265521eb00 177 void STSPIN820::STSPIN820_Enable(uint8_t deviceId)
Davidroid 1:bc265521eb00 178 {
Davidroid 1:bc265521eb00 179 STSPIN820_Board_Enable();
Davidroid 1:bc265521eb00 180 }
Davidroid 1:bc265521eb00 181
Davidroid 1:bc265521eb00 182 /******************************************************//**
Davidroid 1:bc265521eb00 183 * @brief Error handler which calls the user callback (if defined)
Davidroid 1:bc265521eb00 184 * @param[in] error Number of the error
Davidroid 1:bc265521eb00 185 * @retval None
Davidroid 1:bc265521eb00 186 **********************************************************/
Davidroid 1:bc265521eb00 187 void STSPIN820::STSPIN820_ErrorHandler(uint16_t error)
Davidroid 1:bc265521eb00 188 {
Davidroid 1:bc265521eb00 189 if (error_handler_callback != 0)
Davidroid 1:bc265521eb00 190 {
Davidroid 1:bc265521eb00 191 (void) error_handler_callback(error);
Davidroid 1:bc265521eb00 192 }
Davidroid 1:bc265521eb00 193 else
Davidroid 1:bc265521eb00 194 {
Davidroid 1:bc265521eb00 195 while(1)
Davidroid 1:bc265521eb00 196 {
Davidroid 1:bc265521eb00 197 /* Infinite loop */
Davidroid 1:bc265521eb00 198 }
Davidroid 1:bc265521eb00 199 }
Davidroid 1:bc265521eb00 200 }
Davidroid 1:bc265521eb00 201
Davidroid 1:bc265521eb00 202 /******************************************************//**
Davidroid 1:bc265521eb00 203 * @brief Exit STSPIN820 device from standby (low power consumption)
Davidroid 1:bc265521eb00 204 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 205 * @retval None
Davidroid 1:bc265521eb00 206 **********************************************************/
Davidroid 1:bc265521eb00 207 void STSPIN820::STSPIN820_ExitDeviceFromStandby(uint8_t deviceId)
Davidroid 1:bc265521eb00 208 {
Davidroid 1:bc265521eb00 209 uint32_t sequencerPosition = device_prm.sequencerPosition;
Davidroid 1:bc265521eb00 210
Davidroid 1:bc265521eb00 211 /* Exit standby and set step mode */
Davidroid 1:bc265521eb00 212 /* Disable step clock */
Davidroid 1:bc265521eb00 213 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
Davidroid 1:bc265521eb00 214 {
Davidroid 1:bc265521eb00 215 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
Davidroid 1:bc265521eb00 216 }
Davidroid 1:bc265521eb00 217
Davidroid 1:bc265521eb00 218 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
Davidroid 1:bc265521eb00 219 /* after the last step clock rising edge triggering the last step */
Davidroid 1:bc265521eb00 220 STSPIN820_Board_Delay(DISABLE_DELAY);
Davidroid 1:bc265521eb00 221 /* Set reference voltage to 0 */
Davidroid 1:bc265521eb00 222 STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);
Davidroid 1:bc265521eb00 223
Davidroid 1:bc265521eb00 224 /* Disable power bridges */
Davidroid 1:bc265521eb00 225 STSPIN820_Board_Disable();
Davidroid 1:bc265521eb00 226
Davidroid 1:bc265521eb00 227 device_prm.commandExecuted = NO_CMD;
Davidroid 1:bc265521eb00 228 device_prm.stepsToTake = 0;
Davidroid 1:bc265521eb00 229 device_prm.speed = 0;
Davidroid 1:bc265521eb00 230
Davidroid 1:bc265521eb00 231 /* Reset the microstepping sequencer position */
Davidroid 1:bc265521eb00 232 device_prm.sequencerPosition = 0;
Davidroid 1:bc265521eb00 233
Davidroid 1:bc265521eb00 234 /* Reset current and mark positions */
Davidroid 1:bc265521eb00 235 device_prm.currentPosition = 0;
Davidroid 1:bc265521eb00 236 device_prm.markPosition = 0;
Davidroid 1:bc265521eb00 237
Davidroid 1:bc265521eb00 238 STSPIN820_SetStepMode(deviceId, device_prm.stepMode);
Davidroid 1:bc265521eb00 239 STSPIN820_Board_ReleaseReset();
Davidroid 1:bc265521eb00 240 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
Davidroid 1:bc265521eb00 241
Davidroid 1:bc265521eb00 242 STSPIN820_SetHome(deviceId);
Davidroid 1:bc265521eb00 243 STSPIN820_SetMark(deviceId);
Davidroid 1:bc265521eb00 244
Davidroid 1:bc265521eb00 245 if (device_prm.sequencerPosition != 0)
Davidroid 1:bc265521eb00 246 {
Davidroid 1:bc265521eb00 247 /* Set direction to FORWARD to ensure the HW sequencer is increased at */
Davidroid 1:bc265521eb00 248 /* each step clock rising edge */
Davidroid 1:bc265521eb00 249 STSPIN820_SetDirection(0, FORWARD);
Davidroid 1:bc265521eb00 250 /* Going out of standby */
Davidroid 1:bc265521eb00 251 device_prm.motionState = STANDBYTOINACTIVE;
Davidroid 1:bc265521eb00 252 /* Initialize the step clock timer */
Davidroid 1:bc265521eb00 253 STSPIN820_Board_TimStckInit();
Davidroid 1:bc265521eb00 254 /* Program the step clock */
Davidroid 1:bc265521eb00 255 STSPIN820_Board_TimStckCompareInit();
Davidroid 1:bc265521eb00 256 STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
Davidroid 1:bc265521eb00 257 toggle_odd = 0;
Davidroid 1:bc265521eb00 258 STSPIN820_Board_TimStckStart();
Davidroid 1:bc265521eb00 259 while (device_prm.sequencerPosition != 0);
Davidroid 1:bc265521eb00 260 while (toggle_odd!=0);
Davidroid 1:bc265521eb00 261 device_prm.sequencerPosition = sequencerPosition;
Davidroid 1:bc265521eb00 262 }
Davidroid 1:bc265521eb00 263
Davidroid 1:bc265521eb00 264 device_prm.motionState = INACTIVE;
Davidroid 1:bc265521eb00 265 }
Davidroid 1:bc265521eb00 266
Davidroid 1:bc265521eb00 267 /******************************************************//**
Davidroid 1:bc265521eb00 268 * @brief Return the acceleration of the specified device
Davidroid 1:bc265521eb00 269 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 270 * @retval Acceleration in pps^2
Davidroid 1:bc265521eb00 271 **********************************************************/
Davidroid 1:bc265521eb00 272 uint16_t STSPIN820::STSPIN820_GetAcceleration(int8_t deviceId)
Davidroid 1:bc265521eb00 273 {
Davidroid 1:bc265521eb00 274 return (device_prm.acceleration);
Davidroid 1:bc265521eb00 275 }
Davidroid 1:bc265521eb00 276
Davidroid 1:bc265521eb00 277 /******************************************************//**
Davidroid 1:bc265521eb00 278 * @brief Return the current speed of the specified device
Davidroid 1:bc265521eb00 279 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 280 * @retval Speed in pps
Davidroid 1:bc265521eb00 281 **********************************************************/
Davidroid 1:bc265521eb00 282 uint16_t STSPIN820::STSPIN820_GetCurrentSpeed(int8_t deviceId)
Davidroid 1:bc265521eb00 283 {
Davidroid 1:bc265521eb00 284 return device_prm.speed;
Davidroid 1:bc265521eb00 285 }
Davidroid 1:bc265521eb00 286
Davidroid 1:bc265521eb00 287 /******************************************************//**
Davidroid 1:bc265521eb00 288 * @brief Return the decay mode of the specified device
Davidroid 1:bc265521eb00 289 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 290 * @retval Decay Mode State (SLOW or MIXED)
Davidroid 1:bc265521eb00 291 **********************************************************/
Davidroid 1:bc265521eb00 292 motor_decay_mode_t STSPIN820::STSPIN820_GetDecayMode(uint8_t deviceId)
Davidroid 1:bc265521eb00 293 {
Davidroid 1:bc265521eb00 294 motor_decay_mode_t status;
Davidroid 1:bc265521eb00 295 if (STSPIN820_Board_GetDecayGpio() != 0)
Davidroid 1:bc265521eb00 296 {
Davidroid 1:bc265521eb00 297 status = SLOW_DECAY;
Davidroid 1:bc265521eb00 298 }
Davidroid 1:bc265521eb00 299 else
Davidroid 1:bc265521eb00 300 {
Davidroid 1:bc265521eb00 301 status = MIXED_DECAY;
Davidroid 1:bc265521eb00 302 }
Davidroid 1:bc265521eb00 303
Davidroid 1:bc265521eb00 304 return status;
Davidroid 1:bc265521eb00 305 }
Davidroid 1:bc265521eb00 306
Davidroid 1:bc265521eb00 307 /******************************************************//**
Davidroid 1:bc265521eb00 308 * @brief Return the deceleration of the specified device
Davidroid 1:bc265521eb00 309 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 310 * @retval Deceleration in pps^2
Davidroid 1:bc265521eb00 311 **********************************************************/
Davidroid 1:bc265521eb00 312 uint16_t STSPIN820::STSPIN820_GetDeceleration(uint8_t deviceId)
Davidroid 1:bc265521eb00 313 {
Davidroid 1:bc265521eb00 314 return (device_prm.deceleration);
Davidroid 1:bc265521eb00 315 }
Davidroid 1:bc265521eb00 316
Davidroid 1:bc265521eb00 317 /******************************************************//**
Davidroid 1:bc265521eb00 318 * @brief Return the device state
Davidroid 1:bc265521eb00 319 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 320 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
Davidroid 1:bc265521eb00 321 **********************************************************/
Davidroid 1:bc265521eb00 322 motor_state_t STSPIN820::STSPIN820_GetDeviceState(uint8_t deviceId)
Davidroid 1:bc265521eb00 323 {
Davidroid 1:bc265521eb00 324 return device_prm.motionState;
Davidroid 1:bc265521eb00 325 }
Davidroid 1:bc265521eb00 326
Davidroid 1:bc265521eb00 327 /******************************************************//**
Davidroid 1:bc265521eb00 328 * @brief Get the motor current direction
Davidroid 1:bc265521eb00 329 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 330 * @retval direction
Davidroid 1:bc265521eb00 331 **********************************************************/
Davidroid 1:bc265521eb00 332 motor_direction_t STSPIN820::STSPIN820_GetDirection(uint8_t deviceId)
Davidroid 1:bc265521eb00 333 {
Davidroid 1:bc265521eb00 334 return device_prm.direction;
Davidroid 1:bc265521eb00 335 }
Davidroid 1:bc265521eb00 336
Davidroid 1:bc265521eb00 337 /******************************************************//**
Davidroid 1:bc265521eb00 338 * @brief Return the FW version of the library
Davidroid 1:bc265521eb00 339 * @retval Stspin220_FW_VERSION
Davidroid 1:bc265521eb00 340 **********************************************************/
Davidroid 1:bc265521eb00 341 uint32_t STSPIN820::STSPIN820_GetFwVersion(void)
Davidroid 1:bc265521eb00 342 {
Davidroid 1:bc265521eb00 343 return (STSPIN820_FW_VERSION);
Davidroid 1:bc265521eb00 344 }
Davidroid 1:bc265521eb00 345
Davidroid 1:bc265521eb00 346 /******************************************************//**
Davidroid 1:bc265521eb00 347 * @brief Return the mark position of the specified device
Davidroid 1:bc265521eb00 348 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 349 * @retval mark position value
Davidroid 1:bc265521eb00 350 **********************************************************/
Davidroid 1:bc265521eb00 351 int32_t STSPIN820::STSPIN820_GetMark(uint8_t deviceId)
Davidroid 1:bc265521eb00 352 {
Davidroid 1:bc265521eb00 353 return device_prm.markPosition;
Davidroid 1:bc265521eb00 354 }
Davidroid 1:bc265521eb00 355
Davidroid 1:bc265521eb00 356 /******************************************************//**
Davidroid 1:bc265521eb00 357 * @brief Return the max speed of the specified device
Davidroid 1:bc265521eb00 358 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 359 * @retval maxSpeed in pps
Davidroid 1:bc265521eb00 360 **********************************************************/
Davidroid 1:bc265521eb00 361 uint16_t STSPIN820::STSPIN820_GetMaxSpeed(uint8_t deviceId)
Davidroid 1:bc265521eb00 362 {
Davidroid 1:bc265521eb00 363 return (device_prm.maxSpeed);
Davidroid 1:bc265521eb00 364 }
Davidroid 1:bc265521eb00 365
Davidroid 1:bc265521eb00 366 /******************************************************//**
Davidroid 1:bc265521eb00 367 * @brief Return the min speed of the specified device
Davidroid 1:bc265521eb00 368 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 369 * @retval minSpeed in pps
Davidroid 1:bc265521eb00 370 **********************************************************/
Davidroid 1:bc265521eb00 371 uint16_t STSPIN820::STSPIN820_GetMinSpeed(uint8_t deviceId)
Davidroid 1:bc265521eb00 372 {
Davidroid 1:bc265521eb00 373 return (device_prm.minSpeed);
Davidroid 1:bc265521eb00 374 }
Davidroid 1:bc265521eb00 375
Davidroid 1:bc265521eb00 376 /******************************************************//**
Davidroid 1:bc265521eb00 377 * @brief Returns the number of devices
Davidroid 1:bc265521eb00 378 * @retval number of devices
Davidroid 1:bc265521eb00 379 **********************************************************/
Davidroid 1:bc265521eb00 380 uint8_t STSPIN820::STSPIN820_GetNbDevices(void)
Davidroid 1:bc265521eb00 381 {
Davidroid 1:bc265521eb00 382 return (number_of_devices);
Davidroid 1:bc265521eb00 383 }
Davidroid 1:bc265521eb00 384
Davidroid 1:bc265521eb00 385 /******************************************************//**
Davidroid 1:bc265521eb00 386 * @brief Return the current position value of the specified device
Davidroid 1:bc265521eb00 387 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 388 * @retval current position value
Davidroid 1:bc265521eb00 389 **********************************************************/
Davidroid 1:bc265521eb00 390 int32_t STSPIN820::STSPIN820_GetPosition(uint8_t deviceId)
Davidroid 1:bc265521eb00 391 {
Davidroid 1:bc265521eb00 392 return device_prm.currentPosition;
Davidroid 1:bc265521eb00 393 }
Davidroid 1:bc265521eb00 394
Davidroid 1:bc265521eb00 395 /******************************************************//**
Davidroid 1:bc265521eb00 396 * @brief Get the motor step mode
Davidroid 1:bc265521eb00 397 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 398 * @retval step mode
Davidroid 1:bc265521eb00 399 **********************************************************/
Davidroid 1:bc265521eb00 400 motor_step_mode_t STSPIN820::STSPIN820_GetStepMode(uint8_t deviceId)
Davidroid 1:bc265521eb00 401 {
Davidroid 1:bc265521eb00 402 return device_prm.stepMode;
Davidroid 1:bc265521eb00 403 }
Davidroid 1:bc265521eb00 404
Davidroid 1:bc265521eb00 405 /******************************************************//**
Davidroid 1:bc265521eb00 406 * @brief Get the selected stop mode
Davidroid 1:bc265521eb00 407 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 408 * @retval the selected stop mode
Davidroid 1:bc265521eb00 409 **********************************************************/
Davidroid 1:bc265521eb00 410 motor_stop_mode_t STSPIN820::STSPIN820_GetStopMode(uint8_t deviceId)
Davidroid 1:bc265521eb00 411 {
Davidroid 1:bc265521eb00 412 return device_prm.stopMode;
Davidroid 1:bc265521eb00 413 }
Davidroid 1:bc265521eb00 414
Davidroid 1:bc265521eb00 415 /******************************************************//**
Davidroid 1:bc265521eb00 416 * @brief Get the torque of the specified device
Davidroid 1:bc265521eb00 417 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 418 * @param[in] torqueMode torque mode
Davidroid 1:bc265521eb00 419 * @retval the torqueValue in % (from 0 to 100)
Davidroid 1:bc265521eb00 420 * @note
Davidroid 1:bc265521eb00 421 **********************************************************/
Davidroid 1:bc265521eb00 422 uint8_t STSPIN820::STSPIN820_GetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
Davidroid 1:bc265521eb00 423 {
Davidroid 1:bc265521eb00 424 uint8_t torqueValue = 0;
Davidroid 1:bc265521eb00 425 switch(torqueMode)
Davidroid 1:bc265521eb00 426 {
Davidroid 1:bc265521eb00 427 case ACC_TORQUE:
Davidroid 1:bc265521eb00 428 torqueValue = device_prm.accelTorque;
Davidroid 1:bc265521eb00 429 break;
Davidroid 1:bc265521eb00 430 case DEC_TORQUE:
Davidroid 1:bc265521eb00 431 torqueValue = device_prm.decelTorque;
Davidroid 1:bc265521eb00 432 break;
Davidroid 1:bc265521eb00 433 case RUN_TORQUE:
Davidroid 1:bc265521eb00 434 torqueValue = device_prm.runTorque;
Davidroid 1:bc265521eb00 435 break;
Davidroid 1:bc265521eb00 436 case HOLD_TORQUE:
Davidroid 1:bc265521eb00 437 torqueValue = device_prm.holdTorque;
Davidroid 1:bc265521eb00 438 break;
Davidroid 1:bc265521eb00 439 case CURRENT_TORQUE:
Davidroid 1:bc265521eb00 440 torqueValue = device_prm.currentTorque;
Davidroid 1:bc265521eb00 441 break;
Davidroid 1:bc265521eb00 442 default:
Davidroid 1:bc265521eb00 443 break;
Davidroid 1:bc265521eb00 444 }
Davidroid 1:bc265521eb00 445 return torqueValue;
Davidroid 1:bc265521eb00 446 }
Davidroid 1:bc265521eb00 447
Davidroid 1:bc265521eb00 448 /******************************************************//**
Davidroid 1:bc265521eb00 449 * @brief Get the torque boost feature status
Davidroid 1:bc265521eb00 450 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 451 * @retval TRUE if enabled, FALSE if disabled
Davidroid 1:bc265521eb00 452 **********************************************************/
Davidroid 1:bc265521eb00 453 bool STSPIN820::STSPIN820_GetTorqueBoostEnable(uint8_t deviceId)
Davidroid 1:bc265521eb00 454 {
Davidroid 1:bc265521eb00 455 return device_prm.torqueBoostEnable;
Davidroid 1:bc265521eb00 456 }
Davidroid 1:bc265521eb00 457
Davidroid 1:bc265521eb00 458 /******************************************************//**
Davidroid 1:bc265521eb00 459 * @brief Get the torque boost threshold
Davidroid 1:bc265521eb00 460 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
Davidroid 1:bc265521eb00 461 * @retval the torque boost threshold above which the step mode is
Davidroid 1:bc265521eb00 462 * changed to full step
Davidroid 1:bc265521eb00 463 **********************************************************/
Davidroid 1:bc265521eb00 464 uint16_t STSPIN820::STSPIN820_GetTorqueBoostThreshold(uint8_t deviceId)
Davidroid 1:bc265521eb00 465 {
Davidroid 1:bc265521eb00 466 return device_prm.torqueBoostSpeedThreshold;
Davidroid 1:bc265521eb00 467 }
Davidroid 1:bc265521eb00 468
Davidroid 1:bc265521eb00 469 /******************************************************//**
Davidroid 1:bc265521eb00 470 * @brief Request the motor to move to the home position (ABS_POSITION = 0)
Davidroid 1:bc265521eb00 471 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 472 * @retval None
Davidroid 1:bc265521eb00 473 **********************************************************/
Davidroid 1:bc265521eb00 474 void STSPIN820::STSPIN820_GoHome(uint8_t deviceId)
Davidroid 1:bc265521eb00 475 {
Davidroid 1:bc265521eb00 476 deviceId = 0;
Davidroid 1:bc265521eb00 477
Davidroid 1:bc265521eb00 478 STSPIN820_GoTo(deviceId, 0);
Davidroid 1:bc265521eb00 479 }
Davidroid 1:bc265521eb00 480
Davidroid 1:bc265521eb00 481 /******************************************************//**
Davidroid 1:bc265521eb00 482 * @brief Request the motor to move to the mark position
Davidroid 1:bc265521eb00 483 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 484 * @retval None
Davidroid 1:bc265521eb00 485 **********************************************************/
Davidroid 1:bc265521eb00 486 void STSPIN820::STSPIN820_GoMark(uint8_t deviceId)
Davidroid 1:bc265521eb00 487 {
Davidroid 1:bc265521eb00 488 deviceId = 0;
Davidroid 1:bc265521eb00 489
Davidroid 1:bc265521eb00 490 STSPIN820_GoTo(deviceId, device_prm.markPosition);
Davidroid 1:bc265521eb00 491 }
Davidroid 1:bc265521eb00 492
Davidroid 1:bc265521eb00 493 /******************************************************//**
Davidroid 1:bc265521eb00 494 * @brief Request the motor to move to the specified position
Davidroid 1:bc265521eb00 495 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 496 * @param[in] targetPosition absolute position in steps
Davidroid 1:bc265521eb00 497 * @retval None
Davidroid 1:bc265521eb00 498 * @note The position is at the resolution corresponding to the
Davidroid 1:bc265521eb00 499 * selected step mode.
Davidroid 1:bc265521eb00 500 * STEP_MODE_FULL : Full step
Davidroid 1:bc265521eb00 501 * STEP_MODE_HALF : 1/2 step
Davidroid 1:bc265521eb00 502 * STEP_MODE_1_4 : 1/4 step
Davidroid 1:bc265521eb00 503 * STEP_MODE_1_8 : 1/8 step
Davidroid 1:bc265521eb00 504 * STEP_MODE_1_16 : 1/16 step
Davidroid 1:bc265521eb00 505 * STEP_MODE_1_32 : 1/32 step
Davidroid 1:bc265521eb00 506 * STEP_MODE_1_128 : 1/128 step
Davidroid 1:bc265521eb00 507 * STEP_MODE_1_256 : 1/256 step
Davidroid 1:bc265521eb00 508 * @note The 1/64 step mode is not allowed
Davidroid 1:bc265521eb00 509 **********************************************************/
Davidroid 1:bc265521eb00 510 void STSPIN820::STSPIN820_GoTo(uint8_t deviceId, int32_t targetPosition)
Davidroid 1:bc265521eb00 511 {
Davidroid 1:bc265521eb00 512 motor_direction_t direction;
Davidroid 1:bc265521eb00 513 deviceId = 0;
Davidroid 1:bc265521eb00 514
Davidroid 1:bc265521eb00 515 /* Exit from standby if needed */
Davidroid 1:bc265521eb00 516 if (device_prm.motionState == STANDBY)
Davidroid 1:bc265521eb00 517 {
Davidroid 1:bc265521eb00 518 STSPIN820_ExitDeviceFromStandby(deviceId);
Davidroid 1:bc265521eb00 519 }
Davidroid 1:bc265521eb00 520 /* Deactivate motor if needed */
Davidroid 1:bc265521eb00 521 else if (device_prm.motionState != INACTIVE)
Davidroid 1:bc265521eb00 522 {
Davidroid 1:bc265521eb00 523 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 524 }
Davidroid 1:bc265521eb00 525
Davidroid 1:bc265521eb00 526 if (targetPosition > device_prm.currentPosition)
Davidroid 1:bc265521eb00 527 {
Davidroid 1:bc265521eb00 528 device_prm.stepsToTake = targetPosition -\
Davidroid 1:bc265521eb00 529 device_prm.currentPosition;
Davidroid 1:bc265521eb00 530 if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
Davidroid 1:bc265521eb00 531 {
Davidroid 1:bc265521eb00 532 direction = FORWARD;
Davidroid 1:bc265521eb00 533 }
Davidroid 1:bc265521eb00 534 else
Davidroid 1:bc265521eb00 535 {
Davidroid 1:bc265521eb00 536 direction = BACKWARD;
Davidroid 1:bc265521eb00 537 device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
Davidroid 1:bc265521eb00 538 device_prm.stepsToTake;
Davidroid 1:bc265521eb00 539 }
Davidroid 1:bc265521eb00 540 }
Davidroid 1:bc265521eb00 541 else
Davidroid 1:bc265521eb00 542 {
Davidroid 1:bc265521eb00 543 device_prm.stepsToTake = device_prm.currentPosition -\
Davidroid 1:bc265521eb00 544 targetPosition;
Davidroid 1:bc265521eb00 545 if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
Davidroid 1:bc265521eb00 546 {
Davidroid 1:bc265521eb00 547 direction = BACKWARD;
Davidroid 1:bc265521eb00 548 }
Davidroid 1:bc265521eb00 549 else
Davidroid 1:bc265521eb00 550 {
Davidroid 1:bc265521eb00 551 direction = FORWARD;
Davidroid 1:bc265521eb00 552 device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
Davidroid 1:bc265521eb00 553 device_prm.stepsToTake;
Davidroid 1:bc265521eb00 554 }
Davidroid 1:bc265521eb00 555 }
Davidroid 1:bc265521eb00 556
Davidroid 1:bc265521eb00 557 if (device_prm.stepsToTake != 0)
Davidroid 1:bc265521eb00 558 {
Davidroid 1:bc265521eb00 559 device_prm.commandExecuted = MOVE_CMD;
Davidroid 1:bc265521eb00 560
Davidroid 1:bc265521eb00 561 /* Direction setup */
Davidroid 1:bc265521eb00 562 STSPIN820_SetDirection(deviceId, direction);
Davidroid 1:bc265521eb00 563
Davidroid 1:bc265521eb00 564 STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
Davidroid 1:bc265521eb00 565
Davidroid 1:bc265521eb00 566 /* Motor activation */
Davidroid 1:bc265521eb00 567 STSPIN820_StartMovement(deviceId);
Davidroid 1:bc265521eb00 568 }
Davidroid 1:bc265521eb00 569 }
Davidroid 1:bc265521eb00 570
Davidroid 1:bc265521eb00 571 /******************************************************//**
Davidroid 1:bc265521eb00 572 * @brief Move the motor to the absolute position
Davidroid 1:bc265521eb00 573 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 574 * @param[in] direction FORWARD or BACKWARD
Davidroid 1:bc265521eb00 575 * @param[in] targetPosition 32 bit signed value position
Davidroid 1:bc265521eb00 576 * @retval None
Davidroid 1:bc265521eb00 577 * @note The position is at the resolution corresponding to the
Davidroid 1:bc265521eb00 578 * selected step mode.
Davidroid 1:bc265521eb00 579 * STEP_MODE_FULL : step
Davidroid 1:bc265521eb00 580 * STEP_MODE_HALF : 1/2 step
Davidroid 1:bc265521eb00 581 * STEP_MODE_1_4 : 1/4 step
Davidroid 1:bc265521eb00 582 * STEP_MODE_1_8 : 1/8 step
Davidroid 1:bc265521eb00 583 * STEP_MODE_1_16 : 1/16 step
Davidroid 1:bc265521eb00 584 * STEP_MODE_1_32 : 1/32 step
Davidroid 1:bc265521eb00 585 * STEP_MODE_1_128 : 1/128 step
Davidroid 1:bc265521eb00 586 * STEP_MODE_1_256 : 1/256 step
Davidroid 1:bc265521eb00 587 * @note The 1/64 step mode is not allowed
Davidroid 1:bc265521eb00 588 **********************************************************/
Davidroid 1:bc265521eb00 589 void STSPIN820::STSPIN820_GoToDir(uint8_t deviceId, motor_direction_t direction, int32_t targetPosition)
Davidroid 1:bc265521eb00 590 {
Davidroid 1:bc265521eb00 591 deviceId = 0;
Davidroid 1:bc265521eb00 592
Davidroid 1:bc265521eb00 593 /* Exit from standby if needed */
Davidroid 1:bc265521eb00 594 if (device_prm.motionState == STANDBY)
Davidroid 1:bc265521eb00 595 {
Davidroid 1:bc265521eb00 596 STSPIN820_ExitDeviceFromStandby(deviceId);
Davidroid 1:bc265521eb00 597 }
Davidroid 1:bc265521eb00 598 /* Deactivate motor if needed */
Davidroid 1:bc265521eb00 599 else if (device_prm.motionState != INACTIVE)
Davidroid 1:bc265521eb00 600 {
Davidroid 1:bc265521eb00 601 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 602 }
Davidroid 1:bc265521eb00 603
Davidroid 1:bc265521eb00 604 if (direction != BACKWARD)
Davidroid 1:bc265521eb00 605 {
Davidroid 1:bc265521eb00 606 if (targetPosition > device_prm.currentPosition)
Davidroid 1:bc265521eb00 607 {
Davidroid 1:bc265521eb00 608 device_prm.stepsToTake = targetPosition -\
Davidroid 1:bc265521eb00 609 device_prm.currentPosition;
Davidroid 1:bc265521eb00 610 }
Davidroid 1:bc265521eb00 611 else
Davidroid 1:bc265521eb00 612 {
Davidroid 1:bc265521eb00 613 device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
Davidroid 1:bc265521eb00 614 (targetPosition -\
Davidroid 1:bc265521eb00 615 device_prm.currentPosition);
Davidroid 1:bc265521eb00 616 }
Davidroid 1:bc265521eb00 617 }
Davidroid 1:bc265521eb00 618 else
Davidroid 1:bc265521eb00 619 {
Davidroid 1:bc265521eb00 620 if (targetPosition > device_prm.currentPosition)
Davidroid 1:bc265521eb00 621 {
Davidroid 1:bc265521eb00 622 device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
Davidroid 1:bc265521eb00 623 (device_prm.currentPosition -\
Davidroid 1:bc265521eb00 624 targetPosition);
Davidroid 1:bc265521eb00 625 }
Davidroid 1:bc265521eb00 626 else
Davidroid 1:bc265521eb00 627 {
Davidroid 1:bc265521eb00 628 device_prm.stepsToTake = device_prm.currentPosition -\
Davidroid 1:bc265521eb00 629 targetPosition;
Davidroid 1:bc265521eb00 630 }
Davidroid 1:bc265521eb00 631 }
Davidroid 1:bc265521eb00 632
Davidroid 1:bc265521eb00 633 if (device_prm.stepsToTake != 0)
Davidroid 1:bc265521eb00 634 {
Davidroid 1:bc265521eb00 635 device_prm.commandExecuted = MOVE_CMD;
Davidroid 1:bc265521eb00 636
Davidroid 1:bc265521eb00 637 /* Direction setup */
Davidroid 1:bc265521eb00 638 STSPIN820_SetDirection(deviceId, direction);
Davidroid 1:bc265521eb00 639
Davidroid 1:bc265521eb00 640 STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake);
Davidroid 1:bc265521eb00 641
Davidroid 1:bc265521eb00 642 /* Motor activation */
Davidroid 1:bc265521eb00 643 STSPIN820_StartMovement(deviceId);
Davidroid 1:bc265521eb00 644 }
Davidroid 1:bc265521eb00 645 }
Davidroid 1:bc265521eb00 646
Davidroid 1:bc265521eb00 647 /******************************************************//**
Davidroid 1:bc265521eb00 648 * @brief Immediatly stop the motor and disable the power bridge
Davidroid 1:bc265521eb00 649 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 650 * @retval None
Davidroid 1:bc265521eb00 651 **********************************************************/
Davidroid 1:bc265521eb00 652 void STSPIN820::STSPIN820_HardHiZ(uint8_t deviceId)
Davidroid 1:bc265521eb00 653 {
Davidroid 1:bc265521eb00 654 /* Set inactive state */
Davidroid 1:bc265521eb00 655 device_prm.motionState = INACTIVE;
Davidroid 1:bc265521eb00 656
Davidroid 1:bc265521eb00 657 /* Disable step clock */
Davidroid 1:bc265521eb00 658 if (STSPIN820_Board_TimStckStop(handle &toggle_odd) == 0)
Davidroid 1:bc265521eb00 659 {
Davidroid 1:bc265521eb00 660 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
Davidroid 1:bc265521eb00 661 }
Davidroid 1:bc265521eb00 662
Davidroid 1:bc265521eb00 663 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
Davidroid 1:bc265521eb00 664 /* after the last step clock rising edge triggering the last step */
Davidroid 1:bc265521eb00 665 STSPIN820_Board_Delay(DISABLE_DELAY);
Davidroid 1:bc265521eb00 666
Davidroid 1:bc265521eb00 667 /* Set reference voltage to 0 */
Davidroid 1:bc265521eb00 668 STSPIN820_SetTorque(0, CURRENT_TORQUE, 0);
Davidroid 1:bc265521eb00 669
Davidroid 1:bc265521eb00 670 /* Disable power bridges */
Davidroid 1:bc265521eb00 671 STSPIN820_Board_Disable();
Davidroid 1:bc265521eb00 672
Davidroid 1:bc265521eb00 673 /* Comeback to nominal step mode */
Davidroid 1:bc265521eb00 674 if (device_prm.stepModeLatched != device_prm.stepMode)
Davidroid 1:bc265521eb00 675 {
Davidroid 1:bc265521eb00 676 motor_step_mode_t StepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 677 STSPIN820_SetStepMode(0, StepMode);
Davidroid 1:bc265521eb00 678 device_prm.stepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 679 }
Davidroid 1:bc265521eb00 680
Davidroid 1:bc265521eb00 681 device_prm.commandExecuted = NO_CMD;
Davidroid 1:bc265521eb00 682 device_prm.stepsToTake = 0;
Davidroid 1:bc265521eb00 683 device_prm.speed = 0;
Davidroid 1:bc265521eb00 684 }
Davidroid 1:bc265521eb00 685
Davidroid 1:bc265521eb00 686 /******************************************************//**
Davidroid 1:bc265521eb00 687 * @brief Immediatly stop the motor
Davidroid 1:bc265521eb00 688 * and either set holding torque when stop mode is HOLD_MODE,
Davidroid 1:bc265521eb00 689 * or call STSPIN820_HardHiz function when stop mode is HIZ_MODE,
Davidroid 1:bc265521eb00 690 * or call STSPIN820_PutDeviceInStandby function when stop mode is STANDBY_MODE
Davidroid 1:bc265521eb00 691 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 692 * @retval None
Davidroid 1:bc265521eb00 693 **********************************************************/
Davidroid 1:bc265521eb00 694 void STSPIN820::STSPIN820_HardStop(uint8_t deviceId)
Davidroid 1:bc265521eb00 695 {
Davidroid 1:bc265521eb00 696 deviceId = 0;
Davidroid 1:bc265521eb00 697
Davidroid 1:bc265521eb00 698 if (device_prm.stopMode == HOLD_MODE)
Davidroid 1:bc265521eb00 699 {
Davidroid 1:bc265521eb00 700 /* Set inactive state */
Davidroid 1:bc265521eb00 701 device_prm.motionState = INACTIVE;
Davidroid 1:bc265521eb00 702
Davidroid 1:bc265521eb00 703 /* Disable step clock */
Davidroid 1:bc265521eb00 704 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
Davidroid 1:bc265521eb00 705 {
Davidroid 1:bc265521eb00 706 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
Davidroid 1:bc265521eb00 707 }
Davidroid 1:bc265521eb00 708
Davidroid 1:bc265521eb00 709 /* Set holding torque */
Davidroid 1:bc265521eb00 710 STSPIN820_ApplyTorque(deviceId, HOLD_TORQUE);
Davidroid 1:bc265521eb00 711
Davidroid 1:bc265521eb00 712 /* Comeback to nominal step mode */
Davidroid 1:bc265521eb00 713 if (device_prm.stepModeLatched != device_prm.stepMode)
Davidroid 1:bc265521eb00 714 {
Davidroid 1:bc265521eb00 715 motor_step_mode_t StepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 716 STSPIN820_SetStepMode(0, StepMode);
Davidroid 1:bc265521eb00 717 device_prm.stepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 718 }
Davidroid 1:bc265521eb00 719
Davidroid 1:bc265521eb00 720 device_prm.commandExecuted = NO_CMD;
Davidroid 1:bc265521eb00 721 device_prm.stepsToTake = 0;
Davidroid 1:bc265521eb00 722 device_prm.speed = 0;
Davidroid 1:bc265521eb00 723 }
Davidroid 1:bc265521eb00 724 else if (device_prm.stopMode == HIZ_MODE)
Davidroid 1:bc265521eb00 725 {
Davidroid 1:bc265521eb00 726 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 727 }
Davidroid 1:bc265521eb00 728 else if (device_prm.stopMode == STANDBY_MODE)
Davidroid 1:bc265521eb00 729 {
Davidroid 1:bc265521eb00 730 STSPIN820_PutDeviceInStandby(deviceId);
Davidroid 1:bc265521eb00 731 }
Davidroid 1:bc265521eb00 732 }
Davidroid 1:bc265521eb00 733
Davidroid 1:bc265521eb00 734 /******************************************************//**
Davidroid 1:bc265521eb00 735 * @brief Moves the motor of the specified number of steps
Davidroid 1:bc265521eb00 736 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 737 * @param[in] direction FORWARD or BACKWARD
Davidroid 1:bc265521eb00 738 * @param[in] stepCount Number of steps to perform
Davidroid 1:bc265521eb00 739 * @retval None
Davidroid 1:bc265521eb00 740 **********************************************************/
Davidroid 1:bc265521eb00 741 void STSPIN820::STSPIN820_Move(uint8_t deviceId, motor_direction_t direction, uint32_t stepCount)
Davidroid 1:bc265521eb00 742 {
Davidroid 1:bc265521eb00 743 deviceId = 0;
Davidroid 1:bc265521eb00 744
Davidroid 1:bc265521eb00 745 /* Exit from standby if needed */
Davidroid 1:bc265521eb00 746 if (device_prm.motionState == STANDBY)
Davidroid 1:bc265521eb00 747 {
Davidroid 1:bc265521eb00 748 STSPIN820_ExitDeviceFromStandby(deviceId);
Davidroid 1:bc265521eb00 749 }
Davidroid 1:bc265521eb00 750 /* Deactivate motor if needed */
Davidroid 1:bc265521eb00 751 else if (device_prm.motionState != INACTIVE)
Davidroid 1:bc265521eb00 752 {
Davidroid 1:bc265521eb00 753 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 754 }
Davidroid 1:bc265521eb00 755
Davidroid 1:bc265521eb00 756 if (stepCount != 0)
Davidroid 1:bc265521eb00 757 {
Davidroid 1:bc265521eb00 758 device_prm.stepsToTake = stepCount;
Davidroid 1:bc265521eb00 759 device_prm.commandExecuted = MOVE_CMD;
Davidroid 1:bc265521eb00 760
Davidroid 1:bc265521eb00 761 /* Direction setup */
Davidroid 1:bc265521eb00 762 STSPIN820_SetDirection(deviceId, direction);
Davidroid 1:bc265521eb00 763
Davidroid 1:bc265521eb00 764 STSPIN820_ComputeSpeedProfile(deviceId, stepCount);
Davidroid 1:bc265521eb00 765
Davidroid 1:bc265521eb00 766 /* Motor activation */
Davidroid 1:bc265521eb00 767 STSPIN820_StartMovement(deviceId);
Davidroid 1:bc265521eb00 768 }
Davidroid 1:bc265521eb00 769 }
Davidroid 1:bc265521eb00 770
Davidroid 1:bc265521eb00 771 /******************************************************//**
Davidroid 1:bc265521eb00 772 * @brief Put STSPIN820 device in standby (low power consumption)
Davidroid 1:bc265521eb00 773 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 774 * @retval None
Davidroid 1:bc265521eb00 775 **********************************************************/
Davidroid 1:bc265521eb00 776 void STSPIN820::STSPIN820_PutDeviceInStandby(uint8_t deviceId)
Davidroid 1:bc265521eb00 777 {
Davidroid 1:bc265521eb00 778 /* Stop movement */
Davidroid 1:bc265521eb00 779 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 780
Davidroid 1:bc265521eb00 781 /* Enter standby */
Davidroid 1:bc265521eb00 782 STSPIN820_Board_Reset();
Davidroid 1:bc265521eb00 783
Davidroid 1:bc265521eb00 784 device_prm.motionState = STANDBY;
Davidroid 1:bc265521eb00 785 }
Davidroid 1:bc265521eb00 786
Davidroid 1:bc265521eb00 787 /******************************************************//**
Davidroid 1:bc265521eb00 788 * @brief Runs the motor. It will accelerate from the min
Davidroid 1:bc265521eb00 789 * speed up to the max speed by using the device acceleration.
Davidroid 1:bc265521eb00 790 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 791 * @param[in] direction FORWARD or BACKWARD
Davidroid 1:bc265521eb00 792 * @retval None
Davidroid 1:bc265521eb00 793 **********************************************************/
Davidroid 1:bc265521eb00 794 void STSPIN820::STSPIN820_Run(uint8_t deviceId, motor_direction_t direction)
Davidroid 1:bc265521eb00 795 {
Davidroid 1:bc265521eb00 796 /* Exit from standby if needed */
Davidroid 1:bc265521eb00 797 if (device_prm.motionState == STANDBY)
Davidroid 1:bc265521eb00 798 {
Davidroid 1:bc265521eb00 799 STSPIN820_ExitDeviceFromStandby(deviceId);
Davidroid 1:bc265521eb00 800 }
Davidroid 1:bc265521eb00 801 /* Deactivate motor if needed */
Davidroid 1:bc265521eb00 802 else if (device_prm.motionState != INACTIVE)
Davidroid 1:bc265521eb00 803 {
Davidroid 1:bc265521eb00 804 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 805 }
Davidroid 1:bc265521eb00 806
Davidroid 1:bc265521eb00 807 /* Direction setup */
Davidroid 1:bc265521eb00 808 STSPIN820_SetDirection(deviceId,direction);
Davidroid 1:bc265521eb00 809 device_prm.commandExecuted = RUN_CMD;
Davidroid 1:bc265521eb00 810 /* Motor activation */
Davidroid 1:bc265521eb00 811 STSPIN820_StartMovement(deviceId);
Davidroid 1:bc265521eb00 812 }
Davidroid 1:bc265521eb00 813
Davidroid 1:bc265521eb00 814 /******************************************************//**
Davidroid 1:bc265521eb00 815 * @brief Changes the acceleration of the specified device
Davidroid 1:bc265521eb00 816 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 817 * @param[in] newAcc New acceleration to apply in pps^2
Davidroid 1:bc265521eb00 818 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 819 * @note The command is not performed if the device is executing
Davidroid 1:bc265521eb00 820 * a MOVE or GOTO command (but it can be used during a RUN command)
Davidroid 1:bc265521eb00 821 **********************************************************/
Davidroid 1:bc265521eb00 822 bool STSPIN820::STSPIN820_SetAcceleration(uint8_t deviceId, uint16_t newAcc)
Davidroid 1:bc265521eb00 823 {
Davidroid 1:bc265521eb00 824 bool cmdExecuted = FALSE;
Davidroid 1:bc265521eb00 825 if ((newAcc != 0)&&
Davidroid 1:bc265521eb00 826 (((device_prm.motionState & INACTIVE) == INACTIVE)||
Davidroid 1:bc265521eb00 827 (device_prm.commandExecuted == RUN_CMD)))
Davidroid 1:bc265521eb00 828 {
Davidroid 1:bc265521eb00 829 device_prm.acceleration = newAcc;
Davidroid 1:bc265521eb00 830 cmdExecuted = TRUE;
Davidroid 1:bc265521eb00 831 }
Davidroid 1:bc265521eb00 832 return cmdExecuted;
Davidroid 1:bc265521eb00 833 }
Davidroid 1:bc265521eb00 834
Davidroid 1:bc265521eb00 835 /******************************************************//**
Davidroid 1:bc265521eb00 836 * @brief Specifies the decay mode
Davidroid 1:bc265521eb00 837 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 838 * @param[in] decay SLOW_DECAY or MIXED_DECAY
Davidroid 1:bc265521eb00 839 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 840 **********************************************************/
Davidroid 1:bc265521eb00 841 void STSPIN820::STSPIN820_SetDecayMode(uint8_t deviceId, motor_decay_mode_t decay)
Davidroid 1:bc265521eb00 842 {
Davidroid 1:bc265521eb00 843 if (decay == SLOW_DECAY)
Davidroid 1:bc265521eb00 844 {
Davidroid 1:bc265521eb00 845 STSPIN820_Board_SetDecayGpio(1);
Davidroid 1:bc265521eb00 846 }
Davidroid 1:bc265521eb00 847 else if (decay == MIXED_DECAY)
Davidroid 1:bc265521eb00 848 {
Davidroid 1:bc265521eb00 849 STSPIN820_Board_SetDecayGpio(0);
Davidroid 1:bc265521eb00 850 }
Davidroid 1:bc265521eb00 851 }
Davidroid 1:bc265521eb00 852
Davidroid 1:bc265521eb00 853 /******************************************************//**
Davidroid 1:bc265521eb00 854 * @brief Changes the deceleration of the specified device
Davidroid 1:bc265521eb00 855 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 856 * @param[in] newDec New deceleration to apply in pps^2
Davidroid 1:bc265521eb00 857 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 858 * @note The command is not performed if the device is executing
Davidroid 1:bc265521eb00 859 * a MOVE or GOTO command (but it can be used during a RUN command)
Davidroid 1:bc265521eb00 860 **********************************************************/
Davidroid 1:bc265521eb00 861 bool STSPIN820::STSPIN820_SetDeceleration(uint8_t deviceId, uint16_t newDec)
Davidroid 1:bc265521eb00 862 {
Davidroid 1:bc265521eb00 863 bool cmdExecuted = FALSE;
Davidroid 1:bc265521eb00 864 if ((newDec != 0)&&
Davidroid 1:bc265521eb00 865 (((device_prm.motionState & INACTIVE) == INACTIVE)||
Davidroid 1:bc265521eb00 866 (device_prm.commandExecuted == RUN_CMD)))
Davidroid 1:bc265521eb00 867 {
Davidroid 1:bc265521eb00 868 device_prm.deceleration = newDec;
Davidroid 1:bc265521eb00 869 cmdExecuted = TRUE;
Davidroid 1:bc265521eb00 870 }
Davidroid 1:bc265521eb00 871 return cmdExecuted;
Davidroid 1:bc265521eb00 872 }
Davidroid 1:bc265521eb00 873
Davidroid 1:bc265521eb00 874 /******************************************************//**
Davidroid 1:bc265521eb00 875 * @brief Specifies the direction
Davidroid 1:bc265521eb00 876 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 877 * @param[in] dir FORWARD or BACKWARD
Davidroid 1:bc265521eb00 878 * @note The direction change is applied if the device
Davidroid 1:bc265521eb00 879 * is in INACTIVE or STANDBY state or if the device is
Davidroid 1:bc265521eb00 880 * executing a run command
Davidroid 1:bc265521eb00 881 * @retval None
Davidroid 1:bc265521eb00 882 **********************************************************/
Davidroid 1:bc265521eb00 883 void STSPIN820::STSPIN820_SetDirection(uint8_t deviceId, motor_direction_t dir)
Davidroid 1:bc265521eb00 884 {
Davidroid 1:bc265521eb00 885 if ((device_prm.motionState == INACTIVE)||\
Davidroid 1:bc265521eb00 886 (device_prm.motionState == STANDBY))
Davidroid 1:bc265521eb00 887 {
Davidroid 1:bc265521eb00 888 device_prm.direction = dir;
Davidroid 1:bc265521eb00 889 STSPIN820_Board_SetDirectionGpio(dir);
Davidroid 1:bc265521eb00 890 }
Davidroid 1:bc265521eb00 891 else if ((device_prm.commandExecuted&RUN_CMD)!=0)
Davidroid 1:bc265521eb00 892 {
Davidroid 1:bc265521eb00 893 device_prm.commandExecuted|=STSPIN820_DIR_CHANGE_BIT_MASK;
Davidroid 1:bc265521eb00 894 }
Davidroid 1:bc265521eb00 895 }
Davidroid 1:bc265521eb00 896
Davidroid 1:bc265521eb00 897 /******************************************************//**
Davidroid 1:bc265521eb00 898 * @brief Set current position to be the Home position
Davidroid 1:bc265521eb00 899 * (current position set to 0)
Davidroid 1:bc265521eb00 900 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 901 * @retval None
Davidroid 1:bc265521eb00 902 **********************************************************/
Davidroid 1:bc265521eb00 903 void STSPIN820::STSPIN820_SetHome(uint8_t deviceId)
Davidroid 1:bc265521eb00 904 {
Davidroid 1:bc265521eb00 905 device_prm.currentPosition = 0;
Davidroid 1:bc265521eb00 906 }
Davidroid 1:bc265521eb00 907
Davidroid 1:bc265521eb00 908 /******************************************************//**
Davidroid 1:bc265521eb00 909 * @brief Set current position to be the Mark position
Davidroid 1:bc265521eb00 910 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 911 * @retval None
Davidroid 1:bc265521eb00 912 **********************************************************/
Davidroid 1:bc265521eb00 913 void STSPIN820::STSPIN820_SetMark(uint8_t deviceId)
Davidroid 1:bc265521eb00 914 {
Davidroid 1:bc265521eb00 915 device_prm.markPosition = device_prm.currentPosition;
Davidroid 1:bc265521eb00 916 }
Davidroid 1:bc265521eb00 917
Davidroid 1:bc265521eb00 918 /******************************************************//**
Davidroid 1:bc265521eb00 919 * @brief Changes the max speed of the specified device
Davidroid 1:bc265521eb00 920 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 921 * @param[in] newMaxSpeed New max speed to apply in pps
Davidroid 1:bc265521eb00 922 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 923 * @note The command is not performed is the device is executing
Davidroid 1:bc265521eb00 924 * a MOVE or GOTO command (but it can be used during a RUN command).
Davidroid 1:bc265521eb00 925 **********************************************************/
Davidroid 1:bc265521eb00 926 bool STSPIN820::STSPIN820_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed)
Davidroid 1:bc265521eb00 927 {
Davidroid 1:bc265521eb00 928 bool cmdExecuted = FALSE;
Davidroid 1:bc265521eb00 929 if ((newMaxSpeed >= STSPIN820_MIN_STCK_FREQ)&&\
Davidroid 1:bc265521eb00 930 ((newMaxSpeed <= STSPIN820_MAX_STCK_FREQ)||\
Davidroid 1:bc265521eb00 931 ((device_prm.torqueBoostEnable != FALSE)&&\
Davidroid 1:bc265521eb00 932 ((newMaxSpeed>>STSPIN820_GetStepMode(deviceId))<= STSPIN820_MAX_STCK_FREQ)))&&\
Davidroid 1:bc265521eb00 933 (device_prm.minSpeed <= newMaxSpeed) &&\
Davidroid 1:bc265521eb00 934 (((device_prm.motionState & INACTIVE) == INACTIVE)||\
Davidroid 1:bc265521eb00 935 (device_prm.commandExecuted == RUN_CMD)))
Davidroid 1:bc265521eb00 936 {
Davidroid 1:bc265521eb00 937 device_prm.maxSpeed = newMaxSpeed;
Davidroid 1:bc265521eb00 938 cmdExecuted = TRUE;
Davidroid 1:bc265521eb00 939 }
Davidroid 1:bc265521eb00 940 return cmdExecuted;
Davidroid 1:bc265521eb00 941 }
Davidroid 1:bc265521eb00 942
Davidroid 1:bc265521eb00 943 /******************************************************//**
Davidroid 1:bc265521eb00 944 * @brief Changes the min speed of the specified device
Davidroid 1:bc265521eb00 945 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 946 * @param[in] newMinSpeed New min speed to apply in pps
Davidroid 1:bc265521eb00 947 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 948 * @note The command is not performed is the device is executing
Davidroid 1:bc265521eb00 949 * a MOVE or GOTO command (but it can be used during a RUN command).
Davidroid 1:bc265521eb00 950 **********************************************************/
Davidroid 1:bc265521eb00 951 bool STSPIN820::STSPIN820_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed)
Davidroid 1:bc265521eb00 952 {
Davidroid 1:bc265521eb00 953 bool cmdExecuted = FALSE;
Davidroid 1:bc265521eb00 954 if ((newMinSpeed >= STSPIN820_MIN_STCK_FREQ)&&
Davidroid 1:bc265521eb00 955 (newMinSpeed <= STSPIN820_MAX_STCK_FREQ) &&
Davidroid 1:bc265521eb00 956 (newMinSpeed <= device_prm.maxSpeed) &&
Davidroid 1:bc265521eb00 957 (((device_prm.motionState & INACTIVE) == INACTIVE)||
Davidroid 1:bc265521eb00 958 (device_prm.commandExecuted == RUN_CMD)))
Davidroid 1:bc265521eb00 959 {
Davidroid 1:bc265521eb00 960 device_prm.minSpeed = newMinSpeed;
Davidroid 1:bc265521eb00 961 cmdExecuted = TRUE;
Davidroid 1:bc265521eb00 962 }
Davidroid 1:bc265521eb00 963 return cmdExecuted;
Davidroid 1:bc265521eb00 964 }
Davidroid 1:bc265521eb00 965
Davidroid 1:bc265521eb00 966 /******************************************************//**
Davidroid 1:bc265521eb00 967 * @brief Sets the number of devices to be used
Davidroid 1:bc265521eb00 968 * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES)
Davidroid 1:bc265521eb00 969 * @retval TRUE if successfull, FALSE if failure, attempt to set a number of
Davidroid 1:bc265521eb00 970 * devices greater than MAX_NUMBER_OF_DEVICES
Davidroid 1:bc265521eb00 971 **********************************************************/
Davidroid 1:bc265521eb00 972 bool STSPIN820::STSPIN820_SetNbDevices(uint8_t nbDevices)
Davidroid 1:bc265521eb00 973 {
Davidroid 1:bc265521eb00 974 if (nbDevices <= MAX_NUMBER_OF_DEVICES)
Davidroid 1:bc265521eb00 975 {
Davidroid 1:bc265521eb00 976 number_of_devices = nbDevices;
Davidroid 1:bc265521eb00 977 return TRUE;
Davidroid 1:bc265521eb00 978 }
Davidroid 1:bc265521eb00 979 else
Davidroid 1:bc265521eb00 980 {
Davidroid 1:bc265521eb00 981 return FALSE;
Davidroid 1:bc265521eb00 982 }
Davidroid 1:bc265521eb00 983 }
Davidroid 1:bc265521eb00 984
Davidroid 1:bc265521eb00 985 /******************************************************//**
Davidroid 1:bc265521eb00 986 * @brief Set the stepping mode
Davidroid 1:bc265521eb00 987 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 988 * @param[in] stepMode from full step to 1/256 microstep
Davidroid 1:bc265521eb00 989 * as specified in enum motor_step_mode_t
Davidroid 1:bc265521eb00 990 * 1/64 microstep mode not supported by STSPIN820
Davidroid 1:bc265521eb00 991 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 992 **********************************************************/
Davidroid 1:bc265521eb00 993 bool STSPIN820::STSPIN820_SetStepMode(uint8_t deviceId, motor_step_mode_t stepMode)
Davidroid 1:bc265521eb00 994 {
Davidroid 1:bc265521eb00 995 /* Store step mode */
Davidroid 1:bc265521eb00 996 device_prm.stepMode = stepMode;
Davidroid 1:bc265521eb00 997 device_prm.stepModeLatched = stepMode;
Davidroid 1:bc265521eb00 998
Davidroid 1:bc265521eb00 999 /* Set the mode pins to the levels corresponding to the selected step mode */
Davidroid 1:bc265521eb00 1000 switch (stepMode)
Davidroid 1:bc265521eb00 1001 {
Davidroid 1:bc265521eb00 1002 case STEP_MODE_FULL:
Davidroid 1:bc265521eb00 1003 STSPIN820_Board_SetFullStep();
Davidroid 1:bc265521eb00 1004 break;
Davidroid 1:bc265521eb00 1005 case STEP_MODE_HALF:
Davidroid 1:bc265521eb00 1006 STSPIN820_Board_SetModePins(1, 0, 0);
Davidroid 1:bc265521eb00 1007 break;
Davidroid 1:bc265521eb00 1008 case STEP_MODE_1_4:
Davidroid 1:bc265521eb00 1009 STSPIN820_Board_SetModePins(0, 1, 0);
Davidroid 1:bc265521eb00 1010 break;
Davidroid 1:bc265521eb00 1011 case STEP_MODE_1_8:
Davidroid 1:bc265521eb00 1012 STSPIN820_Board_SetModePins(1, 1, 0);
Davidroid 1:bc265521eb00 1013 break;
Davidroid 1:bc265521eb00 1014 case STEP_MODE_1_16:
Davidroid 1:bc265521eb00 1015 STSPIN820_Board_SetModePins(0, 0, 1);
Davidroid 1:bc265521eb00 1016 break;
Davidroid 1:bc265521eb00 1017 case STEP_MODE_1_32:
Davidroid 1:bc265521eb00 1018 STSPIN820_Board_SetModePins(1, 0, 1);
Davidroid 1:bc265521eb00 1019 break;
Davidroid 1:bc265521eb00 1020 case STEP_MODE_1_128:
Davidroid 1:bc265521eb00 1021 STSPIN820_Board_SetModePins(0, 1, 1);
Davidroid 1:bc265521eb00 1022 break;
Davidroid 1:bc265521eb00 1023 case STEP_MODE_1_256:
Davidroid 1:bc265521eb00 1024 STSPIN820_Board_SetModePins(1, 1, 1);
Davidroid 1:bc265521eb00 1025 break;
Davidroid 1:bc265521eb00 1026 default:
Davidroid 1:bc265521eb00 1027 return FALSE;
Davidroid 1:bc265521eb00 1028 }
Davidroid 1:bc265521eb00 1029
Davidroid 1:bc265521eb00 1030 return TRUE;
Davidroid 1:bc265521eb00 1031
Davidroid 1:bc265521eb00 1032 }
Davidroid 1:bc265521eb00 1033
Davidroid 1:bc265521eb00 1034 /******************************************************//**
Davidroid 1:bc265521eb00 1035 * @brief Select the mode to stop the motor.
Davidroid 1:bc265521eb00 1036 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1037 * @param[in] stopMode HOLD_MODE to let power bridge enabled
Davidroid 1:bc265521eb00 1038 * @retval None
Davidroid 1:bc265521eb00 1039 **********************************************************/
Davidroid 1:bc265521eb00 1040 void STSPIN820::STSPIN820_SetStopMode(uint8_t deviceId, motor_stop_mode_t stopMode)
Davidroid 1:bc265521eb00 1041 {
Davidroid 1:bc265521eb00 1042 device_prm.stopMode = stopMode;
Davidroid 1:bc265521eb00 1043 }
Davidroid 1:bc265521eb00 1044
Davidroid 1:bc265521eb00 1045 /******************************************************//**
Davidroid 1:bc265521eb00 1046 * @brief Set the torque of the specified device
Davidroid 1:bc265521eb00 1047 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1048 * @param[in] torqueMode Torque mode as specified in enum motor_torque_mode_t
Davidroid 1:bc265521eb00 1049 * @param[in] torqueValue in % (from 0 to 100)
Davidroid 1:bc265521eb00 1050 * @retval None
Davidroid 1:bc265521eb00 1051 * @note
Davidroid 1:bc265521eb00 1052 **********************************************************/
Davidroid 1:bc265521eb00 1053 void STSPIN820::STSPIN820_SetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode, uint8_t torqueValue)
Davidroid 1:bc265521eb00 1054 {
Davidroid 1:bc265521eb00 1055 device_prm.updateTorque = TRUE;
Davidroid 1:bc265521eb00 1056 if (torqueValue>100)
Davidroid 1:bc265521eb00 1057 {
Davidroid 1:bc265521eb00 1058 torqueValue = 100;
Davidroid 1:bc265521eb00 1059 }
Davidroid 1:bc265521eb00 1060 switch(torqueMode)
Davidroid 1:bc265521eb00 1061 {
Davidroid 1:bc265521eb00 1062 case ACC_TORQUE:
Davidroid 1:bc265521eb00 1063 device_prm.accelTorque = torqueValue;
Davidroid 1:bc265521eb00 1064 break;
Davidroid 1:bc265521eb00 1065 case DEC_TORQUE:
Davidroid 1:bc265521eb00 1066 device_prm.decelTorque = torqueValue;
Davidroid 1:bc265521eb00 1067 break;
Davidroid 1:bc265521eb00 1068 case RUN_TORQUE:
Davidroid 1:bc265521eb00 1069 device_prm.runTorque = torqueValue;
Davidroid 1:bc265521eb00 1070 break;
Davidroid 1:bc265521eb00 1071 case HOLD_TORQUE:
Davidroid 1:bc265521eb00 1072 device_prm.holdTorque = torqueValue;
Davidroid 1:bc265521eb00 1073 if (device_prm.motionState != INACTIVE)
Davidroid 1:bc265521eb00 1074 {
Davidroid 1:bc265521eb00 1075 break;
Davidroid 1:bc265521eb00 1076 }
Davidroid 1:bc265521eb00 1077 case CURRENT_TORQUE:
Davidroid 1:bc265521eb00 1078 device_prm.currentTorque = torqueValue;
Davidroid 1:bc265521eb00 1079 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
Davidroid 1:bc265521eb00 1080 default:
Davidroid 1:bc265521eb00 1081 device_prm.updateTorque = FALSE;
Davidroid 1:bc265521eb00 1082 break; //ignore error
Davidroid 1:bc265521eb00 1083 }
Davidroid 1:bc265521eb00 1084 }
Davidroid 1:bc265521eb00 1085
Davidroid 1:bc265521eb00 1086 /******************************************************//**
Davidroid 1:bc265521eb00 1087 * @brief Enable or disable the torque boost feature
Davidroid 1:bc265521eb00 1088 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1089 * @param[in] enable true to enable torque boost, false to disable
Davidroid 1:bc265521eb00 1090 * @retval None
Davidroid 1:bc265521eb00 1091 **********************************************************/
Davidroid 1:bc265521eb00 1092 void STSPIN820::STSPIN820_SetTorqueBoostEnable(uint8_t deviceId, bool enable)
Davidroid 1:bc265521eb00 1093 {
Davidroid 1:bc265521eb00 1094 device_prm.torqueBoostEnable = enable;
Davidroid 1:bc265521eb00 1095 }
Davidroid 1:bc265521eb00 1096
Davidroid 1:bc265521eb00 1097 /******************************************************//**
Davidroid 1:bc265521eb00 1098 * @brief Set the torque boost threshold
Davidroid 1:bc265521eb00 1099 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1)
Davidroid 1:bc265521eb00 1100 * @param[in] speedThreshold speed threshold above which the step mode is
Davidroid 1:bc265521eb00 1101 * changed to full step
Davidroid 1:bc265521eb00 1102 * @retval None
Davidroid 1:bc265521eb00 1103 **********************************************************/
Davidroid 1:bc265521eb00 1104 void STSPIN820::STSPIN820_SetTorqueBoostThreshold(uint8_t deviceId, uint16_t speedThreshold)
Davidroid 1:bc265521eb00 1105 {
Davidroid 1:bc265521eb00 1106 device_prm.torqueBoostSpeedThreshold = speedThreshold;
Davidroid 1:bc265521eb00 1107 }
Davidroid 1:bc265521eb00 1108
Davidroid 1:bc265521eb00 1109 /******************************************************//**
Davidroid 1:bc265521eb00 1110 * @brief Stops the motor by using the device deceleration
Davidroid 1:bc265521eb00 1111 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1112 * @retval true if the command is successfully executed, else false
Davidroid 1:bc265521eb00 1113 * @note The command is not performed if the device is in INACTIVE,
Davidroid 1:bc265521eb00 1114 * STANDBYTOINACTIVE or STANDBY state.
Davidroid 1:bc265521eb00 1115 **********************************************************/
Davidroid 1:bc265521eb00 1116 bool STSPIN820::STSPIN820_SoftStop(uint8_t deviceId)
Davidroid 1:bc265521eb00 1117 {
Davidroid 1:bc265521eb00 1118 bool cmdExecuted = FALSE;
Davidroid 1:bc265521eb00 1119 if ((device_prm.motionState & INACTIVE) != INACTIVE)
Davidroid 1:bc265521eb00 1120 {
Davidroid 1:bc265521eb00 1121 device_prm.commandExecuted |= STSPIN820_SOFT_STOP_BIT_MASK;
Davidroid 1:bc265521eb00 1122 cmdExecuted = TRUE;
Davidroid 1:bc265521eb00 1123 }
Davidroid 1:bc265521eb00 1124 return (cmdExecuted);
Davidroid 1:bc265521eb00 1125 }
Davidroid 1:bc265521eb00 1126
Davidroid 1:bc265521eb00 1127 /******************************************************//**
Davidroid 1:bc265521eb00 1128 * @brief Get the frequency of REF PWM of the specified device
Davidroid 1:bc265521eb00 1129 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1130 * @retval the frequency of REF PWM in Hz
Davidroid 1:bc265521eb00 1131 * @note
Davidroid 1:bc265521eb00 1132 **********************************************************/
Davidroid 1:bc265521eb00 1133 uint32_t STSPIN820::STSPIN820_VrefPwmGetFreq(uint8_t deviceId)
Davidroid 1:bc265521eb00 1134 {
Davidroid 1:bc265521eb00 1135 return device_prm.refPwmFreq;
Davidroid 1:bc265521eb00 1136 }
Davidroid 1:bc265521eb00 1137
Davidroid 1:bc265521eb00 1138 /******************************************************//**
Davidroid 1:bc265521eb00 1139 * @brief Set the frequency of REF PWM of the specified device
Davidroid 1:bc265521eb00 1140 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1141 * @param[in] newFreq in Hz
Davidroid 1:bc265521eb00 1142 * @retval None
Davidroid 1:bc265521eb00 1143 * @note
Davidroid 1:bc265521eb00 1144 **********************************************************/
Davidroid 1:bc265521eb00 1145 void STSPIN820::STSPIN820_VrefPwmSetFreq(uint8_t deviceId, uint32_t newFreq)
Davidroid 1:bc265521eb00 1146 {
Davidroid 1:bc265521eb00 1147 device_prm.refPwmFreq = newFreq;
Davidroid 1:bc265521eb00 1148 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(newFreq,device_prm.currentTorque);
Davidroid 1:bc265521eb00 1149 }
Davidroid 1:bc265521eb00 1150
Davidroid 1:bc265521eb00 1151 /******************************************************//**
Davidroid 1:bc265521eb00 1152 * @brief Locks until the device state becomes Inactive
Davidroid 1:bc265521eb00 1153 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1154 * @retval None
Davidroid 1:bc265521eb00 1155 **********************************************************/
Davidroid 1:bc265521eb00 1156 void STSPIN820::STSPIN820_WaitWhileActive(uint8_t deviceId)
Davidroid 1:bc265521eb00 1157 {
Davidroid 1:bc265521eb00 1158 /* Wait while motor is running */
Davidroid 1:bc265521eb00 1159 while (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)!=INACTIVE)||\
Davidroid 1:bc265521eb00 1160 (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)==INACTIVE)&&(toggle_odd!=0)));
Davidroid 1:bc265521eb00 1161 }
Davidroid 1:bc265521eb00 1162
Davidroid 1:bc265521eb00 1163 /******************************************************//**
Davidroid 1:bc265521eb00 1164 * @brief Updates the current speed of the device
Davidroid 1:bc265521eb00 1165 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1166 * @param[in] newSpeed in pps
Davidroid 1:bc265521eb00 1167 * @retval None
Davidroid 1:bc265521eb00 1168 **********************************************************/
Davidroid 1:bc265521eb00 1169 void STSPIN820::STSPIN820_ApplySpeed(uint8_t deviceId, uint16_t newSpeed)
Davidroid 1:bc265521eb00 1170 {
Davidroid 1:bc265521eb00 1171 if (device_prm.torqueBoostEnable != FALSE)
Davidroid 1:bc265521eb00 1172 {
Davidroid 1:bc265521eb00 1173 if (device_prm.stepMode > STEP_MODE_1_256)
Davidroid 1:bc265521eb00 1174 {
Davidroid 1:bc265521eb00 1175 STSPIN820_ErrorHandler(STSPIN820_ERROR_APPLY_SPEED);
Davidroid 1:bc265521eb00 1176 }
Davidroid 1:bc265521eb00 1177 if (device_prm.stepMode != STEP_MODE_FULL)
Davidroid 1:bc265521eb00 1178 {
Davidroid 1:bc265521eb00 1179 if (((newSpeed>>device_prm.stepModeLatched)>\
Davidroid 1:bc265521eb00 1180 device_prm.torqueBoostSpeedThreshold)&&\
Davidroid 1:bc265521eb00 1181 (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) != MOVE_CMD) ||\
Davidroid 1:bc265521eb00 1182 ((device_prm.stepsToTake-device_prm.relativePos)>=\
Davidroid 1:bc265521eb00 1183 (1<<device_prm.stepModeLatched))))
Davidroid 1:bc265521eb00 1184 {
Davidroid 1:bc265521eb00 1185 if ((device_prm.sequencerPosition & 0xFF) == 0x80)
Davidroid 1:bc265521eb00 1186 {
Davidroid 1:bc265521eb00 1187 STSPIN820_Board_SetFullStep();
Davidroid 1:bc265521eb00 1188 device_prm.stepMode = STEP_MODE_FULL;
Davidroid 1:bc265521eb00 1189 device_prm.accu >>= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1190 newSpeed >>= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1191 }
Davidroid 1:bc265521eb00 1192 }
Davidroid 1:bc265521eb00 1193 }
Davidroid 1:bc265521eb00 1194 else if (((newSpeed <= device_prm.torqueBoostSpeedThreshold) &&\
Davidroid 1:bc265521eb00 1195 (device_prm.stepModeLatched != STEP_MODE_FULL))||\
Davidroid 1:bc265521eb00 1196 (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) == MOVE_CMD)&&\
Davidroid 1:bc265521eb00 1197 ((device_prm.stepsToTake-device_prm.relativePos)<=\
Davidroid 1:bc265521eb00 1198 (1<<device_prm.stepModeLatched))))
Davidroid 1:bc265521eb00 1199 {
Davidroid 1:bc265521eb00 1200 STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
Davidroid 1:bc265521eb00 1201 device_prm.stepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1202 device_prm.accu <<= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1203 newSpeed <<= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1204 }
Davidroid 1:bc265521eb00 1205 }
Davidroid 1:bc265521eb00 1206 else if (device_prm.stepMode != device_prm.stepModeLatched)
Davidroid 1:bc265521eb00 1207 {
Davidroid 1:bc265521eb00 1208 //torqueBoostEnable has just been disabled
Davidroid 1:bc265521eb00 1209 STSPIN820_SetStepMode(0, device_prm.stepModeLatched);
Davidroid 1:bc265521eb00 1210 device_prm.stepMode = device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1211 device_prm.accu <<= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1212 newSpeed <<= device_prm.stepModeLatched;
Davidroid 1:bc265521eb00 1213 }
Davidroid 1:bc265521eb00 1214
Davidroid 1:bc265521eb00 1215 if (newSpeed < STSPIN820_MIN_STCK_FREQ)
Davidroid 1:bc265521eb00 1216 {
Davidroid 1:bc265521eb00 1217 newSpeed = STSPIN820_MIN_STCK_FREQ;
Davidroid 1:bc265521eb00 1218 }
Davidroid 1:bc265521eb00 1219 if (newSpeed > STSPIN820_MAX_STCK_FREQ)
Davidroid 1:bc265521eb00 1220 {
Davidroid 1:bc265521eb00 1221 newSpeed = STSPIN820_MAX_STCK_FREQ;
Davidroid 1:bc265521eb00 1222 }
Davidroid 1:bc265521eb00 1223
Davidroid 1:bc265521eb00 1224 device_prm.speed = newSpeed;
Davidroid 1:bc265521eb00 1225 STSPIN820_Board_TimStckSetFreq(newSpeed);
Davidroid 1:bc265521eb00 1226
Davidroid 1:bc265521eb00 1227 }
Davidroid 1:bc265521eb00 1228
Davidroid 1:bc265521eb00 1229 /******************************************************//**
Davidroid 1:bc265521eb00 1230 * @brief Apply the set torque to the specified device
Davidroid 1:bc265521eb00 1231 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1232 * @param[in] torqueMode torque mode
Davidroid 1:bc265521eb00 1233 * @retval None
Davidroid 1:bc265521eb00 1234 * @note
Davidroid 1:bc265521eb00 1235 **********************************************************/
Davidroid 1:bc265521eb00 1236 void STSPIN820::STSPIN820_ApplyTorque(uint8_t deviceId, motor_torque_mode_t torqueMode)
Davidroid 1:bc265521eb00 1237 {
Davidroid 1:bc265521eb00 1238 uint8_t torqueValue = 0;
Davidroid 1:bc265521eb00 1239 device_prm.updateTorque = FALSE;
Davidroid 1:bc265521eb00 1240 switch(torqueMode)
Davidroid 1:bc265521eb00 1241 {
Davidroid 1:bc265521eb00 1242 case ACC_TORQUE:
Davidroid 1:bc265521eb00 1243 device_prm.currentTorque = device_prm.accelTorque;
Davidroid 1:bc265521eb00 1244 break;
Davidroid 1:bc265521eb00 1245 case DEC_TORQUE:
Davidroid 1:bc265521eb00 1246 device_prm.currentTorque = device_prm.decelTorque;
Davidroid 1:bc265521eb00 1247 break;
Davidroid 1:bc265521eb00 1248 case RUN_TORQUE:
Davidroid 1:bc265521eb00 1249 device_prm.currentTorque = device_prm.runTorque;
Davidroid 1:bc265521eb00 1250 break;
Davidroid 1:bc265521eb00 1251 case HOLD_TORQUE:
Davidroid 1:bc265521eb00 1252 device_prm.currentTorque = device_prm.holdTorque;
Davidroid 1:bc265521eb00 1253 break;
Davidroid 1:bc265521eb00 1254 case CURRENT_TORQUE:
Davidroid 1:bc265521eb00 1255 break;
Davidroid 1:bc265521eb00 1256 default:
Davidroid 1:bc265521eb00 1257 return; //ignore error
Davidroid 1:bc265521eb00 1258 }
Davidroid 1:bc265521eb00 1259 torqueValue = device_prm.currentTorque;
Davidroid 1:bc265521eb00 1260 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
Davidroid 1:bc265521eb00 1261 }
Davidroid 1:bc265521eb00 1262
Davidroid 1:bc265521eb00 1263 /******************************************************//**
Davidroid 1:bc265521eb00 1264 * @brief Computes the speed profile according to the number of steps to move
Davidroid 1:bc265521eb00 1265 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1266 * @param[in] nbSteps number of steps to perform
Davidroid 1:bc265521eb00 1267 * @retval None
Davidroid 1:bc265521eb00 1268 * @note Using the acceleration and deceleration of the device,
Davidroid 1:bc265521eb00 1269 * this function determines the duration in steps of the acceleration,
Davidroid 1:bc265521eb00 1270 * steady and deceleration phases.
Davidroid 1:bc265521eb00 1271 * If the total number of steps to perform is big enough, a trapezoidal move
Davidroid 1:bc265521eb00 1272 * is performed (i.e. there is a steady phase where the motor runs at the maximum
Davidroid 1:bc265521eb00 1273 * speed.
Davidroid 1:bc265521eb00 1274 * Else, a triangular move is performed (no steady phase: the maximum speed is never
Davidroid 1:bc265521eb00 1275 * reached.
Davidroid 1:bc265521eb00 1276 **********************************************************/
Davidroid 1:bc265521eb00 1277 void STSPIN820::STSPIN820_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps)
Davidroid 1:bc265521eb00 1278 {
Davidroid 1:bc265521eb00 1279 uint32_t reqAccSteps;
Davidroid 1:bc265521eb00 1280 uint32_t reqDecSteps;
Davidroid 1:bc265521eb00 1281
Davidroid 1:bc265521eb00 1282 /* compute the number of steps to get the targeted speed */
Davidroid 1:bc265521eb00 1283 uint16_t minSpeed = device_prm.minSpeed;
Davidroid 1:bc265521eb00 1284 reqAccSteps = (device_prm.maxSpeed - minSpeed);
Davidroid 1:bc265521eb00 1285 reqAccSteps *= (device_prm.maxSpeed + minSpeed);
Davidroid 1:bc265521eb00 1286 reqDecSteps = reqAccSteps;
Davidroid 1:bc265521eb00 1287 reqAccSteps /= (uint32_t)device_prm.acceleration;
Davidroid 1:bc265521eb00 1288 reqAccSteps /= 2;
Davidroid 1:bc265521eb00 1289
Davidroid 1:bc265521eb00 1290 /* compute the number of steps to stop */
Davidroid 1:bc265521eb00 1291 reqDecSteps /= (uint32_t)device_prm.deceleration;
Davidroid 1:bc265521eb00 1292 reqDecSteps /= 2;
Davidroid 1:bc265521eb00 1293
Davidroid 1:bc265521eb00 1294 if(( reqAccSteps + reqDecSteps ) > nbSteps)
Davidroid 1:bc265521eb00 1295 {
Davidroid 1:bc265521eb00 1296 /* Triangular move */
Davidroid 1:bc265521eb00 1297 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
Davidroid 1:bc265521eb00 1298 uint32_t dec = device_prm.deceleration;
Davidroid 1:bc265521eb00 1299 uint32_t acc = device_prm.acceleration;
Davidroid 1:bc265521eb00 1300
Davidroid 1:bc265521eb00 1301 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec);
Davidroid 1:bc265521eb00 1302 if (reqDecSteps > 1)
Davidroid 1:bc265521eb00 1303 {
Davidroid 1:bc265521eb00 1304 reqAccSteps = reqDecSteps - 1;
Davidroid 1:bc265521eb00 1305 if(reqAccSteps == 0)
Davidroid 1:bc265521eb00 1306 {
Davidroid 1:bc265521eb00 1307 reqAccSteps = 1;
Davidroid 1:bc265521eb00 1308 }
Davidroid 1:bc265521eb00 1309 }
Davidroid 1:bc265521eb00 1310 else
Davidroid 1:bc265521eb00 1311 {
Davidroid 1:bc265521eb00 1312 reqAccSteps = 0;
Davidroid 1:bc265521eb00 1313 }
Davidroid 1:bc265521eb00 1314 device_prm.endAccPos = reqAccSteps;
Davidroid 1:bc265521eb00 1315 device_prm.startDecPos = reqDecSteps;
Davidroid 1:bc265521eb00 1316 }
Davidroid 1:bc265521eb00 1317 else
Davidroid 1:bc265521eb00 1318 {
Davidroid 1:bc265521eb00 1319 /* Trapezoidal move */
Davidroid 1:bc265521eb00 1320 /* accelerating phase to endAccPos */
Davidroid 1:bc265521eb00 1321 /* steady phase from endAccPos to startDecPos */
Davidroid 1:bc265521eb00 1322 /* decelerating from startDecPos to stepsToTake*/
Davidroid 1:bc265521eb00 1323 device_prm.endAccPos = reqAccSteps;
Davidroid 1:bc265521eb00 1324 device_prm.startDecPos = nbSteps - reqDecSteps - 1;
Davidroid 1:bc265521eb00 1325 }
Davidroid 1:bc265521eb00 1326 }
Davidroid 1:bc265521eb00 1327
Davidroid 1:bc265521eb00 1328 /******************************************************//**
Davidroid 1:bc265521eb00 1329 * @brief Handlers of the flag interrupt which calls the user callback (if defined)
Davidroid 1:bc265521eb00 1330 * @retval None
Davidroid 1:bc265521eb00 1331 **********************************************************/
Davidroid 1:bc265521eb00 1332 void STSPIN820::STSPIN820_FlagInterruptHandler(void)
Davidroid 1:bc265521eb00 1333 {
Davidroid 1:bc265521eb00 1334 if (flag_interrupt_callback != 0)
Davidroid 1:bc265521eb00 1335 {
Davidroid 1:bc265521eb00 1336 flag_interrupt_callback();
Davidroid 1:bc265521eb00 1337 }
Davidroid 1:bc265521eb00 1338 }
Davidroid 1:bc265521eb00 1339
Davidroid 1:bc265521eb00 1340 /******************************************************//**
Davidroid 1:bc265521eb00 1341 * @brief Set the parameters of the device whose values are not defined in
Davidroid 1:bc265521eb00 1342 * STSPIN820_config.h
Davidroid 1:bc265521eb00 1343 * @retval None
Davidroid 1:bc265521eb00 1344 **********************************************************/
Davidroid 1:bc265521eb00 1345 void STSPIN820::STSPIN820_SetDeviceParamsOtherValues(void)
Davidroid 1:bc265521eb00 1346 {
Davidroid 1:bc265521eb00 1347 uint16_t tmp;
Davidroid 1:bc265521eb00 1348
Davidroid 1:bc265521eb00 1349 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1350 device_prm.currentPosition = 0;
Davidroid 1:bc265521eb00 1351 device_prm.sequencerPosition = 0;
Davidroid 1:bc265521eb00 1352 device_prm.endAccPos = 0;
Davidroid 1:bc265521eb00 1353 device_prm.relativePos = 0;
Davidroid 1:bc265521eb00 1354 device_prm.startDecPos = 0;
Davidroid 1:bc265521eb00 1355 device_prm.stepsToTake = 0;
Davidroid 1:bc265521eb00 1356 device_prm.updateTorque = FALSE;
Davidroid 1:bc265521eb00 1357 device_prm.speed = 0;
Davidroid 1:bc265521eb00 1358 device_prm.commandExecuted = NO_CMD;
Davidroid 1:bc265521eb00 1359 device_prm.direction = FORWARD;
Davidroid 1:bc265521eb00 1360 tmp = device_prm.minSpeed;
Davidroid 1:bc265521eb00 1361 if (((device_prm.torqueBoostEnable != FALSE)&&\
Davidroid 1:bc265521eb00 1362 (device_prm.torqueBoostSpeedThreshold>STSPIN820_MAX_STCK_FREQ))||\
Davidroid 1:bc265521eb00 1363 (tmp>device_prm.maxSpeed))
Davidroid 1:bc265521eb00 1364 {
Davidroid 1:bc265521eb00 1365 STSPIN820_ErrorHandler(STSPIN820_ERROR_INIT);
Davidroid 1:bc265521eb00 1366 }
Davidroid 1:bc265521eb00 1367 }
Davidroid 1:bc265521eb00 1368
Davidroid 1:bc265521eb00 1369 /******************************************************//**
Davidroid 1:bc265521eb00 1370 * @brief Set the parameters of the device to values of initDevicePrm structure
Davidroid 1:bc265521eb00 1371 * @param pInitDevicePrm structure containing values to initialize the device
Davidroid 1:bc265521eb00 1372 * parameters
Davidroid 1:bc265521eb00 1373 * @retval None
Davidroid 1:bc265521eb00 1374 **********************************************************/
Davidroid 1:bc265521eb00 1375 void STSPIN820::STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* pInitDevicePrm)
Davidroid 1:bc265521eb00 1376 {
Davidroid 1:bc265521eb00 1377 device_prm.motionState = STANDBY;;
Davidroid 1:bc265521eb00 1378
Davidroid 1:bc265521eb00 1379 if (STSPIN820_SetAcceleration(0,pInitDevicePrm->acceleration)==FALSE)
Davidroid 1:bc265521eb00 1380 {
Davidroid 1:bc265521eb00 1381 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
Davidroid 1:bc265521eb00 1382 }
Davidroid 1:bc265521eb00 1383 if (STSPIN820_SetDeceleration(0,pInitDevicePrm->deceleration)==FALSE)
Davidroid 1:bc265521eb00 1384 {
Davidroid 1:bc265521eb00 1385 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
Davidroid 1:bc265521eb00 1386 }
Davidroid 1:bc265521eb00 1387 if (STSPIN820_SetMaxSpeed(0,pInitDevicePrm->maxSpeed)==FALSE)
Davidroid 1:bc265521eb00 1388 {
Davidroid 1:bc265521eb00 1389 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
Davidroid 1:bc265521eb00 1390 }
Davidroid 1:bc265521eb00 1391 if (STSPIN820_SetMinSpeed(0,pInitDevicePrm->minSpeed)==FALSE)
Davidroid 1:bc265521eb00 1392 {
Davidroid 1:bc265521eb00 1393 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
Davidroid 1:bc265521eb00 1394 }
Davidroid 1:bc265521eb00 1395
Davidroid 1:bc265521eb00 1396 STSPIN820_VrefPwmSetFreq(0,pInitDevicePrm->vrefPwmFreq);
Davidroid 1:bc265521eb00 1397 STSPIN820_SetTorque(0,ACC_TORQUE,pInitDevicePrm->accelTorque);
Davidroid 1:bc265521eb00 1398 STSPIN820_SetTorque(0,DEC_TORQUE,pInitDevicePrm->decelTorque);
Davidroid 1:bc265521eb00 1399 STSPIN820_SetTorque(0,RUN_TORQUE,pInitDevicePrm->runTorque);
Davidroid 1:bc265521eb00 1400 STSPIN820_SetTorque(0,HOLD_TORQUE,pInitDevicePrm->holdTorque);
Davidroid 1:bc265521eb00 1401 device_prm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
Davidroid 1:bc265521eb00 1402 device_prm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
Davidroid 1:bc265521eb00 1403 STSPIN820_SetStopMode(0,pInitDevicePrm->stopMode);
Davidroid 1:bc265521eb00 1404
Davidroid 1:bc265521eb00 1405 STSPIN820_SetDeviceParamsOtherValues();
Davidroid 1:bc265521eb00 1406
Davidroid 1:bc265521eb00 1407 /* Eventually deactivate motor */
Davidroid 1:bc265521eb00 1408 if ((device_prm.motionState != INACTIVE)&&\
Davidroid 1:bc265521eb00 1409 (device_prm.motionState != STANDBY))
Davidroid 1:bc265521eb00 1410 {
Davidroid 1:bc265521eb00 1411 STSPIN820_HardHiZ(0);
Davidroid 1:bc265521eb00 1412 }
Davidroid 1:bc265521eb00 1413
Davidroid 1:bc265521eb00 1414 /* Enter standby */
Davidroid 1:bc265521eb00 1415 STSPIN820_Board_Reset();
Davidroid 1:bc265521eb00 1416
Davidroid 1:bc265521eb00 1417 /* Reset the microstepping sequencer position */
Davidroid 1:bc265521eb00 1418 device_prm.sequencerPosition = 0;
Davidroid 1:bc265521eb00 1419
Davidroid 1:bc265521eb00 1420 /* Reset current and mark positions */
Davidroid 1:bc265521eb00 1421 device_prm.currentPosition = 0;
Davidroid 1:bc265521eb00 1422 device_prm.markPosition = 0;
Davidroid 1:bc265521eb00 1423
Davidroid 1:bc265521eb00 1424 /* Set predefined step mode */
Davidroid 1:bc265521eb00 1425 STSPIN820_SetStepMode(0, pInitDevicePrm->stepMode);
Davidroid 1:bc265521eb00 1426
Davidroid 1:bc265521eb00 1427 /* Wait */
Davidroid 1:bc265521eb00 1428 STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
Davidroid 1:bc265521eb00 1429
Davidroid 1:bc265521eb00 1430 /* Exit standby */
Davidroid 1:bc265521eb00 1431 STSPIN820_Board_ReleaseReset();
Davidroid 1:bc265521eb00 1432
Davidroid 1:bc265521eb00 1433 /* Let a delay after reset release*/
Davidroid 1:bc265521eb00 1434 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
Davidroid 1:bc265521eb00 1435
Davidroid 1:bc265521eb00 1436 }
Davidroid 1:bc265521eb00 1437
Davidroid 1:bc265521eb00 1438 /******************************************************//**
Davidroid 1:bc265521eb00 1439 * @brief Sets the parameters of the device to predefined values
Davidroid 1:bc265521eb00 1440 * from STSPIN820_config.h
Davidroid 1:bc265521eb00 1441 * @retval None
Davidroid 1:bc265521eb00 1442 **********************************************************/
Davidroid 1:bc265521eb00 1443 void STSPIN820::STSPIN820_SetDeviceParamsToPredefinedValues(void)
Davidroid 1:bc265521eb00 1444 {
Davidroid 1:bc265521eb00 1445 device_prm.motionState = STANDBY;
Davidroid 1:bc265521eb00 1446
Davidroid 1:bc265521eb00 1447 if (STSPIN820_SetAcceleration(0,STSPIN820_CONF_PARAM_ACC)==FALSE)
Davidroid 1:bc265521eb00 1448 {
Davidroid 1:bc265521eb00 1449 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
Davidroid 1:bc265521eb00 1450 }
Davidroid 1:bc265521eb00 1451 if (STSPIN820_SetDeceleration(0,STSPIN820_CONF_PARAM_DEC)==FALSE)
Davidroid 1:bc265521eb00 1452 {
Davidroid 1:bc265521eb00 1453 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
Davidroid 1:bc265521eb00 1454 }
Davidroid 1:bc265521eb00 1455 if (STSPIN820_SetMaxSpeed(0,STSPIN820_CONF_PARAM_RUNNING_SPEED)==FALSE)
Davidroid 1:bc265521eb00 1456 {
Davidroid 1:bc265521eb00 1457 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
Davidroid 1:bc265521eb00 1458 }
Davidroid 1:bc265521eb00 1459 if (STSPIN820_SetMinSpeed(0,STSPIN820_CONF_PARAM_MIN_SPEED)==FALSE)
Davidroid 1:bc265521eb00 1460 {
Davidroid 1:bc265521eb00 1461 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
Davidroid 1:bc265521eb00 1462 }
Davidroid 1:bc265521eb00 1463
Davidroid 1:bc265521eb00 1464 STSPIN820_VrefPwmSetFreq(0,STSPIN820_CONF_PARAM_REF_PWM_FREQUENCY);
Davidroid 1:bc265521eb00 1465 STSPIN820_SetTorque(0,ACC_TORQUE,STSPIN820_CONF_PARAM_ACC_TORQUE);
Davidroid 1:bc265521eb00 1466 STSPIN820_SetTorque(0,DEC_TORQUE,STSPIN820_CONF_PARAM_DEC_TORQUE);
Davidroid 1:bc265521eb00 1467 STSPIN820_SetTorque(0,RUN_TORQUE,STSPIN820_CONF_PARAM_RUNNING_TORQUE);
Davidroid 1:bc265521eb00 1468 STSPIN820_SetTorque(0,HOLD_TORQUE,STSPIN820_CONF_PARAM_HOLDING_TORQUE);
Davidroid 1:bc265521eb00 1469 device_prm.torqueBoostEnable = STSPIN820_CONF_PARAM_TORQUE_BOOST_EN;
Davidroid 1:bc265521eb00 1470 device_prm.torqueBoostSpeedThreshold = STSPIN820_CONF_PARAM_TORQUE_BOOST_TH;
Davidroid 1:bc265521eb00 1471 STSPIN820_SetStopMode(0,STSPIN820_CONF_PARAM_AUTO_HIZ_STOP);
Davidroid 1:bc265521eb00 1472
Davidroid 1:bc265521eb00 1473 STSPIN820_SetDeviceParamsOtherValues();
Davidroid 1:bc265521eb00 1474
Davidroid 1:bc265521eb00 1475 /* Eventually deactivate motor */
Davidroid 1:bc265521eb00 1476 if ((device_prm.motionState != INACTIVE)&&\
Davidroid 1:bc265521eb00 1477 (device_prm.motionState != STANDBY))
Davidroid 1:bc265521eb00 1478 {
Davidroid 1:bc265521eb00 1479 STSPIN820_HardHiZ(0);
Davidroid 1:bc265521eb00 1480 }
Davidroid 1:bc265521eb00 1481
Davidroid 1:bc265521eb00 1482 /* Enter standby */
Davidroid 1:bc265521eb00 1483 STSPIN820_Board_Reset();
Davidroid 1:bc265521eb00 1484
Davidroid 1:bc265521eb00 1485 /* Reset the microstepping sequencer position */
Davidroid 1:bc265521eb00 1486 device_prm.sequencerPosition = 0;
Davidroid 1:bc265521eb00 1487
Davidroid 1:bc265521eb00 1488 /* Reset current and mark positions */
Davidroid 1:bc265521eb00 1489 device_prm.currentPosition = 0;
Davidroid 1:bc265521eb00 1490 device_prm.markPosition = 0;
Davidroid 1:bc265521eb00 1491
Davidroid 1:bc265521eb00 1492 /* Set predefined step mode */
Davidroid 1:bc265521eb00 1493 STSPIN820_SetStepMode(0, STSPIN820_CONF_PARAM_STEP_MODE);
Davidroid 1:bc265521eb00 1494
Davidroid 1:bc265521eb00 1495 /* Wait */
Davidroid 1:bc265521eb00 1496 STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY);
Davidroid 1:bc265521eb00 1497
Davidroid 1:bc265521eb00 1498 /* Exit standby */
Davidroid 1:bc265521eb00 1499 STSPIN820_Board_ReleaseReset();
Davidroid 1:bc265521eb00 1500
Davidroid 1:bc265521eb00 1501 /* Let a delay after reset release*/
Davidroid 1:bc265521eb00 1502 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
Davidroid 1:bc265521eb00 1503 }
Davidroid 1:bc265521eb00 1504
Davidroid 1:bc265521eb00 1505 /******************************************************//**
Davidroid 1:bc265521eb00 1506 * @brief Initialises the bridge parameters to start the movement
Davidroid 1:bc265521eb00 1507 * and enable the power bridge
Davidroid 1:bc265521eb00 1508 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1509 * @retval None
Davidroid 1:bc265521eb00 1510 **********************************************************/
Davidroid 1:bc265521eb00 1511 void STSPIN820::STSPIN820_StartMovement(uint8_t deviceId)
Davidroid 1:bc265521eb00 1512 {
Davidroid 1:bc265521eb00 1513 deviceId = 0;
Davidroid 1:bc265521eb00 1514
Davidroid 1:bc265521eb00 1515 /* Enable STSPIN820 powerstage */
Davidroid 1:bc265521eb00 1516 STSPIN820_Enable(deviceId);
Davidroid 1:bc265521eb00 1517 toggle_odd = 0;
Davidroid 1:bc265521eb00 1518 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1519 device_prm.relativePos = 0;
Davidroid 1:bc265521eb00 1520 if ((device_prm.endAccPos == 0)&&\
Davidroid 1:bc265521eb00 1521 (device_prm.commandExecuted != RUN_CMD))
Davidroid 1:bc265521eb00 1522 {
Davidroid 1:bc265521eb00 1523 device_prm.motionState = DECELERATING;
Davidroid 1:bc265521eb00 1524 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
Davidroid 1:bc265521eb00 1525 }
Davidroid 1:bc265521eb00 1526 else
Davidroid 1:bc265521eb00 1527 {
Davidroid 1:bc265521eb00 1528 device_prm.motionState = ACCELERATING;
Davidroid 1:bc265521eb00 1529 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
Davidroid 1:bc265521eb00 1530 }
Davidroid 1:bc265521eb00 1531 STSPIN820_Board_PwmRefStart();
Davidroid 1:bc265521eb00 1532 /* Initialize the step clock timer */
Davidroid 1:bc265521eb00 1533 STSPIN820_Board_TimStckInit();
Davidroid 1:bc265521eb00 1534 /* Program the step clock */
Davidroid 1:bc265521eb00 1535 STSPIN820_Board_TimStckCompareInit();
Davidroid 1:bc265521eb00 1536 STSPIN820_ApplySpeed(deviceId, device_prm.minSpeed);
Davidroid 1:bc265521eb00 1537 STSPIN820_Board_TimStckStart();
Davidroid 1:bc265521eb00 1538 }
Davidroid 1:bc265521eb00 1539
Davidroid 1:bc265521eb00 1540 /******************************************************//**
Davidroid 1:bc265521eb00 1541 * @brief Handles the device state machine at each pulse
Davidroid 1:bc265521eb00 1542 * @param[in] deviceId Unused parameter
Davidroid 1:bc265521eb00 1543 * @retval None
Davidroid 1:bc265521eb00 1544 * @note Must only be called by the timer ISR
Davidroid 1:bc265521eb00 1545 **********************************************************/
Davidroid 1:bc265521eb00 1546 void STSPIN820::STSPIN820_StepClockHandler(uint8_t deviceId)
Davidroid 1:bc265521eb00 1547 {
Davidroid 1:bc265521eb00 1548 uint32_t stepModeShift = device_prm.stepModeLatched - device_prm.stepMode;
Davidroid 1:bc265521eb00 1549 uint16_t tmp;
Davidroid 1:bc265521eb00 1550 deviceId = 0;
Davidroid 1:bc265521eb00 1551
Davidroid 1:bc265521eb00 1552 if (device_prm.motionState == STANDBYTOINACTIVE)
Davidroid 1:bc265521eb00 1553 {
Davidroid 1:bc265521eb00 1554 if (toggle_odd != 0)
Davidroid 1:bc265521eb00 1555 {
Davidroid 1:bc265521eb00 1556 toggle_odd = 0;
Davidroid 1:bc265521eb00 1557 if (device_prm.sequencerPosition == 0)
Davidroid 1:bc265521eb00 1558 {
Davidroid 1:bc265521eb00 1559 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
Davidroid 1:bc265521eb00 1560 {
Davidroid 1:bc265521eb00 1561 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
Davidroid 1:bc265521eb00 1562 }
Davidroid 1:bc265521eb00 1563 return;
Davidroid 1:bc265521eb00 1564 }
Davidroid 1:bc265521eb00 1565 }
Davidroid 1:bc265521eb00 1566 else
Davidroid 1:bc265521eb00 1567 {
Davidroid 1:bc265521eb00 1568 toggle_odd = 1;
Davidroid 1:bc265521eb00 1569 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
Davidroid 1:bc265521eb00 1570 device_prm.sequencerPosition -= tmp;
Davidroid 1:bc265521eb00 1571 }
Davidroid 1:bc265521eb00 1572 STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
Davidroid 1:bc265521eb00 1573 return;
Davidroid 1:bc265521eb00 1574 }
Davidroid 1:bc265521eb00 1575
Davidroid 1:bc265521eb00 1576 if (toggle_odd == 0)
Davidroid 1:bc265521eb00 1577 {
Davidroid 1:bc265521eb00 1578 toggle_odd = 1;
Davidroid 1:bc265521eb00 1579 }
Davidroid 1:bc265521eb00 1580 else
Davidroid 1:bc265521eb00 1581 {
Davidroid 1:bc265521eb00 1582 toggle_odd = 0;
Davidroid 1:bc265521eb00 1583 /* Incrementation of the relative position */
Davidroid 1:bc265521eb00 1584 device_prm.relativePos += (1 << stepModeShift);
Davidroid 1:bc265521eb00 1585
Davidroid 1:bc265521eb00 1586 /* Incrementation of the current position */
Davidroid 1:bc265521eb00 1587 if (device_prm.direction != BACKWARD)
Davidroid 1:bc265521eb00 1588 {
Davidroid 1:bc265521eb00 1589 device_prm.currentPosition += (1 << stepModeShift);
Davidroid 1:bc265521eb00 1590 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
Davidroid 1:bc265521eb00 1591 device_prm.sequencerPosition += tmp;
Davidroid 1:bc265521eb00 1592 if (device_prm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
Davidroid 1:bc265521eb00 1593 {
Davidroid 1:bc265521eb00 1594 device_prm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
Davidroid 1:bc265521eb00 1595 }
Davidroid 1:bc265521eb00 1596 }
Davidroid 1:bc265521eb00 1597 else
Davidroid 1:bc265521eb00 1598 {
Davidroid 1:bc265521eb00 1599 device_prm.currentPosition -= (1 << stepModeShift);
Davidroid 1:bc265521eb00 1600 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
Davidroid 1:bc265521eb00 1601 device_prm.sequencerPosition -= tmp;
Davidroid 1:bc265521eb00 1602 if (device_prm.sequencerPosition < 0)
Davidroid 1:bc265521eb00 1603 {
Davidroid 1:bc265521eb00 1604 device_prm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
Davidroid 1:bc265521eb00 1605 }
Davidroid 1:bc265521eb00 1606 }
Davidroid 1:bc265521eb00 1607
Davidroid 1:bc265521eb00 1608 switch (device_prm.motionState)
Davidroid 1:bc265521eb00 1609 {
Davidroid 1:bc265521eb00 1610 case ACCELERATING:
Davidroid 1:bc265521eb00 1611 {
Davidroid 1:bc265521eb00 1612 uint32_t relPos = device_prm.relativePos;
Davidroid 1:bc265521eb00 1613 uint32_t endAccPos = device_prm.endAccPos;
Davidroid 1:bc265521eb00 1614 uint16_t speed = device_prm.speed;
Davidroid 1:bc265521eb00 1615 uint32_t acc = ((uint32_t)device_prm.acceleration << 16)>>stepModeShift;
Davidroid 1:bc265521eb00 1616
Davidroid 1:bc265521eb00 1617 if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
Davidroid 1:bc265521eb00 1618 ((device_prm.commandExecuted==MOVE_CMD)&&(relPos>=device_prm.startDecPos)))
Davidroid 1:bc265521eb00 1619 {
Davidroid 1:bc265521eb00 1620 device_prm.motionState = DECELERATING;
Davidroid 1:bc265521eb00 1621 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1622 /* Apply decelerating torque */
Davidroid 1:bc265521eb00 1623 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
Davidroid 1:bc265521eb00 1624 }
Davidroid 1:bc265521eb00 1625 else if ((speed>=(device_prm.maxSpeed>>stepModeShift))||\
Davidroid 1:bc265521eb00 1626 ((device_prm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
Davidroid 1:bc265521eb00 1627 {
Davidroid 1:bc265521eb00 1628 device_prm.motionState = STEADY;
Davidroid 1:bc265521eb00 1629 /* Apply running torque */
Davidroid 1:bc265521eb00 1630 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
Davidroid 1:bc265521eb00 1631 }
Davidroid 1:bc265521eb00 1632 else
Davidroid 1:bc265521eb00 1633 {
Davidroid 1:bc265521eb00 1634 bool speedUpdated = FALSE;
Davidroid 1:bc265521eb00 1635 /* Go on accelerating */
Davidroid 1:bc265521eb00 1636 if (speed==0)
Davidroid 1:bc265521eb00 1637 {
Davidroid 1:bc265521eb00 1638 speed =1;
Davidroid 1:bc265521eb00 1639 }
Davidroid 1:bc265521eb00 1640 device_prm.accu += acc / speed;
Davidroid 1:bc265521eb00 1641 while (device_prm.accu>=(0X10000L))
Davidroid 1:bc265521eb00 1642 {
Davidroid 1:bc265521eb00 1643 device_prm.accu -= (0X10000L);
Davidroid 1:bc265521eb00 1644 speed +=1;
Davidroid 1:bc265521eb00 1645 speedUpdated = TRUE;
Davidroid 1:bc265521eb00 1646 }
Davidroid 1:bc265521eb00 1647
Davidroid 1:bc265521eb00 1648 if (speedUpdated)
Davidroid 1:bc265521eb00 1649 {
Davidroid 1:bc265521eb00 1650 if (speed>(device_prm.maxSpeed>>stepModeShift))
Davidroid 1:bc265521eb00 1651 {
Davidroid 1:bc265521eb00 1652 speed = device_prm.maxSpeed>>stepModeShift;
Davidroid 1:bc265521eb00 1653 }
Davidroid 1:bc265521eb00 1654 device_prm.speed = speed;
Davidroid 1:bc265521eb00 1655 }
Davidroid 1:bc265521eb00 1656
Davidroid 1:bc265521eb00 1657 if (device_prm.updateTorque!=FALSE)
Davidroid 1:bc265521eb00 1658 {
Davidroid 1:bc265521eb00 1659 /* Apply accelerating torque */
Davidroid 1:bc265521eb00 1660 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
Davidroid 1:bc265521eb00 1661 }
Davidroid 1:bc265521eb00 1662 }
Davidroid 1:bc265521eb00 1663 break;
Davidroid 1:bc265521eb00 1664 }
Davidroid 1:bc265521eb00 1665 case STEADY:
Davidroid 1:bc265521eb00 1666 {
Davidroid 1:bc265521eb00 1667 uint16_t maxSpeed = device_prm.maxSpeed>>stepModeShift;
Davidroid 1:bc265521eb00 1668 uint32_t relativePos = device_prm.relativePos;
Davidroid 1:bc265521eb00 1669 if (device_prm.updateTorque!=FALSE)
Davidroid 1:bc265521eb00 1670 {
Davidroid 1:bc265521eb00 1671 /* Apply accelerating torque */
Davidroid 1:bc265521eb00 1672 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
Davidroid 1:bc265521eb00 1673 }
Davidroid 1:bc265521eb00 1674 if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
Davidroid 1:bc265521eb00 1675 ((device_prm.commandExecuted==MOVE_CMD)&&\
Davidroid 1:bc265521eb00 1676 (relativePos>=(device_prm.startDecPos)))||\
Davidroid 1:bc265521eb00 1677 ((device_prm.commandExecuted==RUN_CMD)&&\
Davidroid 1:bc265521eb00 1678 (device_prm.speed>maxSpeed)))
Davidroid 1:bc265521eb00 1679 {
Davidroid 1:bc265521eb00 1680 device_prm.motionState = DECELERATING;
Davidroid 1:bc265521eb00 1681 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1682 /* Apply decelerating torque */
Davidroid 1:bc265521eb00 1683 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
Davidroid 1:bc265521eb00 1684 }
Davidroid 1:bc265521eb00 1685 else if ((device_prm.commandExecuted==RUN_CMD)&&(device_prm.speed<maxSpeed))
Davidroid 1:bc265521eb00 1686 {
Davidroid 1:bc265521eb00 1687 device_prm.motionState = ACCELERATING;
Davidroid 1:bc265521eb00 1688 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1689 /* Apply accelerating torque */
Davidroid 1:bc265521eb00 1690 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
Davidroid 1:bc265521eb00 1691 }
Davidroid 1:bc265521eb00 1692 break;
Davidroid 1:bc265521eb00 1693 }
Davidroid 1:bc265521eb00 1694 case DECELERATING:
Davidroid 1:bc265521eb00 1695 {
Davidroid 1:bc265521eb00 1696 uint32_t relativePos = device_prm.relativePos;
Davidroid 1:bc265521eb00 1697 uint16_t speed = device_prm.speed;
Davidroid 1:bc265521eb00 1698 uint32_t dec = ((uint32_t)device_prm.deceleration << 16)>>stepModeShift;
Davidroid 1:bc265521eb00 1699 if ((((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)&&\
Davidroid 1:bc265521eb00 1700 (speed<=(device_prm.minSpeed>>stepModeShift)))||\
Davidroid 1:bc265521eb00 1701 ((device_prm.commandExecuted==MOVE_CMD)&&(relativePos>=device_prm.stepsToTake)))
Davidroid 1:bc265521eb00 1702 {
Davidroid 1:bc265521eb00 1703 /* Motion process complete */
Davidroid 1:bc265521eb00 1704 if ((device_prm.commandExecuted&STSPIN820_DIR_CHANGE_BIT_MASK)!=0)
Davidroid 1:bc265521eb00 1705 {
Davidroid 1:bc265521eb00 1706 device_prm.commandExecuted&=~STSPIN820_DIR_CHANGE_BIT_MASK;
Davidroid 1:bc265521eb00 1707 if (device_prm.direction==BACKWARD)
Davidroid 1:bc265521eb00 1708 {
Davidroid 1:bc265521eb00 1709 device_prm.direction=FORWARD;
Davidroid 1:bc265521eb00 1710 }
Davidroid 1:bc265521eb00 1711 else device_prm.direction=BACKWARD;
Davidroid 1:bc265521eb00 1712 STSPIN820_Board_SetDirectionGpio(device_prm.direction);
Davidroid 1:bc265521eb00 1713 if ((device_prm.commandExecuted&STSPIN820_SOFT_STOP_BIT_MASK)==0)
Davidroid 1:bc265521eb00 1714 {
Davidroid 1:bc265521eb00 1715 device_prm.motionState = ACCELERATING;
Davidroid 1:bc265521eb00 1716 device_prm.accu = 0;
Davidroid 1:bc265521eb00 1717 /* Apply accelerating torque */
Davidroid 1:bc265521eb00 1718 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE);
Davidroid 1:bc265521eb00 1719 break;
Davidroid 1:bc265521eb00 1720 }
Davidroid 1:bc265521eb00 1721 }
Davidroid 1:bc265521eb00 1722 if (device_prm.stopMode==HOLD_MODE)
Davidroid 1:bc265521eb00 1723 {
Davidroid 1:bc265521eb00 1724 STSPIN820_HardStop(deviceId);
Davidroid 1:bc265521eb00 1725 }
Davidroid 1:bc265521eb00 1726 else if (device_prm.stopMode==STANDBY_MODE)
Davidroid 1:bc265521eb00 1727 {
Davidroid 1:bc265521eb00 1728 STSPIN820_PutDeviceInStandby(deviceId);
Davidroid 1:bc265521eb00 1729 }
Davidroid 1:bc265521eb00 1730 else
Davidroid 1:bc265521eb00 1731 {
Davidroid 1:bc265521eb00 1732 STSPIN820_HardHiZ(deviceId);
Davidroid 1:bc265521eb00 1733 }
Davidroid 1:bc265521eb00 1734 }
Davidroid 1:bc265521eb00 1735 else if ((device_prm.commandExecuted==RUN_CMD)&&
Davidroid 1:bc265521eb00 1736 (speed<=(device_prm.maxSpeed>>stepModeShift)))
Davidroid 1:bc265521eb00 1737 {
Davidroid 1:bc265521eb00 1738 device_prm.motionState = STEADY;
Davidroid 1:bc265521eb00 1739 /* Apply running torque */
Davidroid 1:bc265521eb00 1740 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE);
Davidroid 1:bc265521eb00 1741 }
Davidroid 1:bc265521eb00 1742 else
Davidroid 1:bc265521eb00 1743 {
Davidroid 1:bc265521eb00 1744 /* Go on decelerating */
Davidroid 1:bc265521eb00 1745 if (speed>(device_prm.minSpeed>>stepModeShift))
Davidroid 1:bc265521eb00 1746 {
Davidroid 1:bc265521eb00 1747 bool speedUpdated = FALSE;
Davidroid 1:bc265521eb00 1748 if (speed==0)
Davidroid 1:bc265521eb00 1749 {
Davidroid 1:bc265521eb00 1750 speed = 1;
Davidroid 1:bc265521eb00 1751 }
Davidroid 1:bc265521eb00 1752 device_prm.accu += dec / speed;
Davidroid 1:bc265521eb00 1753 while (device_prm.accu>=(0X10000L))
Davidroid 1:bc265521eb00 1754 {
Davidroid 1:bc265521eb00 1755 device_prm.accu -= (0X10000L);
Davidroid 1:bc265521eb00 1756 if (speed>1)
Davidroid 1:bc265521eb00 1757 {
Davidroid 1:bc265521eb00 1758 speed -=1;
Davidroid 1:bc265521eb00 1759 }
Davidroid 1:bc265521eb00 1760 speedUpdated = TRUE;
Davidroid 1:bc265521eb00 1761 }
Davidroid 1:bc265521eb00 1762
Davidroid 1:bc265521eb00 1763 if (speedUpdated)
Davidroid 1:bc265521eb00 1764 {
Davidroid 1:bc265521eb00 1765 if (speed<(device_prm.minSpeed>>stepModeShift))
Davidroid 1:bc265521eb00 1766 {
Davidroid 1:bc265521eb00 1767 speed = device_prm.minSpeed>>stepModeShift;
Davidroid 1:bc265521eb00 1768 }
Davidroid 1:bc265521eb00 1769 device_prm.speed = speed;
Davidroid 1:bc265521eb00 1770 }
Davidroid 1:bc265521eb00 1771
Davidroid 1:bc265521eb00 1772 if (device_prm.updateTorque!=FALSE)
Davidroid 1:bc265521eb00 1773 {
Davidroid 1:bc265521eb00 1774 /* Apply decelerating torque */
Davidroid 1:bc265521eb00 1775 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE);
Davidroid 1:bc265521eb00 1776 }
Davidroid 1:bc265521eb00 1777 }
Davidroid 1:bc265521eb00 1778 }
Davidroid 1:bc265521eb00 1779 break;
Davidroid 1:bc265521eb00 1780 }
Davidroid 1:bc265521eb00 1781 default:
Davidroid 1:bc265521eb00 1782 {
Davidroid 1:bc265521eb00 1783 break;
Davidroid 1:bc265521eb00 1784 }
Davidroid 1:bc265521eb00 1785 }
Davidroid 1:bc265521eb00 1786 }
Davidroid 1:bc265521eb00 1787 if ((device_prm.motionState & INACTIVE) != INACTIVE)
Davidroid 1:bc265521eb00 1788 {
Davidroid 1:bc265521eb00 1789 STSPIN820_ApplySpeed(deviceId, device_prm.speed);
Davidroid 1:bc265521eb00 1790 }
Davidroid 1:bc265521eb00 1791 else
Davidroid 1:bc265521eb00 1792 {
Davidroid 1:bc265521eb00 1793 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0)
Davidroid 1:bc265521eb00 1794 {
Davidroid 1:bc265521eb00 1795 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
Davidroid 1:bc265521eb00 1796 }
Davidroid 1:bc265521eb00 1797 }
Davidroid 1:bc265521eb00 1798 }
Davidroid 1:bc265521eb00 1799 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/