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:
Davidroid
Date:
Fri Jul 28 13:28:00 2017 +0000
Revision:
5:fd1315beea32
Parent:
4:265c30b9112a
Aligning to ARM mbed coding style.

Who changed what in which revision?

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