Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.

Dependencies:   ST_INTERFACES

Dependents:   HelloWorld_IHM06A1

Fork of X-NUCLEO-IHM06A1 by ST Expansion SW Team

Motor Control Library

Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.

It features the:

  • Read and write of the device parameters; GPIO, PWM and IRQ configuration; microstepping, direction position, speed, acceleration, deceleration and torque controls
  • Automatic full-step switch management; high impedance or hold stop mode selection; enable and standby management
  • Fault interrupts handling (over current, short-circuit and over temperature)
  • Command locking until the device completes movement

The API allows to easily:

  • perform various positioning, moves and stops
  • get/set or monitor the motor positions
  • set the home position and mark another position
  • get/set the minimum and maximum speed
  • get the current speed
  • get/set the acceleration and deceleration
  • get/set the stop mode (hold, hiz or standby)
  • get/set the torque
  • get/set the torque boost
  • get/set the step mode (up to 1/256)

Platform compatibility

Compatible platforms have been tested with the configurations provided by the HelloWorld_IHM06A1 example.

Committer:
nucleosam
Date:
Wed Jul 27 11:56:39 2016 +0000
Revision:
1:0c33082063bc
Parent:
0:0ccd34c56c66
Fixed Stspin220_Board_TimStckStop (missing __enable_irq() function call).; Fixed conditions to exit from FS mode in Stspin220_ApplySpeed function.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucleosam 0:0ccd34c56c66 1 /**
nucleosam 0:0ccd34c56c66 2 ******************************************************************************
nucleosam 0:0ccd34c56c66 3 * @file stspin220_class.cpp
nucleosam 0:0ccd34c56c66 4 * @author IPC Rennes
nucleosam 0:0ccd34c56c66 5 * @version V1.0.0
nucleosam 1:0c33082063bc 6 * @date July 27th, 2016
nucleosam 0:0ccd34c56c66 7 * @brief STSPIN220 product related routines
nucleosam 0:0ccd34c56c66 8 * @note (C) COPYRIGHT 2016 STMicroelectronics
nucleosam 0:0ccd34c56c66 9 ******************************************************************************
nucleosam 0:0ccd34c56c66 10 * @attention
nucleosam 0:0ccd34c56c66 11 *
nucleosam 0:0ccd34c56c66 12 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
nucleosam 0:0ccd34c56c66 13 *
nucleosam 0:0ccd34c56c66 14 * Redistribution and use in source and binary forms, with or without modification,
nucleosam 0:0ccd34c56c66 15 * are permitted provided that the following conditions are met:
nucleosam 0:0ccd34c56c66 16 * 1. Redistributions of source code must retain the above copyright notice,
nucleosam 0:0ccd34c56c66 17 * this list of conditions and the following disclaimer.
nucleosam 0:0ccd34c56c66 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
nucleosam 0:0ccd34c56c66 19 * this list of conditions and the following disclaimer in the documentation
nucleosam 0:0ccd34c56c66 20 * and/or other materials provided with the distribution.
nucleosam 0:0ccd34c56c66 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
nucleosam 0:0ccd34c56c66 22 * may be used to endorse or promote products derived from this software
nucleosam 0:0ccd34c56c66 23 * without specific prior written permission.
nucleosam 0:0ccd34c56c66 24 *
nucleosam 0:0ccd34c56c66 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
nucleosam 0:0ccd34c56c66 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
nucleosam 0:0ccd34c56c66 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
nucleosam 0:0ccd34c56c66 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
nucleosam 0:0ccd34c56c66 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
nucleosam 0:0ccd34c56c66 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
nucleosam 0:0ccd34c56c66 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
nucleosam 0:0ccd34c56c66 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
nucleosam 0:0ccd34c56c66 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
nucleosam 0:0ccd34c56c66 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
nucleosam 0:0ccd34c56c66 35 *
nucleosam 0:0ccd34c56c66 36 ******************************************************************************
nucleosam 0:0ccd34c56c66 37 */
nucleosam 0:0ccd34c56c66 38
nucleosam 0:0ccd34c56c66 39 /* Includes ------------------------------------------------------------------*/
nucleosam 0:0ccd34c56c66 40 #include "stspin220_class.h"
nucleosam 0:0ccd34c56c66 41
nucleosam 0:0ccd34c56c66 42 /* Definitions ---------------------------------------------------------------*/
nucleosam 0:0ccd34c56c66 43
nucleosam 0:0ccd34c56c66 44 /* Variables ----------------------------------------------------------------*/
nucleosam 0:0ccd34c56c66 45 /* Number of devices. */
nucleosam 0:0ccd34c56c66 46 uint8_t STSPIN220::numberOfDevices = 0;
nucleosam 0:0ccd34c56c66 47
nucleosam 0:0ccd34c56c66 48 /* Methods -------------------------------------------------------------------*/
nucleosam 0:0ccd34c56c66 49 /******************************************************//**
nucleosam 0:0ccd34c56c66 50 * @brief Start the STSPIN220 library
nucleosam 0:0ccd34c56c66 51 * @param[in] pInit pointer to the initialization data
nucleosam 0:0ccd34c56c66 52 * @retval COMPONENT_OK in case of success.
nucleosam 0:0ccd34c56c66 53 **********************************************************/
nucleosam 0:0ccd34c56c66 54 Status_t STSPIN220::Stspin220_Init(void* pInit)
nucleosam 0:0ccd34c56c66 55 {
nucleosam 0:0ccd34c56c66 56 Stspin220_Disable();
nucleosam 0:0ccd34c56c66 57 if (pInit == NULL)
nucleosam 0:0ccd34c56c66 58 {
nucleosam 0:0ccd34c56c66 59 /* Set context variables to the predefined values from Stspin220_target_config.h */
nucleosam 0:0ccd34c56c66 60 /* Set GPIO according to these values */
nucleosam 0:0ccd34c56c66 61 Stspin220_SetDeviceParamsToPredefinedValues();
nucleosam 0:0ccd34c56c66 62 }
nucleosam 0:0ccd34c56c66 63 else
nucleosam 0:0ccd34c56c66 64 {
nucleosam 0:0ccd34c56c66 65 Stspin220_SetDeviceParamsToGivenValues((Stspin220_Init_t*) pInit);
nucleosam 0:0ccd34c56c66 66 }
nucleosam 0:0ccd34c56c66 67 Stspin220_Board_TimStckInit(false);
nucleosam 0:0ccd34c56c66 68 return COMPONENT_OK;
nucleosam 0:0ccd34c56c66 69 }
nucleosam 0:0ccd34c56c66 70
nucleosam 0:0ccd34c56c66 71 /**********************************************************
nucleosam 0:0ccd34c56c66 72 * @brief Read id
nucleosam 0:0ccd34c56c66 73 * @param id pointer to the identifier to be read.
nucleosam 0:0ccd34c56c66 74 * @retval COMPONENT_OK in case of success.
nucleosam 0:0ccd34c56c66 75 **********************************************************/
nucleosam 0:0ccd34c56c66 76 Status_t STSPIN220::Stspin220_ReadID(uint8_t *id)
nucleosam 0:0ccd34c56c66 77 {
nucleosam 0:0ccd34c56c66 78 *id = deviceInstance;
nucleosam 0:0ccd34c56c66 79
nucleosam 0:0ccd34c56c66 80 return COMPONENT_OK;
nucleosam 0:0ccd34c56c66 81 }
nucleosam 0:0ccd34c56c66 82
nucleosam 0:0ccd34c56c66 83 /**********************************************************
nucleosam 0:0ccd34c56c66 84 * @brief Attaches a user callback to the error Handler.
nucleosam 0:0ccd34c56c66 85 * The call back will be then called each time the library
nucleosam 0:0ccd34c56c66 86 * detects an error
nucleosam 0:0ccd34c56c66 87 * @param[in] callback Name of the callback to attach
nucleosam 0:0ccd34c56c66 88 * to the error Hanlder
nucleosam 0:0ccd34c56c66 89 * @retval None
nucleosam 0:0ccd34c56c66 90 **********************************************************/
nucleosam 0:0ccd34c56c66 91 void STSPIN220::Stspin220_AttachErrorHandler(void (*callback)(uint16_t error))
nucleosam 0:0ccd34c56c66 92 {
nucleosam 0:0ccd34c56c66 93 errorHandlerCallback = (void (*)(uint16_t error)) callback;
nucleosam 0:0ccd34c56c66 94 }
nucleosam 0:0ccd34c56c66 95
nucleosam 0:0ccd34c56c66 96 /******************************************************//**
nucleosam 0:0ccd34c56c66 97 * @brief Apply the set torque
nucleosam 0:0ccd34c56c66 98 * @param[in] torqueMode torque mode
nucleosam 0:0ccd34c56c66 99 * @retval None
nucleosam 0:0ccd34c56c66 100 * @note
nucleosam 0:0ccd34c56c66 101 **********************************************************/
nucleosam 0:0ccd34c56c66 102 void STSPIN220::Stspin220_ApplyTorque(motorTorqueMode_t torqueMode)
nucleosam 0:0ccd34c56c66 103 {
nucleosam 0:0ccd34c56c66 104 uint8_t torqueValue = 0;
nucleosam 0:0ccd34c56c66 105 devicePrm.updateTorque = false;
nucleosam 0:0ccd34c56c66 106 switch(torqueMode)
nucleosam 0:0ccd34c56c66 107 {
nucleosam 0:0ccd34c56c66 108 case ACC_TORQUE:
nucleosam 0:0ccd34c56c66 109 devicePrm.currentTorque = devicePrm.accelTorque;
nucleosam 0:0ccd34c56c66 110 break;
nucleosam 0:0ccd34c56c66 111 case DEC_TORQUE:
nucleosam 0:0ccd34c56c66 112 devicePrm.currentTorque = devicePrm.decelTorque;
nucleosam 0:0ccd34c56c66 113 break;
nucleosam 0:0ccd34c56c66 114 case RUN_TORQUE:
nucleosam 0:0ccd34c56c66 115 devicePrm.currentTorque = devicePrm.runTorque;
nucleosam 0:0ccd34c56c66 116 break;
nucleosam 0:0ccd34c56c66 117 case HOLD_TORQUE:
nucleosam 0:0ccd34c56c66 118 devicePrm.currentTorque = devicePrm.holdTorque;
nucleosam 0:0ccd34c56c66 119 break;
nucleosam 0:0ccd34c56c66 120 case CURRENT_TORQUE:
nucleosam 0:0ccd34c56c66 121 break;
nucleosam 0:0ccd34c56c66 122 default:
nucleosam 0:0ccd34c56c66 123 return; //ignore error
nucleosam 0:0ccd34c56c66 124 }
nucleosam 0:0ccd34c56c66 125 torqueValue = devicePrm.currentTorque;
nucleosam 0:0ccd34c56c66 126 Stspin220_Board_PwmRefSetDutyCycle(torqueValue);
nucleosam 0:0ccd34c56c66 127 }
nucleosam 0:0ccd34c56c66 128
nucleosam 0:0ccd34c56c66 129 /******************************************************//**
nucleosam 0:0ccd34c56c66 130 * @brief Disable the power bridges (leave the output bridges HiZ)
nucleosam 0:0ccd34c56c66 131 * @retval None
nucleosam 0:0ccd34c56c66 132 **********************************************************/
nucleosam 0:0ccd34c56c66 133 void STSPIN220::Stspin220_Disable(void)
nucleosam 0:0ccd34c56c66 134 {
nucleosam 0:0ccd34c56c66 135 Stspin220_Board_Disable();
nucleosam 0:0ccd34c56c66 136 }
nucleosam 0:0ccd34c56c66 137
nucleosam 0:0ccd34c56c66 138 /******************************************************//**
nucleosam 0:0ccd34c56c66 139 * @brief Enable the power bridges
nucleosam 0:0ccd34c56c66 140 * @retval None
nucleosam 0:0ccd34c56c66 141 **********************************************************/
nucleosam 0:0ccd34c56c66 142 void STSPIN220::Stspin220_Enable(void)
nucleosam 0:0ccd34c56c66 143 {
nucleosam 0:0ccd34c56c66 144 Stspin220_Board_Enable();
nucleosam 0:0ccd34c56c66 145 }
nucleosam 0:0ccd34c56c66 146
nucleosam 0:0ccd34c56c66 147 /******************************************************//**
nucleosam 0:0ccd34c56c66 148 * @brief Error handler which calls the user callback (if defined)
nucleosam 0:0ccd34c56c66 149 * @param[in] error Number of the error
nucleosam 0:0ccd34c56c66 150 * @retval None
nucleosam 0:0ccd34c56c66 151 **********************************************************/
nucleosam 0:0ccd34c56c66 152 void STSPIN220::Stspin220_ErrorHandler(uint16_t error)
nucleosam 0:0ccd34c56c66 153 {
nucleosam 0:0ccd34c56c66 154 if (errorHandlerCallback != 0)
nucleosam 0:0ccd34c56c66 155 {
nucleosam 0:0ccd34c56c66 156 errorHandlerCallback(error);
nucleosam 0:0ccd34c56c66 157 }
nucleosam 0:0ccd34c56c66 158 else
nucleosam 0:0ccd34c56c66 159 {
nucleosam 0:0ccd34c56c66 160 while(1)
nucleosam 0:0ccd34c56c66 161 {
nucleosam 0:0ccd34c56c66 162 /* Infinite loop */
nucleosam 0:0ccd34c56c66 163 }
nucleosam 0:0ccd34c56c66 164 }
nucleosam 0:0ccd34c56c66 165 }
nucleosam 0:0ccd34c56c66 166
nucleosam 0:0ccd34c56c66 167 /******************************************************//**
nucleosam 0:0ccd34c56c66 168 * @brief Exit STSPIN220 device from standby (low power consumption)
nucleosam 0:0ccd34c56c66 169 * @retval None
nucleosam 0:0ccd34c56c66 170 **********************************************************/
nucleosam 0:0ccd34c56c66 171 void STSPIN220::Stspin220_ExitDeviceFromStandby(void)
nucleosam 0:0ccd34c56c66 172 {
nucleosam 0:0ccd34c56c66 173 uint32_t sequencerPosition = devicePrm.sequencerPosition;
nucleosam 0:0ccd34c56c66 174
nucleosam 0:0ccd34c56c66 175 /* Exit standby and set step mode */
nucleosam 0:0ccd34c56c66 176 Stspin220_SetStepModeWithoutReset(devicePrm.stepMode);
nucleosam 0:0ccd34c56c66 177
nucleosam 0:0ccd34c56c66 178 if (devicePrm.sequencerPosition != 0)
nucleosam 0:0ccd34c56c66 179 {
nucleosam 0:0ccd34c56c66 180 /* Set direction to FORWARD to ensure the HW sequencer is increased at */
nucleosam 0:0ccd34c56c66 181 /* each step clock rising edge */
nucleosam 0:0ccd34c56c66 182 Stspin220_SetDirection(FORWARD);
nucleosam 0:0ccd34c56c66 183 /* Going out of standby */
nucleosam 0:0ccd34c56c66 184 devicePrm.motionState = STANDBYTOINACTIVE;
nucleosam 0:0ccd34c56c66 185 /* Program the step clock */
nucleosam 0:0ccd34c56c66 186 Stspin220_Board_TimStckInit(true);
nucleosam 0:0ccd34c56c66 187 Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ);
nucleosam 0:0ccd34c56c66 188 toggleOdd = 0;
nucleosam 0:0ccd34c56c66 189 while (devicePrm.sequencerPosition != 0);
nucleosam 0:0ccd34c56c66 190 while (toggleOdd!=0);
nucleosam 0:0ccd34c56c66 191 devicePrm.sequencerPosition = sequencerPosition;
nucleosam 0:0ccd34c56c66 192 }
nucleosam 0:0ccd34c56c66 193
nucleosam 0:0ccd34c56c66 194 devicePrm.motionState = INACTIVE;
nucleosam 0:0ccd34c56c66 195 }
nucleosam 0:0ccd34c56c66 196
nucleosam 0:0ccd34c56c66 197 /******************************************************//**
nucleosam 0:0ccd34c56c66 198 * @brief Return the acceleration
nucleosam 0:0ccd34c56c66 199 * @retval Acceleration in pps^2
nucleosam 0:0ccd34c56c66 200 **********************************************************/
nucleosam 0:0ccd34c56c66 201 uint16_t STSPIN220::Stspin220_GetAcceleration(void)
nucleosam 0:0ccd34c56c66 202 {
nucleosam 0:0ccd34c56c66 203 return (devicePrm.acceleration);
nucleosam 0:0ccd34c56c66 204 }
nucleosam 0:0ccd34c56c66 205
nucleosam 0:0ccd34c56c66 206 /******************************************************//**
nucleosam 0:0ccd34c56c66 207 * @brief Return the current speed
nucleosam 0:0ccd34c56c66 208 * @retval Speed in pps
nucleosam 0:0ccd34c56c66 209 **********************************************************/
nucleosam 0:0ccd34c56c66 210 uint16_t STSPIN220::Stspin220_GetCurrentSpeed(void)
nucleosam 0:0ccd34c56c66 211 {
nucleosam 0:0ccd34c56c66 212 return devicePrm.speed;
nucleosam 0:0ccd34c56c66 213 }
nucleosam 0:0ccd34c56c66 214
nucleosam 0:0ccd34c56c66 215 /******************************************************//**
nucleosam 0:0ccd34c56c66 216 * @brief Return the deceleration
nucleosam 0:0ccd34c56c66 217 * @retval Deceleration in pps^2
nucleosam 0:0ccd34c56c66 218 **********************************************************/
nucleosam 0:0ccd34c56c66 219 uint16_t STSPIN220::Stspin220_GetDeceleration(void)
nucleosam 0:0ccd34c56c66 220 {
nucleosam 0:0ccd34c56c66 221 return (devicePrm.deceleration);
nucleosam 0:0ccd34c56c66 222 }
nucleosam 0:0ccd34c56c66 223
nucleosam 0:0ccd34c56c66 224 /******************************************************//**
nucleosam 0:0ccd34c56c66 225 * @brief Return the device state
nucleosam 0:0ccd34c56c66 226 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
nucleosam 0:0ccd34c56c66 227 **********************************************************/
nucleosam 0:0ccd34c56c66 228 motorState_t STSPIN220::Stspin220_GetDeviceState(void)
nucleosam 0:0ccd34c56c66 229 {
nucleosam 0:0ccd34c56c66 230 return devicePrm.motionState;
nucleosam 0:0ccd34c56c66 231 }
nucleosam 0:0ccd34c56c66 232
nucleosam 0:0ccd34c56c66 233 /******************************************************//**
nucleosam 0:0ccd34c56c66 234 * @brief Get the motor current direction
nucleosam 0:0ccd34c56c66 235 * @retval direction
nucleosam 0:0ccd34c56c66 236 **********************************************************/
nucleosam 0:0ccd34c56c66 237 motorDir_t STSPIN220::Stspin220_GetDirection(void)
nucleosam 0:0ccd34c56c66 238 {
nucleosam 0:0ccd34c56c66 239 return devicePrm.direction;
nucleosam 0:0ccd34c56c66 240 }
nucleosam 0:0ccd34c56c66 241
nucleosam 0:0ccd34c56c66 242 /******************************************************//**
nucleosam 0:0ccd34c56c66 243 * @brief Return the FW version.
nucleosam 0:0ccd34c56c66 244 * @retval FW version
nucleosam 0:0ccd34c56c66 245 **********************************************************/
nucleosam 0:0ccd34c56c66 246 uint32_t STSPIN220::Stspin220_GetFwVersion(void)
nucleosam 0:0ccd34c56c66 247 {
nucleosam 0:0ccd34c56c66 248 return (STSPIN220_FW_VERSION);
nucleosam 0:0ccd34c56c66 249 }
nucleosam 0:0ccd34c56c66 250
nucleosam 0:0ccd34c56c66 251 /******************************************************//**
nucleosam 0:0ccd34c56c66 252 * @brief Get the mark position (32b signed)
nucleosam 0:0ccd34c56c66 253 * @retval mark position
nucleosam 0:0ccd34c56c66 254 **********************************************************/
nucleosam 0:0ccd34c56c66 255 int32_t STSPIN220::Stspin220_GetMark(void)
nucleosam 0:0ccd34c56c66 256 {
nucleosam 0:0ccd34c56c66 257 return devicePrm.markPosition;
nucleosam 0:0ccd34c56c66 258 }
nucleosam 0:0ccd34c56c66 259
nucleosam 0:0ccd34c56c66 260 /******************************************************//**
nucleosam 0:0ccd34c56c66 261 * @brief Return the max speed
nucleosam 0:0ccd34c56c66 262 * @retval maxSpeed in pps
nucleosam 0:0ccd34c56c66 263 **********************************************************/
nucleosam 0:0ccd34c56c66 264 uint16_t STSPIN220::Stspin220_GetMaxSpeed(void)
nucleosam 0:0ccd34c56c66 265 {
nucleosam 0:0ccd34c56c66 266 return (devicePrm.maxSpeed);
nucleosam 0:0ccd34c56c66 267 }
nucleosam 0:0ccd34c56c66 268
nucleosam 0:0ccd34c56c66 269 /******************************************************//**
nucleosam 0:0ccd34c56c66 270 * @brief Get the min speed
nucleosam 0:0ccd34c56c66 271 * in step/s for full, half and wave modes
nucleosam 0:0ccd34c56c66 272 * in microsteps/s for microstep modes
nucleosam 0:0ccd34c56c66 273 * @retval return the min speed in step/s or microstep/s
nucleosam 0:0ccd34c56c66 274 * @note
nucleosam 0:0ccd34c56c66 275 **********************************************************/
nucleosam 0:0ccd34c56c66 276 uint16_t STSPIN220::Stspin220_GetMinSpeed(void)
nucleosam 0:0ccd34c56c66 277 {
nucleosam 0:0ccd34c56c66 278 return (devicePrm.minSpeed);
nucleosam 0:0ccd34c56c66 279 }
nucleosam 0:0ccd34c56c66 280
nucleosam 0:0ccd34c56c66 281 /******************************************************//**
nucleosam 0:0ccd34c56c66 282 * @brief Get the current position (32b signed)
nucleosam 0:0ccd34c56c66 283 * @retval current position value
nucleosam 0:0ccd34c56c66 284 **********************************************************/
nucleosam 0:0ccd34c56c66 285 int32_t STSPIN220::Stspin220_GetPosition(void)
nucleosam 0:0ccd34c56c66 286 {
nucleosam 0:0ccd34c56c66 287 return devicePrm.currentPosition;
nucleosam 0:0ccd34c56c66 288 }
nucleosam 0:0ccd34c56c66 289
nucleosam 0:0ccd34c56c66 290 /******************************************************//**
nucleosam 0:0ccd34c56c66 291 * @brief Get the motor step mode
nucleosam 0:0ccd34c56c66 292 * @retval step mode
nucleosam 0:0ccd34c56c66 293 **********************************************************/
nucleosam 0:0ccd34c56c66 294 motorStepMode_t STSPIN220::Stspin220_GetStepMode(void)
nucleosam 0:0ccd34c56c66 295 {
nucleosam 0:0ccd34c56c66 296 return devicePrm.stepMode;
nucleosam 0:0ccd34c56c66 297 }
nucleosam 0:0ccd34c56c66 298
nucleosam 0:0ccd34c56c66 299 /******************************************************//**
nucleosam 0:0ccd34c56c66 300 * @brief Get the selected stop mode
nucleosam 0:0ccd34c56c66 301 * @retval the selected stop mode
nucleosam 0:0ccd34c56c66 302 **********************************************************/
nucleosam 0:0ccd34c56c66 303 motorStopMode_t STSPIN220::Stspin220_GetStopMode(void)
nucleosam 0:0ccd34c56c66 304 {
nucleosam 0:0ccd34c56c66 305 return devicePrm.stopMode;
nucleosam 0:0ccd34c56c66 306 }
nucleosam 0:0ccd34c56c66 307
nucleosam 0:0ccd34c56c66 308 /******************************************************//**
nucleosam 0:0ccd34c56c66 309 * @brief Get the torque
nucleosam 0:0ccd34c56c66 310 * @param[in] torqueMode torque mode
nucleosam 0:0ccd34c56c66 311 * @retval the torqueValue in % (from 0 to 100)
nucleosam 0:0ccd34c56c66 312 * @note
nucleosam 0:0ccd34c56c66 313 **********************************************************/
nucleosam 0:0ccd34c56c66 314 uint8_t STSPIN220::Stspin220_GetTorque(motorTorqueMode_t torqueMode)
nucleosam 0:0ccd34c56c66 315 {
nucleosam 0:0ccd34c56c66 316 uint8_t torqueValue = 0;
nucleosam 0:0ccd34c56c66 317 switch(torqueMode)
nucleosam 0:0ccd34c56c66 318 {
nucleosam 0:0ccd34c56c66 319 case ACC_TORQUE:
nucleosam 0:0ccd34c56c66 320 torqueValue = devicePrm.accelTorque;
nucleosam 0:0ccd34c56c66 321 break;
nucleosam 0:0ccd34c56c66 322 case DEC_TORQUE:
nucleosam 0:0ccd34c56c66 323 torqueValue = devicePrm.decelTorque;
nucleosam 0:0ccd34c56c66 324 break;
nucleosam 0:0ccd34c56c66 325 case RUN_TORQUE:
nucleosam 0:0ccd34c56c66 326 torqueValue = devicePrm.runTorque;
nucleosam 0:0ccd34c56c66 327 break;
nucleosam 0:0ccd34c56c66 328 case HOLD_TORQUE:
nucleosam 0:0ccd34c56c66 329 torqueValue = devicePrm.holdTorque;
nucleosam 0:0ccd34c56c66 330 break;
nucleosam 0:0ccd34c56c66 331 case CURRENT_TORQUE:
nucleosam 0:0ccd34c56c66 332 torqueValue = devicePrm.currentTorque;
nucleosam 0:0ccd34c56c66 333 break;
nucleosam 0:0ccd34c56c66 334 default:
nucleosam 0:0ccd34c56c66 335 break;
nucleosam 0:0ccd34c56c66 336 }
nucleosam 0:0ccd34c56c66 337 return torqueValue;
nucleosam 0:0ccd34c56c66 338 }
nucleosam 0:0ccd34c56c66 339
nucleosam 0:0ccd34c56c66 340 /******************************************************//**
nucleosam 0:0ccd34c56c66 341 * @brief Get the torque boost feature status
nucleosam 0:0ccd34c56c66 342 * @retval TRUE if enabled, FALSE if disabled
nucleosam 0:0ccd34c56c66 343 **********************************************************/
nucleosam 0:0ccd34c56c66 344 bool STSPIN220::Stspin220_GetTorqueBoostEnable(void)
nucleosam 0:0ccd34c56c66 345 {
nucleosam 0:0ccd34c56c66 346 return devicePrm.torqueBoostEnable;
nucleosam 0:0ccd34c56c66 347 }
nucleosam 0:0ccd34c56c66 348
nucleosam 0:0ccd34c56c66 349 /******************************************************//**
nucleosam 0:0ccd34c56c66 350 * @brief Get the torque boost threshold
nucleosam 0:0ccd34c56c66 351 * @retval The torque boost threshold above which the step mode is
nucleosam 0:0ccd34c56c66 352 * changed to full step
nucleosam 0:0ccd34c56c66 353 **********************************************************/
nucleosam 0:0ccd34c56c66 354 uint16_t STSPIN220::Stspin220_GetTorqueBoostThreshold(void)
nucleosam 0:0ccd34c56c66 355 {
nucleosam 0:0ccd34c56c66 356 return devicePrm.torqueBoostSpeedThreshold;
nucleosam 0:0ccd34c56c66 357 }
nucleosam 0:0ccd34c56c66 358
nucleosam 0:0ccd34c56c66 359 /******************************************************//**
nucleosam 0:0ccd34c56c66 360 * @brief Go to the home position
nucleosam 0:0ccd34c56c66 361 * @retval None
nucleosam 0:0ccd34c56c66 362 **********************************************************/
nucleosam 0:0ccd34c56c66 363 void STSPIN220::Stspin220_GoHome(void)
nucleosam 0:0ccd34c56c66 364 {
nucleosam 0:0ccd34c56c66 365 Stspin220_GoTo(0);
nucleosam 0:0ccd34c56c66 366 }
nucleosam 0:0ccd34c56c66 367
nucleosam 0:0ccd34c56c66 368 /******************************************************//**
nucleosam 0:0ccd34c56c66 369 * @brief Go to the Mark position
nucleosam 0:0ccd34c56c66 370 * @retval None
nucleosam 0:0ccd34c56c66 371 **********************************************************/
nucleosam 0:0ccd34c56c66 372 void STSPIN220::Stspin220_GoMark(void)
nucleosam 0:0ccd34c56c66 373 {
nucleosam 0:0ccd34c56c66 374 Stspin220_GoTo(devicePrm.markPosition);
nucleosam 0:0ccd34c56c66 375 }
nucleosam 0:0ccd34c56c66 376
nucleosam 0:0ccd34c56c66 377 /******************************************************//**
nucleosam 0:0ccd34c56c66 378 * @brief Request the motor to move to the specified position
nucleosam 0:0ccd34c56c66 379 * @param[in] targetPosition absolute position in steps
nucleosam 0:0ccd34c56c66 380 * @retval None
nucleosam 0:0ccd34c56c66 381 * @note The position is at the resolution corresponding to the
nucleosam 0:0ccd34c56c66 382 * selected step mode.
nucleosam 0:0ccd34c56c66 383 * STEP_MODE_FULL : step
nucleosam 0:0ccd34c56c66 384 * STEP_MODE_HALF : 1/2 step
nucleosam 0:0ccd34c56c66 385 * STEP_MODE_1_4 : 1/4 step
nucleosam 0:0ccd34c56c66 386 * STEP_MODE_1_8 : 1/8 step
nucleosam 0:0ccd34c56c66 387 * STEP_MODE_1_16 : 1/16 step
nucleosam 0:0ccd34c56c66 388 * STEP_MODE_1_32 : 1/32 step
nucleosam 0:0ccd34c56c66 389 * STEP_MODE_1_64 : 1/64 step
nucleosam 0:0ccd34c56c66 390 * STEP_MODE_1_128 : 1/128 step
nucleosam 0:0ccd34c56c66 391 * STEP_MODE_1_256 : 1/256 step
nucleosam 0:0ccd34c56c66 392 **********************************************************/
nucleosam 0:0ccd34c56c66 393 void STSPIN220::Stspin220_GoTo(int32_t targetPosition)
nucleosam 0:0ccd34c56c66 394 {
nucleosam 0:0ccd34c56c66 395 motorDir_t direction;
nucleosam 0:0ccd34c56c66 396
nucleosam 0:0ccd34c56c66 397 /* Exit from standby if needed */
nucleosam 0:0ccd34c56c66 398 if (devicePrm.motionState == STANDBY)
nucleosam 0:0ccd34c56c66 399 {
nucleosam 0:0ccd34c56c66 400 Stspin220_ExitDeviceFromStandby();
nucleosam 0:0ccd34c56c66 401 }
nucleosam 0:0ccd34c56c66 402 /* Deactivate motor if needed */
nucleosam 0:0ccd34c56c66 403 else if (devicePrm.motionState != INACTIVE)
nucleosam 0:0ccd34c56c66 404 {
nucleosam 0:0ccd34c56c66 405 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 406 }
nucleosam 0:0ccd34c56c66 407
nucleosam 0:0ccd34c56c66 408 if (targetPosition > devicePrm.currentPosition)
nucleosam 0:0ccd34c56c66 409 {
nucleosam 0:0ccd34c56c66 410 devicePrm.stepsToTake = targetPosition -\
nucleosam 0:0ccd34c56c66 411 devicePrm.currentPosition;
nucleosam 0:0ccd34c56c66 412 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
nucleosam 0:0ccd34c56c66 413 {
nucleosam 0:0ccd34c56c66 414 direction = FORWARD;
nucleosam 0:0ccd34c56c66 415 }
nucleosam 0:0ccd34c56c66 416 else
nucleosam 0:0ccd34c56c66 417 {
nucleosam 0:0ccd34c56c66 418 direction = BACKWARD;
nucleosam 0:0ccd34c56c66 419 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
nucleosam 0:0ccd34c56c66 420 devicePrm.stepsToTake;
nucleosam 0:0ccd34c56c66 421 }
nucleosam 0:0ccd34c56c66 422 }
nucleosam 0:0ccd34c56c66 423 else
nucleosam 0:0ccd34c56c66 424 {
nucleosam 0:0ccd34c56c66 425 devicePrm.stepsToTake = devicePrm.currentPosition -\
nucleosam 0:0ccd34c56c66 426 targetPosition;
nucleosam 0:0ccd34c56c66 427 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
nucleosam 0:0ccd34c56c66 428 {
nucleosam 0:0ccd34c56c66 429 direction = BACKWARD;
nucleosam 0:0ccd34c56c66 430 }
nucleosam 0:0ccd34c56c66 431 else
nucleosam 0:0ccd34c56c66 432 {
nucleosam 0:0ccd34c56c66 433 direction = FORWARD;
nucleosam 0:0ccd34c56c66 434 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
nucleosam 0:0ccd34c56c66 435 devicePrm.stepsToTake;
nucleosam 0:0ccd34c56c66 436 }
nucleosam 0:0ccd34c56c66 437 }
nucleosam 0:0ccd34c56c66 438
nucleosam 0:0ccd34c56c66 439 if (devicePrm.stepsToTake != 0)
nucleosam 0:0ccd34c56c66 440 {
nucleosam 0:0ccd34c56c66 441 devicePrm.commandExecuted = MOVE_CMD;
nucleosam 0:0ccd34c56c66 442
nucleosam 0:0ccd34c56c66 443 /* Direction setup */
nucleosam 0:0ccd34c56c66 444 Stspin220_SetDirection(direction);
nucleosam 0:0ccd34c56c66 445
nucleosam 0:0ccd34c56c66 446 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake);
nucleosam 0:0ccd34c56c66 447
nucleosam 0:0ccd34c56c66 448 /* Motor activation */
nucleosam 0:0ccd34c56c66 449 Stspin220_StartMovement();
nucleosam 0:0ccd34c56c66 450 }
nucleosam 0:0ccd34c56c66 451 }
nucleosam 0:0ccd34c56c66 452
nucleosam 0:0ccd34c56c66 453 /******************************************************//**
nucleosam 0:0ccd34c56c66 454 * @brief Move the motor to the absolute position
nucleosam 0:0ccd34c56c66 455 * @param[in] direction FORWARD or BACKWARD
nucleosam 0:0ccd34c56c66 456 * @param[in] targetPosition 32 bit signed value position
nucleosam 0:0ccd34c56c66 457 * @retval None
nucleosam 0:0ccd34c56c66 458 * @note The position is at the resolution corresponding to the
nucleosam 0:0ccd34c56c66 459 * selected step mode.
nucleosam 0:0ccd34c56c66 460 * STEP_MODE_FULL : step
nucleosam 0:0ccd34c56c66 461 * STEP_MODE_HALF : 1/2 step
nucleosam 0:0ccd34c56c66 462 * STEP_MODE_1_4 : 1/4 step
nucleosam 0:0ccd34c56c66 463 * STEP_MODE_1_8 : 1/8 step
nucleosam 0:0ccd34c56c66 464 * STEP_MODE_1_16 : 1/16 step
nucleosam 0:0ccd34c56c66 465 * STEP_MODE_1_32 : 1/32 step
nucleosam 0:0ccd34c56c66 466 * STEP_MODE_1_64 : 1/64 step
nucleosam 0:0ccd34c56c66 467 * STEP_MODE_1_128 : 1/128 step
nucleosam 0:0ccd34c56c66 468 * STEP_MODE_1_256 : 1/256 step
nucleosam 0:0ccd34c56c66 469 **********************************************************/
nucleosam 0:0ccd34c56c66 470 void STSPIN220::Stspin220_GoToDir(motorDir_t direction, int32_t targetPosition)
nucleosam 0:0ccd34c56c66 471 {
nucleosam 0:0ccd34c56c66 472 /* Exit from standby if needed */
nucleosam 0:0ccd34c56c66 473 if (devicePrm.motionState == STANDBY)
nucleosam 0:0ccd34c56c66 474 {
nucleosam 0:0ccd34c56c66 475 Stspin220_ExitDeviceFromStandby();
nucleosam 0:0ccd34c56c66 476 }
nucleosam 0:0ccd34c56c66 477 /* Deactivate motor if needed */
nucleosam 0:0ccd34c56c66 478 else if (devicePrm.motionState != INACTIVE)
nucleosam 0:0ccd34c56c66 479 {
nucleosam 0:0ccd34c56c66 480 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 481 }
nucleosam 0:0ccd34c56c66 482
nucleosam 0:0ccd34c56c66 483 if (direction != BACKWARD)
nucleosam 0:0ccd34c56c66 484 {
nucleosam 0:0ccd34c56c66 485 if (targetPosition > devicePrm.currentPosition)
nucleosam 0:0ccd34c56c66 486 {
nucleosam 0:0ccd34c56c66 487 devicePrm.stepsToTake = targetPosition -\
nucleosam 0:0ccd34c56c66 488 devicePrm.currentPosition;
nucleosam 0:0ccd34c56c66 489 }
nucleosam 0:0ccd34c56c66 490 else
nucleosam 0:0ccd34c56c66 491 {
nucleosam 0:0ccd34c56c66 492 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
nucleosam 0:0ccd34c56c66 493 (targetPosition -\
nucleosam 0:0ccd34c56c66 494 devicePrm.currentPosition);
nucleosam 0:0ccd34c56c66 495 }
nucleosam 0:0ccd34c56c66 496 }
nucleosam 0:0ccd34c56c66 497 else
nucleosam 0:0ccd34c56c66 498 {
nucleosam 0:0ccd34c56c66 499 if (targetPosition > devicePrm.currentPosition)
nucleosam 0:0ccd34c56c66 500 {
nucleosam 0:0ccd34c56c66 501 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
nucleosam 0:0ccd34c56c66 502 (devicePrm.currentPosition -\
nucleosam 0:0ccd34c56c66 503 targetPosition);
nucleosam 0:0ccd34c56c66 504 }
nucleosam 0:0ccd34c56c66 505 else
nucleosam 0:0ccd34c56c66 506 {
nucleosam 0:0ccd34c56c66 507 devicePrm.stepsToTake = devicePrm.currentPosition -\
nucleosam 0:0ccd34c56c66 508 targetPosition;
nucleosam 0:0ccd34c56c66 509 }
nucleosam 0:0ccd34c56c66 510 }
nucleosam 0:0ccd34c56c66 511
nucleosam 0:0ccd34c56c66 512 if (devicePrm.stepsToTake != 0)
nucleosam 0:0ccd34c56c66 513 {
nucleosam 0:0ccd34c56c66 514 devicePrm.commandExecuted = MOVE_CMD;
nucleosam 0:0ccd34c56c66 515
nucleosam 0:0ccd34c56c66 516 /* Direction setup */
nucleosam 0:0ccd34c56c66 517 Stspin220_SetDirection(direction);
nucleosam 0:0ccd34c56c66 518
nucleosam 0:0ccd34c56c66 519 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake);
nucleosam 0:0ccd34c56c66 520
nucleosam 0:0ccd34c56c66 521 /* Motor activation */
nucleosam 0:0ccd34c56c66 522 Stspin220_StartMovement();
nucleosam 0:0ccd34c56c66 523 }
nucleosam 0:0ccd34c56c66 524 }
nucleosam 0:0ccd34c56c66 525
nucleosam 0:0ccd34c56c66 526 /******************************************************//**
nucleosam 0:0ccd34c56c66 527 * @brief Immediately stop the motor and disables the power bridges
nucleosam 0:0ccd34c56c66 528 * @retval None
nucleosam 0:0ccd34c56c66 529 **********************************************************/
nucleosam 0:0ccd34c56c66 530 void STSPIN220::Stspin220_HardHiZ(void)
nucleosam 0:0ccd34c56c66 531 {
nucleosam 0:0ccd34c56c66 532 /* Set inactive state */
nucleosam 0:0ccd34c56c66 533 devicePrm.motionState = INACTIVE;
nucleosam 0:0ccd34c56c66 534
nucleosam 0:0ccd34c56c66 535 /* Disable step clock */
nucleosam 0:0ccd34c56c66 536 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
nucleosam 0:0ccd34c56c66 537 {
nucleosam 0:0ccd34c56c66 538 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
nucleosam 0:0ccd34c56c66 539 }
nucleosam 0:0ccd34c56c66 540
nucleosam 0:0ccd34c56c66 541 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
nucleosam 0:0ccd34c56c66 542 /* after the last step clock rising edge triggering the last step */
nucleosam 0:0ccd34c56c66 543 Stspin220_Board_Delay(DISABLE_DELAY);
nucleosam 0:0ccd34c56c66 544
nucleosam 0:0ccd34c56c66 545 /* Set reference voltage to 0 */
nucleosam 0:0ccd34c56c66 546 Stspin220_SetTorque(CURRENT_TORQUE, 0);
nucleosam 0:0ccd34c56c66 547
nucleosam 0:0ccd34c56c66 548 /* Disable power bridges */
nucleosam 0:0ccd34c56c66 549 Stspin220_Board_Disable();
nucleosam 0:0ccd34c56c66 550
nucleosam 0:0ccd34c56c66 551 /* Comeback to nominal step mode */
nucleosam 0:0ccd34c56c66 552 if (devicePrm.stepModeLatched != devicePrm.stepMode)
nucleosam 0:0ccd34c56c66 553 {
nucleosam 0:0ccd34c56c66 554 Stspin220_Board_UnsetFullStep();
nucleosam 0:0ccd34c56c66 555 devicePrm.stepMode = devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 556 }
nucleosam 0:0ccd34c56c66 557
nucleosam 0:0ccd34c56c66 558 devicePrm.commandExecuted = NO_CMD;
nucleosam 0:0ccd34c56c66 559 devicePrm.stepsToTake = 0;
nucleosam 0:0ccd34c56c66 560 devicePrm.speed = 0;
nucleosam 0:0ccd34c56c66 561 }
nucleosam 0:0ccd34c56c66 562
nucleosam 0:0ccd34c56c66 563 /******************************************************//**
nucleosam 0:0ccd34c56c66 564 * @brief Immediatly stop the motor
nucleosam 0:0ccd34c56c66 565 * and either set holding torque when stop mode is HOLD_MODE,
nucleosam 0:0ccd34c56c66 566 * or call Stspin220_HardHiz function when stop mode is HIZ_MODE,
nucleosam 0:0ccd34c56c66 567 * or call Stspin220_PutDeviceInStandby function when stop mode is STANDBY_MODE
nucleosam 0:0ccd34c56c66 568 * @retval None
nucleosam 0:0ccd34c56c66 569 **********************************************************/
nucleosam 0:0ccd34c56c66 570 void STSPIN220::Stspin220_HardStop(void)
nucleosam 0:0ccd34c56c66 571 {
nucleosam 0:0ccd34c56c66 572 if (devicePrm.stopMode == HOLD_MODE)
nucleosam 0:0ccd34c56c66 573 {
nucleosam 0:0ccd34c56c66 574 /* Set inactive state */
nucleosam 0:0ccd34c56c66 575 devicePrm.motionState = INACTIVE;
nucleosam 0:0ccd34c56c66 576
nucleosam 0:0ccd34c56c66 577 /* Disable step clock */
nucleosam 0:0ccd34c56c66 578 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
nucleosam 0:0ccd34c56c66 579 {
nucleosam 0:0ccd34c56c66 580 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
nucleosam 0:0ccd34c56c66 581 }
nucleosam 0:0ccd34c56c66 582
nucleosam 0:0ccd34c56c66 583 /* Set holding torque */
nucleosam 0:0ccd34c56c66 584 Stspin220_ApplyTorque(HOLD_TORQUE);
nucleosam 0:0ccd34c56c66 585
nucleosam 0:0ccd34c56c66 586 /* Comeback to nominal step mode */
nucleosam 0:0ccd34c56c66 587 if (devicePrm.stepModeLatched != devicePrm.stepMode)
nucleosam 0:0ccd34c56c66 588 {
nucleosam 0:0ccd34c56c66 589 Stspin220_Board_UnsetFullStep();
nucleosam 0:0ccd34c56c66 590 devicePrm.stepMode = devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 591 }
nucleosam 0:0ccd34c56c66 592
nucleosam 0:0ccd34c56c66 593 devicePrm.commandExecuted = NO_CMD;
nucleosam 0:0ccd34c56c66 594 devicePrm.stepsToTake = 0;
nucleosam 0:0ccd34c56c66 595 devicePrm.speed = 0;
nucleosam 0:0ccd34c56c66 596 }
nucleosam 0:0ccd34c56c66 597 else if (devicePrm.stopMode == HIZ_MODE)
nucleosam 0:0ccd34c56c66 598 {
nucleosam 0:0ccd34c56c66 599 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 600 }
nucleosam 0:0ccd34c56c66 601 else if (devicePrm.stopMode == STANDBY_MODE)
nucleosam 0:0ccd34c56c66 602 {
nucleosam 0:0ccd34c56c66 603 Stspin220_PutDeviceInStandby();
nucleosam 0:0ccd34c56c66 604 }
nucleosam 0:0ccd34c56c66 605 }
nucleosam 0:0ccd34c56c66 606
nucleosam 0:0ccd34c56c66 607 /******************************************************//**
nucleosam 0:0ccd34c56c66 608 * @brief Moves the motor of the specified number of steps
nucleosam 0:0ccd34c56c66 609 * @param[in] direction FORWARD or BACKWARD
nucleosam 0:0ccd34c56c66 610 * @param[in] stepCount Number of steps to perform
nucleosam 0:0ccd34c56c66 611 * @retval None
nucleosam 0:0ccd34c56c66 612 **********************************************************/
nucleosam 0:0ccd34c56c66 613 void STSPIN220::Stspin220_Move(motorDir_t direction, uint32_t stepCount)
nucleosam 0:0ccd34c56c66 614 {
nucleosam 0:0ccd34c56c66 615 /* Exit from standby if needed */
nucleosam 0:0ccd34c56c66 616 if (devicePrm.motionState == STANDBY)
nucleosam 0:0ccd34c56c66 617 {
nucleosam 0:0ccd34c56c66 618 Stspin220_ExitDeviceFromStandby();
nucleosam 0:0ccd34c56c66 619 }
nucleosam 0:0ccd34c56c66 620 /* Deactivate motor if needed */
nucleosam 0:0ccd34c56c66 621 else if (devicePrm.motionState != INACTIVE)
nucleosam 0:0ccd34c56c66 622 {
nucleosam 0:0ccd34c56c66 623 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 624 }
nucleosam 0:0ccd34c56c66 625
nucleosam 0:0ccd34c56c66 626 if (stepCount != 0)
nucleosam 0:0ccd34c56c66 627 {
nucleosam 0:0ccd34c56c66 628 devicePrm.stepsToTake = stepCount;
nucleosam 0:0ccd34c56c66 629 devicePrm.commandExecuted = MOVE_CMD;
nucleosam 0:0ccd34c56c66 630
nucleosam 0:0ccd34c56c66 631 /* Direction setup */
nucleosam 0:0ccd34c56c66 632 Stspin220_SetDirection(direction);
nucleosam 0:0ccd34c56c66 633
nucleosam 0:0ccd34c56c66 634 Stspin220_ComputeSpeedProfile(stepCount);
nucleosam 0:0ccd34c56c66 635
nucleosam 0:0ccd34c56c66 636 /* Motor activation */
nucleosam 0:0ccd34c56c66 637 Stspin220_StartMovement();
nucleosam 0:0ccd34c56c66 638 }
nucleosam 0:0ccd34c56c66 639 }
nucleosam 0:0ccd34c56c66 640
nucleosam 0:0ccd34c56c66 641 /******************************************************//**
nucleosam 0:0ccd34c56c66 642 * @brief Put STSPIN220 device in standby (low power consumption)
nucleosam 0:0ccd34c56c66 643 * @retval None
nucleosam 0:0ccd34c56c66 644 **********************************************************/
nucleosam 0:0ccd34c56c66 645 void STSPIN220::Stspin220_PutDeviceInStandby(void)
nucleosam 0:0ccd34c56c66 646 {
nucleosam 0:0ccd34c56c66 647 /* Stop movement */
nucleosam 0:0ccd34c56c66 648 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 649
nucleosam 0:0ccd34c56c66 650 /* Enter standby */
nucleosam 0:0ccd34c56c66 651 Stspin220_Board_Reset();
nucleosam 0:0ccd34c56c66 652
nucleosam 0:0ccd34c56c66 653 devicePrm.motionState = STANDBY;
nucleosam 0:0ccd34c56c66 654 }
nucleosam 0:0ccd34c56c66 655
nucleosam 0:0ccd34c56c66 656 /******************************************************//**
nucleosam 0:0ccd34c56c66 657 * @brief Runs the motor. It will accelerate from the min
nucleosam 0:0ccd34c56c66 658 * speed up to the max speed by using the device acceleration.
nucleosam 0:0ccd34c56c66 659 * @param[in] direction FORWARD or BACKWARD
nucleosam 0:0ccd34c56c66 660 * @retval None
nucleosam 0:0ccd34c56c66 661 **********************************************************/
nucleosam 0:0ccd34c56c66 662 void STSPIN220::Stspin220_Run(motorDir_t direction)
nucleosam 0:0ccd34c56c66 663 {
nucleosam 0:0ccd34c56c66 664 /* Exit from standby if needed */
nucleosam 0:0ccd34c56c66 665 if (devicePrm.motionState == STANDBY)
nucleosam 0:0ccd34c56c66 666 {
nucleosam 0:0ccd34c56c66 667 Stspin220_ExitDeviceFromStandby();
nucleosam 0:0ccd34c56c66 668 }
nucleosam 0:0ccd34c56c66 669 /* Deactivate motor if needed */
nucleosam 0:0ccd34c56c66 670 else if (devicePrm.motionState != INACTIVE)
nucleosam 0:0ccd34c56c66 671 {
nucleosam 0:0ccd34c56c66 672 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 673 }
nucleosam 0:0ccd34c56c66 674
nucleosam 0:0ccd34c56c66 675 /* Direction setup */
nucleosam 0:0ccd34c56c66 676 Stspin220_SetDirection(direction);
nucleosam 0:0ccd34c56c66 677 devicePrm.commandExecuted = RUN_CMD;
nucleosam 0:0ccd34c56c66 678 /* Motor activation */
nucleosam 0:0ccd34c56c66 679 Stspin220_StartMovement();
nucleosam 0:0ccd34c56c66 680 }
nucleosam 0:0ccd34c56c66 681
nucleosam 0:0ccd34c56c66 682 /******************************************************//**
nucleosam 0:0ccd34c56c66 683 * @brief Changes the acceleration
nucleosam 0:0ccd34c56c66 684 * @param[in] newAcc New acceleration to apply in pps^2
nucleosam 0:0ccd34c56c66 685 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 686 * @note The command is not performed if the device is executing
nucleosam 0:0ccd34c56c66 687 * a MOVE or GOTO command (but it can be used during a RUN command)
nucleosam 0:0ccd34c56c66 688 **********************************************************/
nucleosam 0:0ccd34c56c66 689 bool STSPIN220::Stspin220_SetAcceleration(uint16_t newAcc)
nucleosam 0:0ccd34c56c66 690 {
nucleosam 0:0ccd34c56c66 691 bool cmdExecuted = false;
nucleosam 0:0ccd34c56c66 692 if ((newAcc != 0)&&
nucleosam 0:0ccd34c56c66 693 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
nucleosam 0:0ccd34c56c66 694 (devicePrm.commandExecuted == RUN_CMD)))
nucleosam 0:0ccd34c56c66 695 {
nucleosam 0:0ccd34c56c66 696 devicePrm.acceleration = newAcc;
nucleosam 0:0ccd34c56c66 697 cmdExecuted = true;
nucleosam 0:0ccd34c56c66 698 }
nucleosam 0:0ccd34c56c66 699 return cmdExecuted;
nucleosam 0:0ccd34c56c66 700 }
nucleosam 0:0ccd34c56c66 701
nucleosam 0:0ccd34c56c66 702 /******************************************************//**
nucleosam 0:0ccd34c56c66 703 * @brief Changes the deceleration
nucleosam 0:0ccd34c56c66 704 * @param[in] newDec New deceleration to apply in pps^2
nucleosam 0:0ccd34c56c66 705 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 706 * @note The command is not performed if the device is executing
nucleosam 0:0ccd34c56c66 707 * a MOVE or GOTO command (but it can be used during a RUN command)
nucleosam 0:0ccd34c56c66 708 **********************************************************/
nucleosam 0:0ccd34c56c66 709 bool STSPIN220::Stspin220_SetDeceleration(uint16_t newDec)
nucleosam 0:0ccd34c56c66 710 {
nucleosam 0:0ccd34c56c66 711 bool cmdExecuted = false;
nucleosam 0:0ccd34c56c66 712 if ((newDec != 0)&&
nucleosam 0:0ccd34c56c66 713 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
nucleosam 0:0ccd34c56c66 714 (devicePrm.commandExecuted == RUN_CMD)))
nucleosam 0:0ccd34c56c66 715 {
nucleosam 0:0ccd34c56c66 716 devicePrm.deceleration = newDec;
nucleosam 0:0ccd34c56c66 717 cmdExecuted = true;
nucleosam 0:0ccd34c56c66 718 }
nucleosam 0:0ccd34c56c66 719 return cmdExecuted;
nucleosam 0:0ccd34c56c66 720 }
nucleosam 0:0ccd34c56c66 721
nucleosam 0:0ccd34c56c66 722 /******************************************************//**
nucleosam 0:0ccd34c56c66 723 * @brief Specifies the direction
nucleosam 0:0ccd34c56c66 724 * @param[in] dir FORWARD or BACKWARD
nucleosam 0:0ccd34c56c66 725 * @note The direction change is applied if the device
nucleosam 0:0ccd34c56c66 726 * is in INACTIVE or STANDBY state or if the device is
nucleosam 0:0ccd34c56c66 727 * executing a run command
nucleosam 0:0ccd34c56c66 728 * @retval None
nucleosam 0:0ccd34c56c66 729 **********************************************************/
nucleosam 0:0ccd34c56c66 730 void STSPIN220::Stspin220_SetDirection(motorDir_t dir)
nucleosam 0:0ccd34c56c66 731 {
nucleosam 0:0ccd34c56c66 732 if ((devicePrm.motionState == INACTIVE)||\
nucleosam 0:0ccd34c56c66 733 (devicePrm.motionState == STANDBY))
nucleosam 0:0ccd34c56c66 734 {
nucleosam 0:0ccd34c56c66 735 devicePrm.direction = dir;
nucleosam 0:0ccd34c56c66 736 Stspin220_Board_SetDirectionGpio(dir);
nucleosam 0:0ccd34c56c66 737 }
nucleosam 0:0ccd34c56c66 738 else if ((devicePrm.commandExecuted&RUN_CMD)!=0)
nucleosam 0:0ccd34c56c66 739 {
nucleosam 0:0ccd34c56c66 740 devicePrm.commandExecuted=(deviceCommand_t)
nucleosam 0:0ccd34c56c66 741 (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted);
nucleosam 0:0ccd34c56c66 742 }
nucleosam 0:0ccd34c56c66 743 }
nucleosam 0:0ccd34c56c66 744
nucleosam 0:0ccd34c56c66 745 /******************************************************//**
nucleosam 0:0ccd34c56c66 746 * @brief Set current position to be the Home position
nucleosam 0:0ccd34c56c66 747 * (current position set to 0)
nucleosam 0:0ccd34c56c66 748 * @retval None
nucleosam 0:0ccd34c56c66 749 **********************************************************/
nucleosam 0:0ccd34c56c66 750 void STSPIN220::Stspin220_SetHome(void)
nucleosam 0:0ccd34c56c66 751 {
nucleosam 0:0ccd34c56c66 752 devicePrm.currentPosition = 0;
nucleosam 0:0ccd34c56c66 753 }
nucleosam 0:0ccd34c56c66 754
nucleosam 0:0ccd34c56c66 755 /******************************************************//**
nucleosam 0:0ccd34c56c66 756 * @brief Set current position to be the Mark position
nucleosam 0:0ccd34c56c66 757 * @retval None
nucleosam 0:0ccd34c56c66 758 **********************************************************/
nucleosam 0:0ccd34c56c66 759 void STSPIN220::Stspin220_SetMark(void)
nucleosam 0:0ccd34c56c66 760 {
nucleosam 0:0ccd34c56c66 761 devicePrm.markPosition = devicePrm.currentPosition;
nucleosam 0:0ccd34c56c66 762 }
nucleosam 0:0ccd34c56c66 763
nucleosam 0:0ccd34c56c66 764 /******************************************************//**
nucleosam 0:0ccd34c56c66 765 * @brief Changes the max speed
nucleosam 0:0ccd34c56c66 766 * @param[in] newMaxSpeed New max speed to apply in pps
nucleosam 0:0ccd34c56c66 767 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 768 * @note The command is not performed is the device is executing
nucleosam 0:0ccd34c56c66 769 * a MOVE or GOTO command (but it can be used during a RUN command).
nucleosam 0:0ccd34c56c66 770 **********************************************************/
nucleosam 0:0ccd34c56c66 771 bool STSPIN220::Stspin220_SetMaxSpeed(uint16_t newMaxSpeed)
nucleosam 0:0ccd34c56c66 772 {
nucleosam 0:0ccd34c56c66 773 bool cmdExecuted = false;
nucleosam 0:0ccd34c56c66 774 if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\
nucleosam 0:0ccd34c56c66 775 ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\
nucleosam 0:0ccd34c56c66 776 ((devicePrm.torqueBoostEnable != false)&&\
nucleosam 0:0ccd34c56c66 777 ((newMaxSpeed>>Stspin220_GetStepMode())<= STSPIN220_MAX_STCK_FREQ)))&&\
nucleosam 0:0ccd34c56c66 778 (devicePrm.minSpeed <= newMaxSpeed) &&\
nucleosam 0:0ccd34c56c66 779 (((devicePrm.motionState & INACTIVE) == INACTIVE)||\
nucleosam 0:0ccd34c56c66 780 (devicePrm.commandExecuted == RUN_CMD)))
nucleosam 0:0ccd34c56c66 781 {
nucleosam 0:0ccd34c56c66 782 devicePrm.maxSpeed = newMaxSpeed;
nucleosam 0:0ccd34c56c66 783 cmdExecuted = true;
nucleosam 0:0ccd34c56c66 784 }
nucleosam 0:0ccd34c56c66 785 return cmdExecuted;
nucleosam 0:0ccd34c56c66 786 }
nucleosam 0:0ccd34c56c66 787
nucleosam 0:0ccd34c56c66 788 /******************************************************//**
nucleosam 0:0ccd34c56c66 789 * @brief Changes the min speed
nucleosam 0:0ccd34c56c66 790 * @param[in] newMinSpeed New min speed to apply in pps
nucleosam 0:0ccd34c56c66 791 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 792 * @note The command is not performed is the device is executing
nucleosam 0:0ccd34c56c66 793 * a MOVE or GOTO command (but it can be used during a RUN command).
nucleosam 0:0ccd34c56c66 794 **********************************************************/
nucleosam 0:0ccd34c56c66 795 bool STSPIN220::Stspin220_SetMinSpeed(uint16_t newMinSpeed)
nucleosam 0:0ccd34c56c66 796 {
nucleosam 0:0ccd34c56c66 797 bool cmdExecuted = false;
nucleosam 0:0ccd34c56c66 798 if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&&
nucleosam 0:0ccd34c56c66 799 (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) &&
nucleosam 0:0ccd34c56c66 800 (newMinSpeed <= devicePrm.maxSpeed) &&
nucleosam 0:0ccd34c56c66 801 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
nucleosam 0:0ccd34c56c66 802 (devicePrm.commandExecuted == RUN_CMD)))
nucleosam 0:0ccd34c56c66 803 {
nucleosam 0:0ccd34c56c66 804 devicePrm.minSpeed = newMinSpeed;
nucleosam 0:0ccd34c56c66 805 cmdExecuted = true;
nucleosam 0:0ccd34c56c66 806 }
nucleosam 0:0ccd34c56c66 807 return cmdExecuted;
nucleosam 0:0ccd34c56c66 808 }
nucleosam 0:0ccd34c56c66 809
nucleosam 0:0ccd34c56c66 810 /******************************************************//**
nucleosam 0:0ccd34c56c66 811 * @brief Set the stepping mode
nucleosam 0:0ccd34c56c66 812 * @param[in] stepMode from full step to 1/256 microstep
nucleosam 0:0ccd34c56c66 813 * as specified in enum motorStepMode_t
nucleosam 0:0ccd34c56c66 814 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 815 **********************************************************/
nucleosam 0:0ccd34c56c66 816 bool STSPIN220::Stspin220_SetStepMode(motorStepMode_t stepMode)
nucleosam 0:0ccd34c56c66 817 {
nucleosam 0:0ccd34c56c66 818 /* Eventually deactivate motor */
nucleosam 0:0ccd34c56c66 819 if ((devicePrm.motionState != INACTIVE)&&\
nucleosam 0:0ccd34c56c66 820 (devicePrm.motionState != STANDBY))
nucleosam 0:0ccd34c56c66 821 {
nucleosam 0:0ccd34c56c66 822 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 823 }
nucleosam 0:0ccd34c56c66 824
nucleosam 0:0ccd34c56c66 825 /* Enter standby */
nucleosam 0:0ccd34c56c66 826 Stspin220_Board_Reset();
nucleosam 0:0ccd34c56c66 827
nucleosam 0:0ccd34c56c66 828 /* Reset the microstepping sequencer position */
nucleosam 0:0ccd34c56c66 829 devicePrm.sequencerPosition = 0;
nucleosam 0:0ccd34c56c66 830
nucleosam 0:0ccd34c56c66 831 /* Reset current and mark positions */
nucleosam 0:0ccd34c56c66 832 devicePrm.currentPosition = 0;
nucleosam 0:0ccd34c56c66 833 devicePrm.markPosition = 0;
nucleosam 0:0ccd34c56c66 834
nucleosam 0:0ccd34c56c66 835 /* Set the step mode */
nucleosam 0:0ccd34c56c66 836 return (Stspin220_SetStepModeWithoutReset(stepMode));
nucleosam 0:0ccd34c56c66 837 }
nucleosam 0:0ccd34c56c66 838
nucleosam 0:0ccd34c56c66 839 /******************************************************//**
nucleosam 0:0ccd34c56c66 840 * @brief Select the mode to stop the motor.
nucleosam 0:0ccd34c56c66 841 * @param[in] stopMode HOLD_MODE to let power bridge enabled
nucleosam 0:0ccd34c56c66 842 * @retval None
nucleosam 0:0ccd34c56c66 843 **********************************************************/
nucleosam 0:0ccd34c56c66 844 void STSPIN220::Stspin220_SetStopMode(motorStopMode_t stopMode)
nucleosam 0:0ccd34c56c66 845 {
nucleosam 0:0ccd34c56c66 846 devicePrm.stopMode = stopMode;
nucleosam 0:0ccd34c56c66 847 }
nucleosam 0:0ccd34c56c66 848
nucleosam 0:0ccd34c56c66 849 /******************************************************//**
nucleosam 0:0ccd34c56c66 850 * @brief Set the torque
nucleosam 0:0ccd34c56c66 851 * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t
nucleosam 0:0ccd34c56c66 852 * @param[in] torqueValue in % (from 0 to 100)
nucleosam 0:0ccd34c56c66 853 * @retval None
nucleosam 0:0ccd34c56c66 854 * @note
nucleosam 0:0ccd34c56c66 855 **********************************************************/
nucleosam 0:0ccd34c56c66 856 void STSPIN220::Stspin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue)
nucleosam 0:0ccd34c56c66 857 {
nucleosam 0:0ccd34c56c66 858 devicePrm.updateTorque = true;
nucleosam 0:0ccd34c56c66 859 if (torqueValue>100) torqueValue = 100;
nucleosam 0:0ccd34c56c66 860 switch(torqueMode)
nucleosam 0:0ccd34c56c66 861 {
nucleosam 0:0ccd34c56c66 862 case ACC_TORQUE:
nucleosam 0:0ccd34c56c66 863 devicePrm.accelTorque = torqueValue;
nucleosam 0:0ccd34c56c66 864 break;
nucleosam 0:0ccd34c56c66 865 case DEC_TORQUE:
nucleosam 0:0ccd34c56c66 866 devicePrm.decelTorque = torqueValue;
nucleosam 0:0ccd34c56c66 867 break;
nucleosam 0:0ccd34c56c66 868 case RUN_TORQUE:
nucleosam 0:0ccd34c56c66 869 devicePrm.runTorque = torqueValue;
nucleosam 0:0ccd34c56c66 870 break;
nucleosam 0:0ccd34c56c66 871 case HOLD_TORQUE:
nucleosam 0:0ccd34c56c66 872 devicePrm.holdTorque = torqueValue;
nucleosam 0:0ccd34c56c66 873 if (devicePrm.motionState != INACTIVE)
nucleosam 0:0ccd34c56c66 874 {
nucleosam 0:0ccd34c56c66 875 break;
nucleosam 0:0ccd34c56c66 876 }
nucleosam 0:0ccd34c56c66 877 case CURRENT_TORQUE:
nucleosam 0:0ccd34c56c66 878 devicePrm.currentTorque = torqueValue;
nucleosam 0:0ccd34c56c66 879 Stspin220_Board_PwmRefSetDutyCycle(torqueValue);
nucleosam 0:0ccd34c56c66 880 default:
nucleosam 0:0ccd34c56c66 881 devicePrm.updateTorque = false;
nucleosam 0:0ccd34c56c66 882 break; //ignore error
nucleosam 0:0ccd34c56c66 883 }
nucleosam 0:0ccd34c56c66 884 }
nucleosam 0:0ccd34c56c66 885
nucleosam 0:0ccd34c56c66 886 /******************************************************//**
nucleosam 0:0ccd34c56c66 887 * @brief Enable or disable the torque boost feature
nucleosam 0:0ccd34c56c66 888 * @param[in] enable true to enable torque boost, false to disable
nucleosam 0:0ccd34c56c66 889 * @retval None
nucleosam 0:0ccd34c56c66 890 **********************************************************/
nucleosam 0:0ccd34c56c66 891 void STSPIN220::Stspin220_SetTorqueBoostEnable(bool enable)
nucleosam 0:0ccd34c56c66 892 {
nucleosam 0:0ccd34c56c66 893 devicePrm.torqueBoostEnable = enable;
nucleosam 0:0ccd34c56c66 894 }
nucleosam 0:0ccd34c56c66 895
nucleosam 0:0ccd34c56c66 896 /******************************************************//**
nucleosam 0:0ccd34c56c66 897 * @brief Set the torque boost threshold
nucleosam 0:0ccd34c56c66 898 * @param[in] speedThreshold speed threshold above which the step mode is
nucleosam 0:0ccd34c56c66 899 * changed to full step
nucleosam 0:0ccd34c56c66 900 * @retval None
nucleosam 0:0ccd34c56c66 901 **********************************************************/
nucleosam 0:0ccd34c56c66 902 void STSPIN220::Stspin220_SetTorqueBoostThreshold(uint16_t speedThreshold)
nucleosam 0:0ccd34c56c66 903 {
nucleosam 0:0ccd34c56c66 904 devicePrm.torqueBoostSpeedThreshold = speedThreshold;
nucleosam 0:0ccd34c56c66 905 }
nucleosam 0:0ccd34c56c66 906
nucleosam 0:0ccd34c56c66 907 /******************************************************//**
nucleosam 0:0ccd34c56c66 908 * @brief Stops the motor by using the device deceleration
nucleosam 0:0ccd34c56c66 909 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 910 * @note The command is not performed if the device is in INACTIVE,
nucleosam 0:0ccd34c56c66 911 * STANDBYTOINACTIVE or STANDBY state.
nucleosam 0:0ccd34c56c66 912 **********************************************************/
nucleosam 0:0ccd34c56c66 913 bool STSPIN220::Stspin220_SoftStop(void)
nucleosam 0:0ccd34c56c66 914 {
nucleosam 0:0ccd34c56c66 915 bool cmdExecuted = false;
nucleosam 0:0ccd34c56c66 916 if ((devicePrm.motionState & INACTIVE) != INACTIVE)
nucleosam 0:0ccd34c56c66 917 {
nucleosam 0:0ccd34c56c66 918 devicePrm.commandExecuted=(deviceCommand_t)
nucleosam 0:0ccd34c56c66 919 (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted);
nucleosam 0:0ccd34c56c66 920 cmdExecuted = true;
nucleosam 0:0ccd34c56c66 921 }
nucleosam 0:0ccd34c56c66 922 return (cmdExecuted);
nucleosam 0:0ccd34c56c66 923 }
nucleosam 0:0ccd34c56c66 924
nucleosam 0:0ccd34c56c66 925 /******************************************************//**
nucleosam 0:0ccd34c56c66 926 * @brief Get the frequency of REF PWM
nucleosam 0:0ccd34c56c66 927 * @retval the frequency of REF PWM in Hz
nucleosam 0:0ccd34c56c66 928 * @note
nucleosam 0:0ccd34c56c66 929 **********************************************************/
nucleosam 0:0ccd34c56c66 930 uint32_t STSPIN220::Stspin220_VrefPwmGetFreq(void)
nucleosam 0:0ccd34c56c66 931 {
nucleosam 0:0ccd34c56c66 932 return devicePrm.refPwmFreq;
nucleosam 0:0ccd34c56c66 933 }
nucleosam 0:0ccd34c56c66 934
nucleosam 0:0ccd34c56c66 935 /******************************************************//**
nucleosam 0:0ccd34c56c66 936 * @brief Set the frequency of REF PWM
nucleosam 0:0ccd34c56c66 937 * @param[in] newFreq in Hz
nucleosam 0:0ccd34c56c66 938 * @retval None
nucleosam 0:0ccd34c56c66 939 * @note
nucleosam 0:0ccd34c56c66 940 **********************************************************/
nucleosam 0:0ccd34c56c66 941 void STSPIN220::Stspin220_VrefPwmSetFreq(uint32_t newFreq)
nucleosam 0:0ccd34c56c66 942 {
nucleosam 0:0ccd34c56c66 943 devicePrm.refPwmFreq = newFreq;
nucleosam 0:0ccd34c56c66 944 Stspin220_Board_PwmRefSetFreq(newFreq);
nucleosam 0:0ccd34c56c66 945 }
nucleosam 0:0ccd34c56c66 946
nucleosam 0:0ccd34c56c66 947 /******************************************************//**
nucleosam 0:0ccd34c56c66 948 * @brief Locks until the device state becomes Inactive
nucleosam 0:0ccd34c56c66 949 * @retval None
nucleosam 0:0ccd34c56c66 950 **********************************************************/
nucleosam 0:0ccd34c56c66 951 void STSPIN220::Stspin220_WaitWhileActive(void)
nucleosam 0:0ccd34c56c66 952 {
nucleosam 0:0ccd34c56c66 953 /* Wait while motor is running */
nucleosam 0:0ccd34c56c66 954 while (((Stspin220_GetDeviceState()&INACTIVE)!=INACTIVE)||\
nucleosam 0:0ccd34c56c66 955 (((Stspin220_GetDeviceState()&INACTIVE)==INACTIVE)&&(toggleOdd!=0)));
nucleosam 0:0ccd34c56c66 956 }
nucleosam 0:0ccd34c56c66 957
nucleosam 0:0ccd34c56c66 958 /* ------------------------------------------------------------------------- */
nucleosam 0:0ccd34c56c66 959 /* Internal functions ------------------------------------------------------ */
nucleosam 0:0ccd34c56c66 960 /* ------------------------------------------------------------------------- */
nucleosam 0:0ccd34c56c66 961 /******************************************************//**
nucleosam 0:0ccd34c56c66 962 * @brief Updates the current speed of the device
nucleosam 0:0ccd34c56c66 963 * @param[in] newSpeed in pps
nucleosam 0:0ccd34c56c66 964 * @retval None
nucleosam 0:0ccd34c56c66 965 **********************************************************/
nucleosam 0:0ccd34c56c66 966 void STSPIN220::Stspin220_ApplySpeed(uint16_t newSpeed)
nucleosam 0:0ccd34c56c66 967 {
nucleosam 0:0ccd34c56c66 968 if (devicePrm.torqueBoostEnable != false)
nucleosam 0:0ccd34c56c66 969 {
nucleosam 0:0ccd34c56c66 970 if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256)
nucleosam 0:0ccd34c56c66 971 {
nucleosam 0:0ccd34c56c66 972 Stspin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED);
nucleosam 0:0ccd34c56c66 973 }
nucleosam 0:0ccd34c56c66 974 if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL)
nucleosam 0:0ccd34c56c66 975 {
nucleosam 0:0ccd34c56c66 976 if (((newSpeed>>devicePrm.stepModeLatched)>\
nucleosam 0:0ccd34c56c66 977 devicePrm.torqueBoostSpeedThreshold)&&\
nucleosam 0:0ccd34c56c66 978 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\
nucleosam 0:0ccd34c56c66 979 ((devicePrm.stepsToTake-devicePrm.relativePos)>=\
nucleosam 0:0ccd34c56c66 980 (1<<devicePrm.stepModeLatched))))
nucleosam 0:0ccd34c56c66 981 {
nucleosam 0:0ccd34c56c66 982 if ((devicePrm.sequencerPosition & 0xFF) == 0X80)
nucleosam 0:0ccd34c56c66 983 {
nucleosam 0:0ccd34c56c66 984 Stspin220_Board_SetFullStep();
nucleosam 0:0ccd34c56c66 985 devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL;
nucleosam 0:0ccd34c56c66 986 devicePrm.accu >>= devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 987 newSpeed >>= devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 988 }
nucleosam 0:0ccd34c56c66 989 }
nucleosam 0:0ccd34c56c66 990 }
nucleosam 0:0ccd34c56c66 991 else if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\
nucleosam 0:0ccd34c56c66 992 (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\
nucleosam 0:0ccd34c56c66 993 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) == MOVE_CMD)&&\
nucleosam 0:0ccd34c56c66 994 ((devicePrm.stepsToTake-devicePrm.relativePos)<=\
nucleosam 0:0ccd34c56c66 995 (1<<devicePrm.stepModeLatched))))
nucleosam 0:0ccd34c56c66 996 {
nucleosam 0:0ccd34c56c66 997 Stspin220_Board_UnsetFullStep();
nucleosam 0:0ccd34c56c66 998 devicePrm.stepMode = devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 999 devicePrm.accu <<= devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 1000 newSpeed <<= devicePrm.stepModeLatched;
nucleosam 0:0ccd34c56c66 1001 }
nucleosam 0:0ccd34c56c66 1002 }
nucleosam 1:0c33082063bc 1003 else if (devicePrm.stepMode != devicePrm.stepModeLatched)
nucleosam 1:0c33082063bc 1004 {
nucleosam 1:0c33082063bc 1005 //torqueBoostEnable has just been disabled
nucleosam 1:0c33082063bc 1006 Stspin220_Board_UnsetFullStep();
nucleosam 1:0c33082063bc 1007 devicePrm.stepMode = devicePrm.stepModeLatched;
nucleosam 1:0c33082063bc 1008 devicePrm.accu <<= devicePrm.stepModeLatched;
nucleosam 1:0c33082063bc 1009 newSpeed <<= devicePrm.stepModeLatched;
nucleosam 1:0c33082063bc 1010 }
nucleosam 0:0ccd34c56c66 1011
nucleosam 0:0ccd34c56c66 1012 if (newSpeed < STSPIN220_MIN_STCK_FREQ)
nucleosam 0:0ccd34c56c66 1013 {
nucleosam 0:0ccd34c56c66 1014 newSpeed = STSPIN220_MIN_STCK_FREQ;
nucleosam 0:0ccd34c56c66 1015 }
nucleosam 0:0ccd34c56c66 1016 if (newSpeed > STSPIN220_MAX_STCK_FREQ)
nucleosam 0:0ccd34c56c66 1017 {
nucleosam 0:0ccd34c56c66 1018 newSpeed = STSPIN220_MAX_STCK_FREQ;
nucleosam 0:0ccd34c56c66 1019 }
nucleosam 0:0ccd34c56c66 1020
nucleosam 0:0ccd34c56c66 1021 devicePrm.speed = newSpeed;
nucleosam 0:0ccd34c56c66 1022 Stspin220_Board_TimStckSetFreq(newSpeed);
nucleosam 0:0ccd34c56c66 1023
nucleosam 0:0ccd34c56c66 1024 }
nucleosam 0:0ccd34c56c66 1025
nucleosam 0:0ccd34c56c66 1026 /******************************************************//**
nucleosam 0:0ccd34c56c66 1027 * @brief Computes the speed profile according to the number of steps to move
nucleosam 0:0ccd34c56c66 1028 * @param[in] nbSteps number of steps to perform
nucleosam 0:0ccd34c56c66 1029 * @retval None
nucleosam 0:0ccd34c56c66 1030 * @note Using the acceleration and deceleration of the device,
nucleosam 0:0ccd34c56c66 1031 * this function determines the duration in steps of the acceleration,
nucleosam 0:0ccd34c56c66 1032 * steady and deceleration phases.
nucleosam 0:0ccd34c56c66 1033 * If the total number of steps to perform is big enough, a trapezoidal move
nucleosam 0:0ccd34c56c66 1034 * is performed (i.e. there is a steady phase where the motor runs at the maximum
nucleosam 0:0ccd34c56c66 1035 * speed.
nucleosam 0:0ccd34c56c66 1036 * Else, a triangular move is performed (no steady phase: the maximum speed is never
nucleosam 0:0ccd34c56c66 1037 * reached.
nucleosam 0:0ccd34c56c66 1038 **********************************************************/
nucleosam 0:0ccd34c56c66 1039 void STSPIN220::Stspin220_ComputeSpeedProfile(uint32_t nbSteps)
nucleosam 0:0ccd34c56c66 1040 {
nucleosam 0:0ccd34c56c66 1041 uint32_t reqAccSteps;
nucleosam 0:0ccd34c56c66 1042 uint32_t reqDecSteps;
nucleosam 0:0ccd34c56c66 1043
nucleosam 0:0ccd34c56c66 1044 /* compute the number of steps to get the targeted speed */
nucleosam 0:0ccd34c56c66 1045 uint16_t minSpeed = devicePrm.minSpeed;
nucleosam 0:0ccd34c56c66 1046 reqAccSteps = (devicePrm.maxSpeed - minSpeed);
nucleosam 0:0ccd34c56c66 1047 reqAccSteps *= (devicePrm.maxSpeed + minSpeed);
nucleosam 0:0ccd34c56c66 1048 reqDecSteps = reqAccSteps;
nucleosam 0:0ccd34c56c66 1049 reqAccSteps /= (uint32_t)devicePrm.acceleration;
nucleosam 0:0ccd34c56c66 1050 reqAccSteps /= 2;
nucleosam 0:0ccd34c56c66 1051
nucleosam 0:0ccd34c56c66 1052 /* compute the number of steps to stop */
nucleosam 0:0ccd34c56c66 1053 reqDecSteps /= (uint32_t)devicePrm.deceleration;
nucleosam 0:0ccd34c56c66 1054 reqDecSteps /= 2;
nucleosam 0:0ccd34c56c66 1055
nucleosam 0:0ccd34c56c66 1056 if(( reqAccSteps + reqDecSteps ) > nbSteps)
nucleosam 0:0ccd34c56c66 1057 {
nucleosam 0:0ccd34c56c66 1058 /* Triangular move */
nucleosam 0:0ccd34c56c66 1059 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
nucleosam 0:0ccd34c56c66 1060 uint32_t dec = devicePrm.deceleration;
nucleosam 0:0ccd34c56c66 1061 uint32_t acc = devicePrm.acceleration;
nucleosam 0:0ccd34c56c66 1062
nucleosam 0:0ccd34c56c66 1063 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec);
nucleosam 0:0ccd34c56c66 1064 if (reqDecSteps > 1)
nucleosam 0:0ccd34c56c66 1065 {
nucleosam 0:0ccd34c56c66 1066 reqAccSteps = reqDecSteps - 1;
nucleosam 0:0ccd34c56c66 1067 if(reqAccSteps == 0)
nucleosam 0:0ccd34c56c66 1068 {
nucleosam 0:0ccd34c56c66 1069 reqAccSteps = 1;
nucleosam 0:0ccd34c56c66 1070 }
nucleosam 0:0ccd34c56c66 1071 }
nucleosam 0:0ccd34c56c66 1072 else
nucleosam 0:0ccd34c56c66 1073 {
nucleosam 0:0ccd34c56c66 1074 reqAccSteps = 0;
nucleosam 0:0ccd34c56c66 1075 }
nucleosam 0:0ccd34c56c66 1076 devicePrm.endAccPos = reqAccSteps;
nucleosam 0:0ccd34c56c66 1077 devicePrm.startDecPos = reqDecSteps;
nucleosam 0:0ccd34c56c66 1078 }
nucleosam 0:0ccd34c56c66 1079 else
nucleosam 0:0ccd34c56c66 1080 {
nucleosam 0:0ccd34c56c66 1081 /* Trapezoidal move */
nucleosam 0:0ccd34c56c66 1082 /* accelerating phase to endAccPos */
nucleosam 0:0ccd34c56c66 1083 /* steady phase from endAccPos to startDecPos */
nucleosam 0:0ccd34c56c66 1084 /* decelerating from startDecPos to stepsToTake*/
nucleosam 0:0ccd34c56c66 1085 devicePrm.endAccPos = reqAccSteps;
nucleosam 0:0ccd34c56c66 1086 devicePrm.startDecPos = nbSteps - reqDecSteps - 1;
nucleosam 0:0ccd34c56c66 1087 }
nucleosam 0:0ccd34c56c66 1088 }
nucleosam 0:0ccd34c56c66 1089
nucleosam 0:0ccd34c56c66 1090 /******************************************************//**
nucleosam 0:0ccd34c56c66 1091 * @brief Set the parameters of the device whose values are not defined in
nucleosam 0:0ccd34c56c66 1092 * stspin220_target_config.h
nucleosam 0:0ccd34c56c66 1093 * @retval None
nucleosam 0:0ccd34c56c66 1094 **********************************************************/
nucleosam 0:0ccd34c56c66 1095 void STSPIN220::Stspin220_SetDeviceParamsOtherValues(void)
nucleosam 0:0ccd34c56c66 1096 {
nucleosam 0:0ccd34c56c66 1097 uint16_t tmp;
nucleosam 0:0ccd34c56c66 1098
nucleosam 0:0ccd34c56c66 1099 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1100 devicePrm.currentPosition = 0;
nucleosam 0:0ccd34c56c66 1101 devicePrm.sequencerPosition = 0;
nucleosam 0:0ccd34c56c66 1102 devicePrm.endAccPos = 0;
nucleosam 0:0ccd34c56c66 1103 devicePrm.relativePos = 0;
nucleosam 0:0ccd34c56c66 1104 devicePrm.startDecPos = 0;
nucleosam 0:0ccd34c56c66 1105 devicePrm.stepsToTake = 0;
nucleosam 0:0ccd34c56c66 1106 devicePrm.updateTorque = false;
nucleosam 0:0ccd34c56c66 1107 devicePrm.speed = 0;
nucleosam 0:0ccd34c56c66 1108 devicePrm.commandExecuted = NO_CMD;
nucleosam 0:0ccd34c56c66 1109 devicePrm.direction = FORWARD;
nucleosam 0:0ccd34c56c66 1110 tmp = devicePrm.minSpeed;
nucleosam 0:0ccd34c56c66 1111 if (((devicePrm.torqueBoostEnable != false)&&\
nucleosam 0:0ccd34c56c66 1112 (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\
nucleosam 0:0ccd34c56c66 1113 (tmp>devicePrm.maxSpeed))
nucleosam 0:0ccd34c56c66 1114 {
nucleosam 0:0ccd34c56c66 1115 Stspin220_ErrorHandler(STSPIN220_ERROR_INIT);
nucleosam 0:0ccd34c56c66 1116 }
nucleosam 0:0ccd34c56c66 1117 }
nucleosam 0:0ccd34c56c66 1118
nucleosam 0:0ccd34c56c66 1119 /******************************************************//**
nucleosam 0:0ccd34c56c66 1120 * @brief Set the parameters of the device to values of the structure pointed
nucleosam 0:0ccd34c56c66 1121 * by pInitDevicePrm. Set GPIO according to these values.
nucleosam 0:0ccd34c56c66 1122 * @param pInitDevicePrm pointer onto the structure containing values to
nucleosam 0:0ccd34c56c66 1123 * initialize the device parameters.
nucleosam 0:0ccd34c56c66 1124 * @retval None
nucleosam 0:0ccd34c56c66 1125 **********************************************************/
nucleosam 0:0ccd34c56c66 1126 void STSPIN220::Stspin220_SetDeviceParamsToGivenValues(Stspin220_Init_t* pInitDevicePrm)
nucleosam 0:0ccd34c56c66 1127 {
nucleosam 0:0ccd34c56c66 1128 devicePrm.motionState = STANDBY;
nucleosam 0:0ccd34c56c66 1129
nucleosam 0:0ccd34c56c66 1130 if (Stspin220_SetAcceleration(pInitDevicePrm->acceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
nucleosam 0:0ccd34c56c66 1131 if (Stspin220_SetDeceleration(pInitDevicePrm->deceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
nucleosam 0:0ccd34c56c66 1132 if (Stspin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
nucleosam 0:0ccd34c56c66 1133 if (Stspin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
nucleosam 0:0ccd34c56c66 1134
nucleosam 0:0ccd34c56c66 1135 Stspin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq);
nucleosam 0:0ccd34c56c66 1136 Stspin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque);
nucleosam 0:0ccd34c56c66 1137 Stspin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque);
nucleosam 0:0ccd34c56c66 1138 Stspin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque);
nucleosam 0:0ccd34c56c66 1139 Stspin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque);
nucleosam 0:0ccd34c56c66 1140 devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
nucleosam 0:0ccd34c56c66 1141 devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
nucleosam 0:0ccd34c56c66 1142 Stspin220_SetStopMode(pInitDevicePrm->stopMode);
nucleosam 0:0ccd34c56c66 1143
nucleosam 0:0ccd34c56c66 1144 Stspin220_SetDeviceParamsOtherValues();
nucleosam 0:0ccd34c56c66 1145
nucleosam 0:0ccd34c56c66 1146 /* Set predefined step mode */
nucleosam 0:0ccd34c56c66 1147 /* Standby-reset deactivation included to latch the MODEX inputs */
nucleosam 0:0ccd34c56c66 1148 Stspin220_SetStepMode(pInitDevicePrm->stepMode);
nucleosam 0:0ccd34c56c66 1149 }
nucleosam 0:0ccd34c56c66 1150
nucleosam 0:0ccd34c56c66 1151 /******************************************************//**
nucleosam 0:0ccd34c56c66 1152 * @brief Sets the parameters of the device to predefined values
nucleosam 0:0ccd34c56c66 1153 * from stspin220_target_config.h
nucleosam 0:0ccd34c56c66 1154 * @retval None
nucleosam 0:0ccd34c56c66 1155 **********************************************************/
nucleosam 0:0ccd34c56c66 1156 void STSPIN220::Stspin220_SetDeviceParamsToPredefinedValues(void)
nucleosam 0:0ccd34c56c66 1157 {
nucleosam 0:0ccd34c56c66 1158 devicePrm.motionState = STANDBY;
nucleosam 0:0ccd34c56c66 1159
nucleosam 0:0ccd34c56c66 1160 if (Stspin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
nucleosam 0:0ccd34c56c66 1161 if (Stspin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
nucleosam 0:0ccd34c56c66 1162 if (Stspin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
nucleosam 0:0ccd34c56c66 1163 if (Stspin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
nucleosam 0:0ccd34c56c66 1164
nucleosam 0:0ccd34c56c66 1165 Stspin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY);
nucleosam 0:0ccd34c56c66 1166 Stspin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE);
nucleosam 0:0ccd34c56c66 1167 Stspin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE);
nucleosam 0:0ccd34c56c66 1168 Stspin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE);
nucleosam 0:0ccd34c56c66 1169 Stspin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE);
nucleosam 0:0ccd34c56c66 1170 devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN;
nucleosam 0:0ccd34c56c66 1171 devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH;
nucleosam 0:0ccd34c56c66 1172 Stspin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP);
nucleosam 0:0ccd34c56c66 1173
nucleosam 0:0ccd34c56c66 1174 Stspin220_SetDeviceParamsOtherValues();
nucleosam 0:0ccd34c56c66 1175
nucleosam 0:0ccd34c56c66 1176 /* Set predefined step mode */
nucleosam 0:0ccd34c56c66 1177 /* Standby-reset deactivation included to latch the MODEX inputs */
nucleosam 0:0ccd34c56c66 1178 Stspin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE);
nucleosam 0:0ccd34c56c66 1179 }
nucleosam 0:0ccd34c56c66 1180
nucleosam 0:0ccd34c56c66 1181 /******************************************************//**
nucleosam 0:0ccd34c56c66 1182 * @brief Set the stepping mode without reset
nucleosam 0:0ccd34c56c66 1183 * @param[in] stepMode from full step to 1/256 microstep
nucleosam 0:0ccd34c56c66 1184 * as specified in enum motorStepMode_t
nucleosam 0:0ccd34c56c66 1185 * @retval true if the command is successfully executed, else false
nucleosam 0:0ccd34c56c66 1186 **********************************************************/
nucleosam 0:0ccd34c56c66 1187 bool STSPIN220::Stspin220_SetStepModeWithoutReset(motorStepMode_t stepMode)
nucleosam 0:0ccd34c56c66 1188 {
nucleosam 0:0ccd34c56c66 1189 /* Store step mode */
nucleosam 0:0ccd34c56c66 1190 devicePrm.stepMode = stepMode;
nucleosam 0:0ccd34c56c66 1191 devicePrm.stepModeLatched = stepMode;
nucleosam 0:0ccd34c56c66 1192
nucleosam 0:0ccd34c56c66 1193 /* Set the mode pins to the levels corresponding to the selected step mode */
nucleosam 0:0ccd34c56c66 1194 switch (stepMode)
nucleosam 0:0ccd34c56c66 1195 {
nucleosam 0:0ccd34c56c66 1196 case STEP_MODE_FULL:
nucleosam 0:0ccd34c56c66 1197 Stspin220_Board_SetFullStep();
nucleosam 0:0ccd34c56c66 1198 break;
nucleosam 0:0ccd34c56c66 1199 case STEP_MODE_HALF:
nucleosam 0:0ccd34c56c66 1200 Stspin220_Board_SetModePins(1, 0, 1, 0);
nucleosam 0:0ccd34c56c66 1201 break;
nucleosam 0:0ccd34c56c66 1202 case STEP_MODE_1_4:
nucleosam 0:0ccd34c56c66 1203 Stspin220_Board_SetModePins(0, 1, 0, 1);
nucleosam 0:0ccd34c56c66 1204 break;
nucleosam 0:0ccd34c56c66 1205 case STEP_MODE_1_8:
nucleosam 0:0ccd34c56c66 1206 Stspin220_Board_SetModePins(1, 1, 1, 0);
nucleosam 0:0ccd34c56c66 1207 break;
nucleosam 0:0ccd34c56c66 1208 case STEP_MODE_1_16:
nucleosam 0:0ccd34c56c66 1209 Stspin220_Board_SetModePins(1, 1, 1, 1);
nucleosam 0:0ccd34c56c66 1210 break;
nucleosam 0:0ccd34c56c66 1211 case STEP_MODE_1_32:
nucleosam 0:0ccd34c56c66 1212 Stspin220_Board_SetModePins(0, 0, 0, 1);
nucleosam 0:0ccd34c56c66 1213 break;
nucleosam 0:0ccd34c56c66 1214 case STEP_MODE_1_64:
nucleosam 0:0ccd34c56c66 1215 Stspin220_Board_SetModePins(1, 1, 0, 1);
nucleosam 0:0ccd34c56c66 1216 break;
nucleosam 0:0ccd34c56c66 1217 case STEP_MODE_1_128:
nucleosam 0:0ccd34c56c66 1218 Stspin220_Board_SetModePins(0, 0, 1, 0);
nucleosam 0:0ccd34c56c66 1219 break;
nucleosam 0:0ccd34c56c66 1220 case STEP_MODE_1_256:
nucleosam 0:0ccd34c56c66 1221 Stspin220_Board_SetModePins(1, 1, 0, 0);
nucleosam 0:0ccd34c56c66 1222 break;
nucleosam 0:0ccd34c56c66 1223 default:
nucleosam 0:0ccd34c56c66 1224 return false;
nucleosam 0:0ccd34c56c66 1225 }
nucleosam 0:0ccd34c56c66 1226
nucleosam 0:0ccd34c56c66 1227 /* Wait */
nucleosam 0:0ccd34c56c66 1228 Stspin220_Board_Delay(SELECT_STEP_MODE_DELAY);
nucleosam 0:0ccd34c56c66 1229
nucleosam 0:0ccd34c56c66 1230 /* Exit standby, selected step mode is latched */
nucleosam 0:0ccd34c56c66 1231 Stspin220_Board_ReleaseReset();
nucleosam 0:0ccd34c56c66 1232
nucleosam 0:0ccd34c56c66 1233 /* Let a delay after reset release and step mode latching*/
nucleosam 0:0ccd34c56c66 1234 Stspin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
nucleosam 0:0ccd34c56c66 1235
nucleosam 0:0ccd34c56c66 1236 /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */
nucleosam 0:0ccd34c56c66 1237 /* after the device quit the standby condition */
nucleosam 0:0ccd34c56c66 1238 if (stepMode!=(motorStepMode_t)STEP_MODE_FULL)
nucleosam 0:0ccd34c56c66 1239 {
nucleosam 0:0ccd34c56c66 1240 Stspin220_Board_UnsetFullStep();
nucleosam 0:0ccd34c56c66 1241 }
nucleosam 0:0ccd34c56c66 1242
nucleosam 0:0ccd34c56c66 1243 return true;
nucleosam 0:0ccd34c56c66 1244 }
nucleosam 0:0ccd34c56c66 1245
nucleosam 0:0ccd34c56c66 1246 /******************************************************//**
nucleosam 0:0ccd34c56c66 1247 * @brief Initialises the bridge parameters to start the movement
nucleosam 0:0ccd34c56c66 1248 * and enable the power bridge
nucleosam 0:0ccd34c56c66 1249 * @retval None
nucleosam 0:0ccd34c56c66 1250 **********************************************************/
nucleosam 0:0ccd34c56c66 1251 void STSPIN220::Stspin220_StartMovement(void)
nucleosam 0:0ccd34c56c66 1252 {
nucleosam 0:0ccd34c56c66 1253 /* Enable STSPIN220 powerstage */
nucleosam 0:0ccd34c56c66 1254 Stspin220_Enable();
nucleosam 0:0ccd34c56c66 1255 toggleOdd = 0;
nucleosam 0:0ccd34c56c66 1256 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1257 devicePrm.relativePos = 0;
nucleosam 0:0ccd34c56c66 1258 Stspin220_Board_TimStckInit(true);
nucleosam 0:0ccd34c56c66 1259 if ((devicePrm.endAccPos == 0)&&\
nucleosam 0:0ccd34c56c66 1260 (devicePrm.commandExecuted != RUN_CMD))
nucleosam 0:0ccd34c56c66 1261 {
nucleosam 0:0ccd34c56c66 1262 devicePrm.motionState = DECELERATING;
nucleosam 0:0ccd34c56c66 1263 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE);
nucleosam 0:0ccd34c56c66 1264 }
nucleosam 0:0ccd34c56c66 1265 else
nucleosam 0:0ccd34c56c66 1266 {
nucleosam 0:0ccd34c56c66 1267 devicePrm.motionState = ACCELERATING;
nucleosam 0:0ccd34c56c66 1268 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE);
nucleosam 0:0ccd34c56c66 1269 }
nucleosam 0:0ccd34c56c66 1270 /* Program the step clock */
nucleosam 0:0ccd34c56c66 1271 Stspin220_ApplySpeed(devicePrm.minSpeed);
nucleosam 0:0ccd34c56c66 1272 }
nucleosam 0:0ccd34c56c66 1273
nucleosam 0:0ccd34c56c66 1274 /******************************************************//**
nucleosam 0:0ccd34c56c66 1275 * @brief Handles the device state machine at each pulse
nucleosam 0:0ccd34c56c66 1276 * @retval None
nucleosam 0:0ccd34c56c66 1277 * @note Must only be called by the timer ISR
nucleosam 0:0ccd34c56c66 1278 **********************************************************/
nucleosam 0:0ccd34c56c66 1279 void STSPIN220::Stspin220_StepClockHandler(void)
nucleosam 0:0ccd34c56c66 1280 {
nucleosam 0:0ccd34c56c66 1281 uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode;
nucleosam 0:0ccd34c56c66 1282 uint16_t tmp;
nucleosam 0:0ccd34c56c66 1283 Stspin220_Board_Monitor_Set();
nucleosam 0:0ccd34c56c66 1284 if (devicePrm.motionState == STANDBYTOINACTIVE)
nucleosam 0:0ccd34c56c66 1285 {
nucleosam 0:0ccd34c56c66 1286 if (toggleOdd != 0)
nucleosam 0:0ccd34c56c66 1287 {
nucleosam 0:0ccd34c56c66 1288 Stspin220_Board_StckMode3_Reset();
nucleosam 0:0ccd34c56c66 1289 toggleOdd = 0;
nucleosam 0:0ccd34c56c66 1290 if (devicePrm.sequencerPosition == 0)
nucleosam 0:0ccd34c56c66 1291 {
nucleosam 0:0ccd34c56c66 1292 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
nucleosam 0:0ccd34c56c66 1293 {
nucleosam 0:0ccd34c56c66 1294 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
nucleosam 0:0ccd34c56c66 1295 }
nucleosam 0:0ccd34c56c66 1296 return;
nucleosam 0:0ccd34c56c66 1297 }
nucleosam 0:0ccd34c56c66 1298 }
nucleosam 0:0ccd34c56c66 1299 else
nucleosam 0:0ccd34c56c66 1300 {
nucleosam 0:0ccd34c56c66 1301 Stspin220_Board_StckMode3_Set();
nucleosam 0:0ccd34c56c66 1302 toggleOdd = 1;
nucleosam 0:0ccd34c56c66 1303 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
nucleosam 0:0ccd34c56c66 1304 devicePrm.sequencerPosition -= tmp;
nucleosam 0:0ccd34c56c66 1305 }
nucleosam 0:0ccd34c56c66 1306 Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ);
nucleosam 0:0ccd34c56c66 1307 return;
nucleosam 0:0ccd34c56c66 1308 }
nucleosam 0:0ccd34c56c66 1309
nucleosam 0:0ccd34c56c66 1310 if (toggleOdd == 0)
nucleosam 0:0ccd34c56c66 1311 {
nucleosam 0:0ccd34c56c66 1312 Stspin220_Board_StckMode3_Set();
nucleosam 0:0ccd34c56c66 1313 toggleOdd = 1;
nucleosam 0:0ccd34c56c66 1314 }
nucleosam 0:0ccd34c56c66 1315 else
nucleosam 0:0ccd34c56c66 1316 {
nucleosam 0:0ccd34c56c66 1317 Stspin220_Board_StckMode3_Reset();
nucleosam 0:0ccd34c56c66 1318 toggleOdd = 0;
nucleosam 0:0ccd34c56c66 1319 /* Incrementation of the relative position */
nucleosam 0:0ccd34c56c66 1320 devicePrm.relativePos += (1 << stepModeShift);
nucleosam 0:0ccd34c56c66 1321
nucleosam 0:0ccd34c56c66 1322 /* Incrementation of the current position */
nucleosam 0:0ccd34c56c66 1323 if (devicePrm.direction != BACKWARD)
nucleosam 0:0ccd34c56c66 1324 {
nucleosam 0:0ccd34c56c66 1325 devicePrm.currentPosition += (1 << stepModeShift);
nucleosam 0:0ccd34c56c66 1326 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
nucleosam 0:0ccd34c56c66 1327 devicePrm.sequencerPosition += tmp;
nucleosam 0:0ccd34c56c66 1328 if (devicePrm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
nucleosam 0:0ccd34c56c66 1329 {
nucleosam 0:0ccd34c56c66 1330 devicePrm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
nucleosam 0:0ccd34c56c66 1331 }
nucleosam 0:0ccd34c56c66 1332 }
nucleosam 0:0ccd34c56c66 1333 else
nucleosam 0:0ccd34c56c66 1334 {
nucleosam 0:0ccd34c56c66 1335 devicePrm.currentPosition -= (1 << stepModeShift);
nucleosam 0:0ccd34c56c66 1336 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
nucleosam 0:0ccd34c56c66 1337 devicePrm.sequencerPosition -= tmp;
nucleosam 0:0ccd34c56c66 1338 if (devicePrm.sequencerPosition < 0)
nucleosam 0:0ccd34c56c66 1339 {
nucleosam 0:0ccd34c56c66 1340 devicePrm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
nucleosam 0:0ccd34c56c66 1341 }
nucleosam 0:0ccd34c56c66 1342 }
nucleosam 0:0ccd34c56c66 1343
nucleosam 0:0ccd34c56c66 1344 switch (devicePrm.motionState)
nucleosam 0:0ccd34c56c66 1345 {
nucleosam 0:0ccd34c56c66 1346 case ACCELERATING:
nucleosam 0:0ccd34c56c66 1347 {
nucleosam 0:0ccd34c56c66 1348 uint32_t relPos = devicePrm.relativePos;
nucleosam 0:0ccd34c56c66 1349 uint32_t endAccPos = devicePrm.endAccPos;
nucleosam 0:0ccd34c56c66 1350 uint16_t speed = devicePrm.speed;
nucleosam 0:0ccd34c56c66 1351 uint32_t acc = ((uint32_t)devicePrm.acceleration << 16)>>stepModeShift;
nucleosam 0:0ccd34c56c66 1352
nucleosam 0:0ccd34c56c66 1353 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
nucleosam 0:0ccd34c56c66 1354 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos)))
nucleosam 0:0ccd34c56c66 1355 {
nucleosam 0:0ccd34c56c66 1356 devicePrm.motionState = DECELERATING;
nucleosam 0:0ccd34c56c66 1357 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1358 /* Apply decelerating torque */
nucleosam 0:0ccd34c56c66 1359 Stspin220_ApplyTorque(DEC_TORQUE);
nucleosam 0:0ccd34c56c66 1360 }
nucleosam 0:0ccd34c56c66 1361 else if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\
nucleosam 0:0ccd34c56c66 1362 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
nucleosam 0:0ccd34c56c66 1363 {
nucleosam 0:0ccd34c56c66 1364 devicePrm.motionState = STEADY;
nucleosam 0:0ccd34c56c66 1365 /* Apply running torque */
nucleosam 0:0ccd34c56c66 1366 Stspin220_ApplyTorque(RUN_TORQUE);
nucleosam 0:0ccd34c56c66 1367 }
nucleosam 0:0ccd34c56c66 1368 else
nucleosam 0:0ccd34c56c66 1369 {
nucleosam 0:0ccd34c56c66 1370 bool speedUpdated = false;
nucleosam 0:0ccd34c56c66 1371 /* Go on accelerating */
nucleosam 0:0ccd34c56c66 1372 if (speed==0) speed =1;
nucleosam 0:0ccd34c56c66 1373 devicePrm.accu += acc / speed;
nucleosam 0:0ccd34c56c66 1374 while (devicePrm.accu>=(0X10000L))
nucleosam 0:0ccd34c56c66 1375 {
nucleosam 0:0ccd34c56c66 1376 devicePrm.accu -= (0X10000L);
nucleosam 0:0ccd34c56c66 1377 speed +=1;
nucleosam 0:0ccd34c56c66 1378 speedUpdated = true;
nucleosam 0:0ccd34c56c66 1379 }
nucleosam 0:0ccd34c56c66 1380
nucleosam 0:0ccd34c56c66 1381 if (speedUpdated)
nucleosam 0:0ccd34c56c66 1382 {
nucleosam 0:0ccd34c56c66 1383 if (speed>(devicePrm.maxSpeed>>stepModeShift))
nucleosam 0:0ccd34c56c66 1384 {
nucleosam 0:0ccd34c56c66 1385 speed = devicePrm.maxSpeed>>stepModeShift;
nucleosam 0:0ccd34c56c66 1386 }
nucleosam 0:0ccd34c56c66 1387 devicePrm.speed = speed;
nucleosam 0:0ccd34c56c66 1388 }
nucleosam 0:0ccd34c56c66 1389
nucleosam 0:0ccd34c56c66 1390 if (devicePrm.updateTorque!=false)
nucleosam 0:0ccd34c56c66 1391 {
nucleosam 0:0ccd34c56c66 1392 /* Apply accelerating torque */
nucleosam 0:0ccd34c56c66 1393 Stspin220_ApplyTorque(ACC_TORQUE);
nucleosam 0:0ccd34c56c66 1394 }
nucleosam 0:0ccd34c56c66 1395 }
nucleosam 0:0ccd34c56c66 1396 break;
nucleosam 0:0ccd34c56c66 1397 }
nucleosam 0:0ccd34c56c66 1398 case STEADY:
nucleosam 0:0ccd34c56c66 1399 {
nucleosam 0:0ccd34c56c66 1400 uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift;
nucleosam 0:0ccd34c56c66 1401 uint32_t relativePos = devicePrm.relativePos;
nucleosam 0:0ccd34c56c66 1402 if (devicePrm.updateTorque!=false)
nucleosam 0:0ccd34c56c66 1403 {
nucleosam 0:0ccd34c56c66 1404 /* Apply accelerating torque */
nucleosam 0:0ccd34c56c66 1405 Stspin220_ApplyTorque(RUN_TORQUE);
nucleosam 0:0ccd34c56c66 1406 }
nucleosam 0:0ccd34c56c66 1407 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
nucleosam 0:0ccd34c56c66 1408 ((devicePrm.commandExecuted==MOVE_CMD)&&\
nucleosam 0:0ccd34c56c66 1409 (relativePos>=(devicePrm.startDecPos)))||\
nucleosam 0:0ccd34c56c66 1410 ((devicePrm.commandExecuted==RUN_CMD)&&\
nucleosam 0:0ccd34c56c66 1411 (devicePrm.speed>maxSpeed)))
nucleosam 0:0ccd34c56c66 1412 {
nucleosam 0:0ccd34c56c66 1413 devicePrm.motionState = DECELERATING;
nucleosam 0:0ccd34c56c66 1414 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1415 /* Apply decelerating torque */
nucleosam 0:0ccd34c56c66 1416 Stspin220_ApplyTorque(DEC_TORQUE);
nucleosam 0:0ccd34c56c66 1417 }
nucleosam 0:0ccd34c56c66 1418 else if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed))
nucleosam 0:0ccd34c56c66 1419 {
nucleosam 0:0ccd34c56c66 1420 devicePrm.motionState = ACCELERATING;
nucleosam 0:0ccd34c56c66 1421 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1422 /* Apply accelerating torque */
nucleosam 0:0ccd34c56c66 1423 Stspin220_ApplyTorque(ACC_TORQUE);
nucleosam 0:0ccd34c56c66 1424 }
nucleosam 0:0ccd34c56c66 1425 break;
nucleosam 0:0ccd34c56c66 1426 }
nucleosam 0:0ccd34c56c66 1427 case DECELERATING:
nucleosam 0:0ccd34c56c66 1428 {
nucleosam 0:0ccd34c56c66 1429 uint32_t relativePos = devicePrm.relativePos;
nucleosam 0:0ccd34c56c66 1430 uint16_t speed = devicePrm.speed;
nucleosam 0:0ccd34c56c66 1431 uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift;
nucleosam 0:0ccd34c56c66 1432 if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\
nucleosam 0:0ccd34c56c66 1433 (speed<=(devicePrm.minSpeed>>stepModeShift)))||\
nucleosam 0:0ccd34c56c66 1434 ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake)))
nucleosam 0:0ccd34c56c66 1435 {
nucleosam 0:0ccd34c56c66 1436 /* Motion process complete */
nucleosam 0:0ccd34c56c66 1437 if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0)
nucleosam 0:0ccd34c56c66 1438 {
nucleosam 0:0ccd34c56c66 1439 devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted);
nucleosam 0:0ccd34c56c66 1440 if (devicePrm.direction==BACKWARD) devicePrm.direction=FORWARD;
nucleosam 0:0ccd34c56c66 1441 else devicePrm.direction=BACKWARD;
nucleosam 0:0ccd34c56c66 1442 Stspin220_Board_SetDirectionGpio(devicePrm.direction);
nucleosam 0:0ccd34c56c66 1443 if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0)
nucleosam 0:0ccd34c56c66 1444 {
nucleosam 0:0ccd34c56c66 1445 devicePrm.motionState = ACCELERATING;
nucleosam 0:0ccd34c56c66 1446 devicePrm.accu = 0;
nucleosam 0:0ccd34c56c66 1447 /* Apply accelerating torque */
nucleosam 0:0ccd34c56c66 1448 Stspin220_ApplyTorque(ACC_TORQUE);
nucleosam 0:0ccd34c56c66 1449 break;
nucleosam 0:0ccd34c56c66 1450 }
nucleosam 0:0ccd34c56c66 1451 }
nucleosam 0:0ccd34c56c66 1452 if (devicePrm.stopMode==HOLD_MODE)
nucleosam 0:0ccd34c56c66 1453 {
nucleosam 0:0ccd34c56c66 1454 Stspin220_HardStop();
nucleosam 0:0ccd34c56c66 1455 }
nucleosam 0:0ccd34c56c66 1456 else if (devicePrm.stopMode==STANDBY_MODE)
nucleosam 0:0ccd34c56c66 1457 {
nucleosam 0:0ccd34c56c66 1458 Stspin220_PutDeviceInStandby();
nucleosam 0:0ccd34c56c66 1459 }
nucleosam 0:0ccd34c56c66 1460 else
nucleosam 0:0ccd34c56c66 1461 {
nucleosam 0:0ccd34c56c66 1462 Stspin220_HardHiZ();
nucleosam 0:0ccd34c56c66 1463 }
nucleosam 0:0ccd34c56c66 1464 }
nucleosam 0:0ccd34c56c66 1465 else if ((devicePrm.commandExecuted==RUN_CMD)&&
nucleosam 0:0ccd34c56c66 1466 (speed<=(devicePrm.maxSpeed>>stepModeShift)))
nucleosam 0:0ccd34c56c66 1467 {
nucleosam 0:0ccd34c56c66 1468 devicePrm.motionState = STEADY;
nucleosam 0:0ccd34c56c66 1469 /* Apply running torque */
nucleosam 0:0ccd34c56c66 1470 Stspin220_ApplyTorque(RUN_TORQUE);
nucleosam 0:0ccd34c56c66 1471 }
nucleosam 0:0ccd34c56c66 1472 else
nucleosam 0:0ccd34c56c66 1473 {
nucleosam 0:0ccd34c56c66 1474 /* Go on decelerating */
nucleosam 0:0ccd34c56c66 1475 if (speed>(devicePrm.minSpeed>>stepModeShift))
nucleosam 0:0ccd34c56c66 1476 {
nucleosam 0:0ccd34c56c66 1477 bool speedUpdated = false;
nucleosam 0:0ccd34c56c66 1478 if (speed==0) speed =1;
nucleosam 0:0ccd34c56c66 1479 devicePrm.accu += dec / speed;
nucleosam 0:0ccd34c56c66 1480 while (devicePrm.accu>=(0X10000L))
nucleosam 0:0ccd34c56c66 1481 {
nucleosam 0:0ccd34c56c66 1482 devicePrm.accu -= (0X10000L);
nucleosam 0:0ccd34c56c66 1483 if (speed>1)
nucleosam 0:0ccd34c56c66 1484 {
nucleosam 0:0ccd34c56c66 1485 speed -=1;
nucleosam 0:0ccd34c56c66 1486 }
nucleosam 0:0ccd34c56c66 1487 speedUpdated = true;
nucleosam 0:0ccd34c56c66 1488 }
nucleosam 0:0ccd34c56c66 1489
nucleosam 0:0ccd34c56c66 1490 if (speedUpdated)
nucleosam 0:0ccd34c56c66 1491 {
nucleosam 0:0ccd34c56c66 1492 if (speed<(devicePrm.minSpeed>>stepModeShift))
nucleosam 0:0ccd34c56c66 1493 {
nucleosam 0:0ccd34c56c66 1494 speed = devicePrm.minSpeed>>stepModeShift;
nucleosam 0:0ccd34c56c66 1495 }
nucleosam 0:0ccd34c56c66 1496 devicePrm.speed = speed;
nucleosam 0:0ccd34c56c66 1497 }
nucleosam 0:0ccd34c56c66 1498
nucleosam 0:0ccd34c56c66 1499 if (devicePrm.updateTorque!=false)
nucleosam 0:0ccd34c56c66 1500 {
nucleosam 0:0ccd34c56c66 1501 /* Apply decelerating torque */
nucleosam 0:0ccd34c56c66 1502 Stspin220_ApplyTorque(DEC_TORQUE);
nucleosam 0:0ccd34c56c66 1503 }
nucleosam 0:0ccd34c56c66 1504 }
nucleosam 0:0ccd34c56c66 1505 }
nucleosam 0:0ccd34c56c66 1506 break;
nucleosam 0:0ccd34c56c66 1507 }
nucleosam 0:0ccd34c56c66 1508 default:
nucleosam 0:0ccd34c56c66 1509 {
nucleosam 0:0ccd34c56c66 1510 break;
nucleosam 0:0ccd34c56c66 1511 }
nucleosam 0:0ccd34c56c66 1512 }
nucleosam 0:0ccd34c56c66 1513 }
nucleosam 0:0ccd34c56c66 1514 if ((devicePrm.motionState & INACTIVE) != INACTIVE)
nucleosam 0:0ccd34c56c66 1515 {
nucleosam 0:0ccd34c56c66 1516 Stspin220_ApplySpeed(devicePrm.speed);
nucleosam 0:0ccd34c56c66 1517 }
nucleosam 0:0ccd34c56c66 1518 else
nucleosam 0:0ccd34c56c66 1519 {
nucleosam 0:0ccd34c56c66 1520 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
nucleosam 0:0ccd34c56c66 1521 {
nucleosam 0:0ccd34c56c66 1522 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
nucleosam 0:0ccd34c56c66 1523 }
nucleosam 0:0ccd34c56c66 1524 }
nucleosam 0:0ccd34c56c66 1525 Stspin220_Board_Monitor_Reset();
nucleosam 0:0ccd34c56c66 1526 }
nucleosam 0:0ccd34c56c66 1527
nucleosam 0:0ccd34c56c66 1528 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/