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:
davide.aliprandi@st.com
Date:
Fri Mar 24 10:59:19 2017 +0100
Revision:
3:a132aa6d66e4
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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 54 status_t STSpin220::Stspin220_Init(void* pInit)
davide.aliprandi@st.com 3:a132aa6d66e4 55 {
davide.aliprandi@st.com 3:a132aa6d66e4 56 Stspin220_Disable();
davide.aliprandi@st.com 3:a132aa6d66e4 57 if (pInit == NULL)
davide.aliprandi@st.com 3:a132aa6d66e4 58 {
davide.aliprandi@st.com 3:a132aa6d66e4 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 */
davide.aliprandi@st.com 3:a132aa6d66e4 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 {
davide.aliprandi@st.com 3:a132aa6d66e4 65 Stspin220_SetDeviceParamsToGivenValues((Stspin220_init_t*) pInit);
davide.aliprandi@st.com 3:a132aa6d66e4 66 }
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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;
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 133 void STSpin220::Stspin220_disable(void)
davide.aliprandi@st.com 3:a132aa6d66e4 134 {
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 142 void STSpin220::Stspin220_enable(void)
davide.aliprandi@st.com 3:a132aa6d66e4 143 {
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 */
davide.aliprandi@st.com 3:a132aa6d66e4 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 */
davide.aliprandi@st.com 3:a132aa6d66e4 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 */
davide.aliprandi@st.com 3:a132aa6d66e4 186 Stspin220_Board_TimStckInit(true);
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 294 motorStepMode_t STSpin220::Stspin220_get_step_mode(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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 363 void STSpin220::Stspin220_GoHome(void)
davide.aliprandi@st.com 3:a132aa6d66e4 364 {
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 372 void STSpin220::Stspin220_GoMark(void)
davide.aliprandi@st.com 3:a132aa6d66e4 373 {
davide.aliprandi@st.com 3:a132aa6d66e4 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 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 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 {
davide.aliprandi@st.com 3:a132aa6d66e4 400 Stspin220_ExitDeviceFromStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 401 }
davide.aliprandi@st.com 3:a132aa6d66e4 402 /* Deactivate motor if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 403 else if (devicePrm.motionState != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 404 {
davide.aliprandi@st.com 3:a132aa6d66e4 405 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 406 }
davide.aliprandi@st.com 3:a132aa6d66e4 407
davide.aliprandi@st.com 3:a132aa6d66e4 408 if (targetPosition > devicePrm.currentPosition)
davide.aliprandi@st.com 3:a132aa6d66e4 409 {
davide.aliprandi@st.com 3:a132aa6d66e4 410 devicePrm.stepsToTake = targetPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 411 devicePrm.currentPosition;
davide.aliprandi@st.com 3:a132aa6d66e4 412 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
davide.aliprandi@st.com 3:a132aa6d66e4 413 {
davide.aliprandi@st.com 3:a132aa6d66e4 414 direction = FORWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 415 }
davide.aliprandi@st.com 3:a132aa6d66e4 416 else
davide.aliprandi@st.com 3:a132aa6d66e4 417 {
davide.aliprandi@st.com 3:a132aa6d66e4 418 direction = BACKWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 419 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
davide.aliprandi@st.com 3:a132aa6d66e4 420 devicePrm.stepsToTake;
davide.aliprandi@st.com 3:a132aa6d66e4 421 }
davide.aliprandi@st.com 3:a132aa6d66e4 422 }
davide.aliprandi@st.com 3:a132aa6d66e4 423 else
davide.aliprandi@st.com 3:a132aa6d66e4 424 {
davide.aliprandi@st.com 3:a132aa6d66e4 425 devicePrm.stepsToTake = devicePrm.currentPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 426 targetPosition;
davide.aliprandi@st.com 3:a132aa6d66e4 427 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1))
davide.aliprandi@st.com 3:a132aa6d66e4 428 {
davide.aliprandi@st.com 3:a132aa6d66e4 429 direction = BACKWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 430 }
davide.aliprandi@st.com 3:a132aa6d66e4 431 else
davide.aliprandi@st.com 3:a132aa6d66e4 432 {
davide.aliprandi@st.com 3:a132aa6d66e4 433 direction = FORWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 434 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\
davide.aliprandi@st.com 3:a132aa6d66e4 435 devicePrm.stepsToTake;
davide.aliprandi@st.com 3:a132aa6d66e4 436 }
davide.aliprandi@st.com 3:a132aa6d66e4 437 }
davide.aliprandi@st.com 3:a132aa6d66e4 438
davide.aliprandi@st.com 3:a132aa6d66e4 439 if (devicePrm.stepsToTake != 0)
davide.aliprandi@st.com 3:a132aa6d66e4 440 {
davide.aliprandi@st.com 3:a132aa6d66e4 441 devicePrm.commandExecuted = MOVE_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 442
davide.aliprandi@st.com 3:a132aa6d66e4 443 /* Direction setup */
davide.aliprandi@st.com 3:a132aa6d66e4 444 Stspin220_SetDirection(direction);
davide.aliprandi@st.com 3:a132aa6d66e4 445
davide.aliprandi@st.com 3:a132aa6d66e4 446 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake);
davide.aliprandi@st.com 3:a132aa6d66e4 447
davide.aliprandi@st.com 3:a132aa6d66e4 448 /* Motor activation */
davide.aliprandi@st.com 3:a132aa6d66e4 449 Stspin220_StartMovement();
davide.aliprandi@st.com 3:a132aa6d66e4 450 }
davide.aliprandi@st.com 3:a132aa6d66e4 451 }
davide.aliprandi@st.com 3:a132aa6d66e4 452
davide.aliprandi@st.com 3:a132aa6d66e4 453 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 454 * @brief move the motor to the absolute position
davide.aliprandi@st.com 3:a132aa6d66e4 455 * @param[in] direction FORWARD or BACKWARD
davide.aliprandi@st.com 3:a132aa6d66e4 456 * @param[in] targetPosition 32 bit signed value position
davide.aliprandi@st.com 3:a132aa6d66e4 457 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 458 * @note The position is at the resolution corresponding to the
davide.aliprandi@st.com 3:a132aa6d66e4 459 * selected step mode.
davide.aliprandi@st.com 3:a132aa6d66e4 460 * STEP_MODE_FULL : step
davide.aliprandi@st.com 3:a132aa6d66e4 461 * STEP_MODE_HALF : 1/2 step
davide.aliprandi@st.com 3:a132aa6d66e4 462 * STEP_MODE_1_4 : 1/4 step
davide.aliprandi@st.com 3:a132aa6d66e4 463 * STEP_MODE_1_8 : 1/8 step
davide.aliprandi@st.com 3:a132aa6d66e4 464 * STEP_MODE_1_16 : 1/16 step
davide.aliprandi@st.com 3:a132aa6d66e4 465 * STEP_MODE_1_32 : 1/32 step
davide.aliprandi@st.com 3:a132aa6d66e4 466 * STEP_MODE_1_64 : 1/64 step
davide.aliprandi@st.com 3:a132aa6d66e4 467 * STEP_MODE_1_128 : 1/128 step
davide.aliprandi@st.com 3:a132aa6d66e4 468 * STEP_MODE_1_256 : 1/256 step
davide.aliprandi@st.com 3:a132aa6d66e4 469 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 470 void STSpin220::Stspin220_GoToDir(motorDir_t direction, int32_t targetPosition)
davide.aliprandi@st.com 3:a132aa6d66e4 471 {
davide.aliprandi@st.com 3:a132aa6d66e4 472 /* Exit from standby if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 473 if (devicePrm.motionState == STANDBY)
davide.aliprandi@st.com 3:a132aa6d66e4 474 {
davide.aliprandi@st.com 3:a132aa6d66e4 475 Stspin220_ExitDeviceFromStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 476 }
davide.aliprandi@st.com 3:a132aa6d66e4 477 /* Deactivate motor if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 478 else if (devicePrm.motionState != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 479 {
davide.aliprandi@st.com 3:a132aa6d66e4 480 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 481 }
davide.aliprandi@st.com 3:a132aa6d66e4 482
davide.aliprandi@st.com 3:a132aa6d66e4 483 if (direction != BACKWARD)
davide.aliprandi@st.com 3:a132aa6d66e4 484 {
davide.aliprandi@st.com 3:a132aa6d66e4 485 if (targetPosition > devicePrm.currentPosition)
davide.aliprandi@st.com 3:a132aa6d66e4 486 {
davide.aliprandi@st.com 3:a132aa6d66e4 487 devicePrm.stepsToTake = targetPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 488 devicePrm.currentPosition;
davide.aliprandi@st.com 3:a132aa6d66e4 489 }
davide.aliprandi@st.com 3:a132aa6d66e4 490 else
davide.aliprandi@st.com 3:a132aa6d66e4 491 {
davide.aliprandi@st.com 3:a132aa6d66e4 492 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
davide.aliprandi@st.com 3:a132aa6d66e4 493 (targetPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 494 devicePrm.currentPosition);
davide.aliprandi@st.com 3:a132aa6d66e4 495 }
davide.aliprandi@st.com 3:a132aa6d66e4 496 }
davide.aliprandi@st.com 3:a132aa6d66e4 497 else
davide.aliprandi@st.com 3:a132aa6d66e4 498 {
davide.aliprandi@st.com 3:a132aa6d66e4 499 if (targetPosition > devicePrm.currentPosition)
davide.aliprandi@st.com 3:a132aa6d66e4 500 {
davide.aliprandi@st.com 3:a132aa6d66e4 501 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\
davide.aliprandi@st.com 3:a132aa6d66e4 502 (devicePrm.currentPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 503 targetPosition);
davide.aliprandi@st.com 3:a132aa6d66e4 504 }
davide.aliprandi@st.com 3:a132aa6d66e4 505 else
davide.aliprandi@st.com 3:a132aa6d66e4 506 {
davide.aliprandi@st.com 3:a132aa6d66e4 507 devicePrm.stepsToTake = devicePrm.currentPosition -\
davide.aliprandi@st.com 3:a132aa6d66e4 508 targetPosition;
davide.aliprandi@st.com 3:a132aa6d66e4 509 }
davide.aliprandi@st.com 3:a132aa6d66e4 510 }
davide.aliprandi@st.com 3:a132aa6d66e4 511
davide.aliprandi@st.com 3:a132aa6d66e4 512 if (devicePrm.stepsToTake != 0)
davide.aliprandi@st.com 3:a132aa6d66e4 513 {
davide.aliprandi@st.com 3:a132aa6d66e4 514 devicePrm.commandExecuted = MOVE_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 515
davide.aliprandi@st.com 3:a132aa6d66e4 516 /* Direction setup */
davide.aliprandi@st.com 3:a132aa6d66e4 517 Stspin220_SetDirection(direction);
davide.aliprandi@st.com 3:a132aa6d66e4 518
davide.aliprandi@st.com 3:a132aa6d66e4 519 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake);
davide.aliprandi@st.com 3:a132aa6d66e4 520
davide.aliprandi@st.com 3:a132aa6d66e4 521 /* Motor activation */
davide.aliprandi@st.com 3:a132aa6d66e4 522 Stspin220_StartMovement();
davide.aliprandi@st.com 3:a132aa6d66e4 523 }
davide.aliprandi@st.com 3:a132aa6d66e4 524 }
davide.aliprandi@st.com 3:a132aa6d66e4 525
davide.aliprandi@st.com 3:a132aa6d66e4 526 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 527 * @brief Immediately stop the motor and disables the power bridges
davide.aliprandi@st.com 3:a132aa6d66e4 528 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 529 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 530 void STSpin220::Stspin220_HardHiZ(void)
davide.aliprandi@st.com 3:a132aa6d66e4 531 {
davide.aliprandi@st.com 3:a132aa6d66e4 532 /* Set inactive state */
davide.aliprandi@st.com 3:a132aa6d66e4 533 devicePrm.motionState = INACTIVE;
davide.aliprandi@st.com 3:a132aa6d66e4 534
davide.aliprandi@st.com 3:a132aa6d66e4 535 /* Disable step clock */
davide.aliprandi@st.com 3:a132aa6d66e4 536 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 537 {
davide.aliprandi@st.com 3:a132aa6d66e4 538 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
davide.aliprandi@st.com 3:a132aa6d66e4 539 }
davide.aliprandi@st.com 3:a132aa6d66e4 540
davide.aliprandi@st.com 3:a132aa6d66e4 541 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
davide.aliprandi@st.com 3:a132aa6d66e4 542 /* after the last step clock rising edge triggering the last step */
davide.aliprandi@st.com 3:a132aa6d66e4 543 Stspin220_Board_Delay(DISABLE_DELAY);
davide.aliprandi@st.com 3:a132aa6d66e4 544
davide.aliprandi@st.com 3:a132aa6d66e4 545 /* Set reference voltage to 0 */
davide.aliprandi@st.com 3:a132aa6d66e4 546 Stspin220_SetTorque(CURRENT_TORQUE, 0);
davide.aliprandi@st.com 3:a132aa6d66e4 547
davide.aliprandi@st.com 3:a132aa6d66e4 548 /* Disable power bridges */
davide.aliprandi@st.com 3:a132aa6d66e4 549 Stspin220_Board_Disable();
davide.aliprandi@st.com 3:a132aa6d66e4 550
davide.aliprandi@st.com 3:a132aa6d66e4 551 /* Comeback to nominal step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 552 if (devicePrm.stepModeLatched != devicePrm.stepMode)
davide.aliprandi@st.com 3:a132aa6d66e4 553 {
davide.aliprandi@st.com 3:a132aa6d66e4 554 Stspin220_Board_UnsetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 555 devicePrm.stepMode = devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 556 }
davide.aliprandi@st.com 3:a132aa6d66e4 557
davide.aliprandi@st.com 3:a132aa6d66e4 558 devicePrm.commandExecuted = NO_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 559 devicePrm.stepsToTake = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 560 devicePrm.speed = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 561 }
davide.aliprandi@st.com 3:a132aa6d66e4 562
davide.aliprandi@st.com 3:a132aa6d66e4 563 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 564 * @brief Immediatly stop the motor
davide.aliprandi@st.com 3:a132aa6d66e4 565 * and either set holding torque when stop mode is HOLD_MODE,
davide.aliprandi@st.com 3:a132aa6d66e4 566 * or call Stspin220_HardHiz function when stop mode is HIZ_MODE,
davide.aliprandi@st.com 3:a132aa6d66e4 567 * or call Stspin220_PutDeviceInStandby function when stop mode is STANDBY_MODE
davide.aliprandi@st.com 3:a132aa6d66e4 568 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 569 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 570 void STSpin220::Stspin220_HardStop(void)
davide.aliprandi@st.com 3:a132aa6d66e4 571 {
davide.aliprandi@st.com 3:a132aa6d66e4 572 if (devicePrm.stopMode == HOLD_MODE)
davide.aliprandi@st.com 3:a132aa6d66e4 573 {
davide.aliprandi@st.com 3:a132aa6d66e4 574 /* Set inactive state */
davide.aliprandi@st.com 3:a132aa6d66e4 575 devicePrm.motionState = INACTIVE;
davide.aliprandi@st.com 3:a132aa6d66e4 576
davide.aliprandi@st.com 3:a132aa6d66e4 577 /* Disable step clock */
davide.aliprandi@st.com 3:a132aa6d66e4 578 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 579 {
davide.aliprandi@st.com 3:a132aa6d66e4 580 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
davide.aliprandi@st.com 3:a132aa6d66e4 581 }
davide.aliprandi@st.com 3:a132aa6d66e4 582
davide.aliprandi@st.com 3:a132aa6d66e4 583 /* Set holding torque */
davide.aliprandi@st.com 3:a132aa6d66e4 584 Stspin220_ApplyTorque(HOLD_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 585
davide.aliprandi@st.com 3:a132aa6d66e4 586 /* Comeback to nominal step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 587 if (devicePrm.stepModeLatched != devicePrm.stepMode)
davide.aliprandi@st.com 3:a132aa6d66e4 588 {
davide.aliprandi@st.com 3:a132aa6d66e4 589 Stspin220_Board_UnsetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 590 devicePrm.stepMode = devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 591 }
davide.aliprandi@st.com 3:a132aa6d66e4 592
davide.aliprandi@st.com 3:a132aa6d66e4 593 devicePrm.commandExecuted = NO_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 594 devicePrm.stepsToTake = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 595 devicePrm.speed = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 596 }
davide.aliprandi@st.com 3:a132aa6d66e4 597 else if (devicePrm.stopMode == HIZ_MODE)
davide.aliprandi@st.com 3:a132aa6d66e4 598 {
davide.aliprandi@st.com 3:a132aa6d66e4 599 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 600 }
davide.aliprandi@st.com 3:a132aa6d66e4 601 else if (devicePrm.stopMode == STANDBY_MODE)
davide.aliprandi@st.com 3:a132aa6d66e4 602 {
davide.aliprandi@st.com 3:a132aa6d66e4 603 Stspin220_PutDeviceInStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 604 }
davide.aliprandi@st.com 3:a132aa6d66e4 605 }
davide.aliprandi@st.com 3:a132aa6d66e4 606
davide.aliprandi@st.com 3:a132aa6d66e4 607 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 608 * @brief Moves the motor of the specified number of steps
davide.aliprandi@st.com 3:a132aa6d66e4 609 * @param[in] direction FORWARD or BACKWARD
davide.aliprandi@st.com 3:a132aa6d66e4 610 * @param[in] stepCount Number of steps to perform
davide.aliprandi@st.com 3:a132aa6d66e4 611 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 612 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 613 void STSpin220::Stspin220_Move(motorDir_t direction, uint32_t stepCount)
davide.aliprandi@st.com 3:a132aa6d66e4 614 {
davide.aliprandi@st.com 3:a132aa6d66e4 615 /* Exit from standby if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 616 if (devicePrm.motionState == STANDBY)
davide.aliprandi@st.com 3:a132aa6d66e4 617 {
davide.aliprandi@st.com 3:a132aa6d66e4 618 Stspin220_ExitDeviceFromStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 619 }
davide.aliprandi@st.com 3:a132aa6d66e4 620 /* Deactivate motor if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 621 else if (devicePrm.motionState != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 622 {
davide.aliprandi@st.com 3:a132aa6d66e4 623 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 624 }
davide.aliprandi@st.com 3:a132aa6d66e4 625
davide.aliprandi@st.com 3:a132aa6d66e4 626 if (stepCount != 0)
davide.aliprandi@st.com 3:a132aa6d66e4 627 {
davide.aliprandi@st.com 3:a132aa6d66e4 628 devicePrm.stepsToTake = stepCount;
davide.aliprandi@st.com 3:a132aa6d66e4 629 devicePrm.commandExecuted = MOVE_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 630
davide.aliprandi@st.com 3:a132aa6d66e4 631 /* Direction setup */
davide.aliprandi@st.com 3:a132aa6d66e4 632 Stspin220_SetDirection(direction);
davide.aliprandi@st.com 3:a132aa6d66e4 633
davide.aliprandi@st.com 3:a132aa6d66e4 634 Stspin220_ComputeSpeedProfile(stepCount);
davide.aliprandi@st.com 3:a132aa6d66e4 635
davide.aliprandi@st.com 3:a132aa6d66e4 636 /* Motor activation */
davide.aliprandi@st.com 3:a132aa6d66e4 637 Stspin220_StartMovement();
davide.aliprandi@st.com 3:a132aa6d66e4 638 }
davide.aliprandi@st.com 3:a132aa6d66e4 639 }
davide.aliprandi@st.com 3:a132aa6d66e4 640
davide.aliprandi@st.com 3:a132aa6d66e4 641 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 642 * @brief Put STSpin220 device in standby (low power consumption)
davide.aliprandi@st.com 3:a132aa6d66e4 643 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 644 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 645 void STSpin220::Stspin220_PutDeviceInStandby(void)
davide.aliprandi@st.com 3:a132aa6d66e4 646 {
davide.aliprandi@st.com 3:a132aa6d66e4 647 /* Stop movement */
davide.aliprandi@st.com 3:a132aa6d66e4 648 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 649
davide.aliprandi@st.com 3:a132aa6d66e4 650 /* Enter standby */
davide.aliprandi@st.com 3:a132aa6d66e4 651 Stspin220_Board_Reset();
davide.aliprandi@st.com 3:a132aa6d66e4 652
davide.aliprandi@st.com 3:a132aa6d66e4 653 devicePrm.motionState = STANDBY;
davide.aliprandi@st.com 3:a132aa6d66e4 654 }
davide.aliprandi@st.com 3:a132aa6d66e4 655
davide.aliprandi@st.com 3:a132aa6d66e4 656 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 657 * @brief Runs the motor. It will accelerate from the min
davide.aliprandi@st.com 3:a132aa6d66e4 658 * speed up to the max speed by using the device acceleration.
davide.aliprandi@st.com 3:a132aa6d66e4 659 * @param[in] direction FORWARD or BACKWARD
davide.aliprandi@st.com 3:a132aa6d66e4 660 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 661 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 662 void STSpin220::Stspin220_Run(motorDir_t direction)
davide.aliprandi@st.com 3:a132aa6d66e4 663 {
davide.aliprandi@st.com 3:a132aa6d66e4 664 /* Exit from standby if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 665 if (devicePrm.motionState == STANDBY)
davide.aliprandi@st.com 3:a132aa6d66e4 666 {
davide.aliprandi@st.com 3:a132aa6d66e4 667 Stspin220_ExitDeviceFromStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 668 }
davide.aliprandi@st.com 3:a132aa6d66e4 669 /* Deactivate motor if needed */
davide.aliprandi@st.com 3:a132aa6d66e4 670 else if (devicePrm.motionState != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 671 {
davide.aliprandi@st.com 3:a132aa6d66e4 672 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 673 }
davide.aliprandi@st.com 3:a132aa6d66e4 674
davide.aliprandi@st.com 3:a132aa6d66e4 675 /* Direction setup */
davide.aliprandi@st.com 3:a132aa6d66e4 676 Stspin220_SetDirection(direction);
davide.aliprandi@st.com 3:a132aa6d66e4 677 devicePrm.commandExecuted = RUN_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 678 /* Motor activation */
davide.aliprandi@st.com 3:a132aa6d66e4 679 Stspin220_StartMovement();
davide.aliprandi@st.com 3:a132aa6d66e4 680 }
davide.aliprandi@st.com 3:a132aa6d66e4 681
davide.aliprandi@st.com 3:a132aa6d66e4 682 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 683 * @brief Changes the acceleration
davide.aliprandi@st.com 3:a132aa6d66e4 684 * @param[in] newAcc New acceleration to apply in pps^2
davide.aliprandi@st.com 3:a132aa6d66e4 685 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 686 * @note The command is not performed if the device is executing
davide.aliprandi@st.com 3:a132aa6d66e4 687 * a MOVE or GOTO command (but it can be used during a RUN command)
davide.aliprandi@st.com 3:a132aa6d66e4 688 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 689 bool STSpin220::Stspin220_SetAcceleration(uint16_t newAcc)
davide.aliprandi@st.com 3:a132aa6d66e4 690 {
davide.aliprandi@st.com 3:a132aa6d66e4 691 bool cmdExecuted = false;
davide.aliprandi@st.com 3:a132aa6d66e4 692 if ((newAcc != 0)&&
davide.aliprandi@st.com 3:a132aa6d66e4 693 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
davide.aliprandi@st.com 3:a132aa6d66e4 694 (devicePrm.commandExecuted == RUN_CMD)))
davide.aliprandi@st.com 3:a132aa6d66e4 695 {
davide.aliprandi@st.com 3:a132aa6d66e4 696 devicePrm.acceleration = newAcc;
davide.aliprandi@st.com 3:a132aa6d66e4 697 cmdExecuted = true;
davide.aliprandi@st.com 3:a132aa6d66e4 698 }
davide.aliprandi@st.com 3:a132aa6d66e4 699 return cmdExecuted;
davide.aliprandi@st.com 3:a132aa6d66e4 700 }
davide.aliprandi@st.com 3:a132aa6d66e4 701
davide.aliprandi@st.com 3:a132aa6d66e4 702 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 703 * @brief Changes the deceleration
davide.aliprandi@st.com 3:a132aa6d66e4 704 * @param[in] newDec New deceleration to apply in pps^2
davide.aliprandi@st.com 3:a132aa6d66e4 705 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 706 * @note The command is not performed if the device is executing
davide.aliprandi@st.com 3:a132aa6d66e4 707 * a MOVE or GOTO command (but it can be used during a RUN command)
davide.aliprandi@st.com 3:a132aa6d66e4 708 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 709 bool STSpin220::Stspin220_SetDeceleration(uint16_t newDec)
davide.aliprandi@st.com 3:a132aa6d66e4 710 {
davide.aliprandi@st.com 3:a132aa6d66e4 711 bool cmdExecuted = false;
davide.aliprandi@st.com 3:a132aa6d66e4 712 if ((newDec != 0)&&
davide.aliprandi@st.com 3:a132aa6d66e4 713 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
davide.aliprandi@st.com 3:a132aa6d66e4 714 (devicePrm.commandExecuted == RUN_CMD)))
davide.aliprandi@st.com 3:a132aa6d66e4 715 {
davide.aliprandi@st.com 3:a132aa6d66e4 716 devicePrm.deceleration = newDec;
davide.aliprandi@st.com 3:a132aa6d66e4 717 cmdExecuted = true;
davide.aliprandi@st.com 3:a132aa6d66e4 718 }
davide.aliprandi@st.com 3:a132aa6d66e4 719 return cmdExecuted;
davide.aliprandi@st.com 3:a132aa6d66e4 720 }
davide.aliprandi@st.com 3:a132aa6d66e4 721
davide.aliprandi@st.com 3:a132aa6d66e4 722 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 723 * @brief Specifies the direction
davide.aliprandi@st.com 3:a132aa6d66e4 724 * @param[in] dir FORWARD or BACKWARD
davide.aliprandi@st.com 3:a132aa6d66e4 725 * @note The direction change is applied if the device
davide.aliprandi@st.com 3:a132aa6d66e4 726 * is in INACTIVE or STANDBY state or if the device is
davide.aliprandi@st.com 3:a132aa6d66e4 727 * executing a run command
davide.aliprandi@st.com 3:a132aa6d66e4 728 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 729 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 730 void STSpin220::Stspin220_SetDirection(motorDir_t dir)
davide.aliprandi@st.com 3:a132aa6d66e4 731 {
davide.aliprandi@st.com 3:a132aa6d66e4 732 if ((devicePrm.motionState == INACTIVE)||\
davide.aliprandi@st.com 3:a132aa6d66e4 733 (devicePrm.motionState == STANDBY))
davide.aliprandi@st.com 3:a132aa6d66e4 734 {
davide.aliprandi@st.com 3:a132aa6d66e4 735 devicePrm.direction = dir;
davide.aliprandi@st.com 3:a132aa6d66e4 736 Stspin220_Board_SetDirectionGpio(dir);
davide.aliprandi@st.com 3:a132aa6d66e4 737 }
davide.aliprandi@st.com 3:a132aa6d66e4 738 else if ((devicePrm.commandExecuted&RUN_CMD)!=0)
davide.aliprandi@st.com 3:a132aa6d66e4 739 {
davide.aliprandi@st.com 3:a132aa6d66e4 740 devicePrm.commandExecuted=(deviceCommand_t)
davide.aliprandi@st.com 3:a132aa6d66e4 741 (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted);
davide.aliprandi@st.com 3:a132aa6d66e4 742 }
davide.aliprandi@st.com 3:a132aa6d66e4 743 }
davide.aliprandi@st.com 3:a132aa6d66e4 744
davide.aliprandi@st.com 3:a132aa6d66e4 745 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 746 * @brief Set current position to be the Home position
davide.aliprandi@st.com 3:a132aa6d66e4 747 * (current position set to 0)
davide.aliprandi@st.com 3:a132aa6d66e4 748 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 749 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 750 void STSpin220::Stspin220_SetHome(void)
davide.aliprandi@st.com 3:a132aa6d66e4 751 {
davide.aliprandi@st.com 3:a132aa6d66e4 752 devicePrm.currentPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 753 }
davide.aliprandi@st.com 3:a132aa6d66e4 754
davide.aliprandi@st.com 3:a132aa6d66e4 755 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 756 * @brief Set current position to be the Mark position
davide.aliprandi@st.com 3:a132aa6d66e4 757 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 758 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 759 void STSpin220::Stspin220_SetMark(void)
davide.aliprandi@st.com 3:a132aa6d66e4 760 {
davide.aliprandi@st.com 3:a132aa6d66e4 761 devicePrm.markPosition = devicePrm.currentPosition;
davide.aliprandi@st.com 3:a132aa6d66e4 762 }
davide.aliprandi@st.com 3:a132aa6d66e4 763
davide.aliprandi@st.com 3:a132aa6d66e4 764 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 765 * @brief Changes the max speed
davide.aliprandi@st.com 3:a132aa6d66e4 766 * @param[in] newMaxSpeed New max speed to apply in pps
davide.aliprandi@st.com 3:a132aa6d66e4 767 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 768 * @note The command is not performed is the device is executing
davide.aliprandi@st.com 3:a132aa6d66e4 769 * a MOVE or GOTO command (but it can be used during a RUN command).
davide.aliprandi@st.com 3:a132aa6d66e4 770 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 771 bool STSpin220::Stspin220_SetMaxSpeed(uint16_t newMaxSpeed)
davide.aliprandi@st.com 3:a132aa6d66e4 772 {
davide.aliprandi@st.com 3:a132aa6d66e4 773 bool cmdExecuted = false;
davide.aliprandi@st.com 3:a132aa6d66e4 774 if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 775 ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\
davide.aliprandi@st.com 3:a132aa6d66e4 776 ((devicePrm.torqueBoostEnable != false)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 777 ((newMaxSpeed>>Stspin220_get_step_mode())<= STSPIN220_MAX_STCK_FREQ)))&&\
davide.aliprandi@st.com 3:a132aa6d66e4 778 (devicePrm.minSpeed <= newMaxSpeed) &&\
davide.aliprandi@st.com 3:a132aa6d66e4 779 (((devicePrm.motionState & INACTIVE) == INACTIVE)||\
davide.aliprandi@st.com 3:a132aa6d66e4 780 (devicePrm.commandExecuted == RUN_CMD)))
davide.aliprandi@st.com 3:a132aa6d66e4 781 {
davide.aliprandi@st.com 3:a132aa6d66e4 782 devicePrm.maxSpeed = newMaxSpeed;
davide.aliprandi@st.com 3:a132aa6d66e4 783 cmdExecuted = true;
davide.aliprandi@st.com 3:a132aa6d66e4 784 }
davide.aliprandi@st.com 3:a132aa6d66e4 785 return cmdExecuted;
davide.aliprandi@st.com 3:a132aa6d66e4 786 }
davide.aliprandi@st.com 3:a132aa6d66e4 787
davide.aliprandi@st.com 3:a132aa6d66e4 788 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 789 * @brief Changes the min speed
davide.aliprandi@st.com 3:a132aa6d66e4 790 * @param[in] newMinSpeed New min speed to apply in pps
davide.aliprandi@st.com 3:a132aa6d66e4 791 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 792 * @note The command is not performed is the device is executing
davide.aliprandi@st.com 3:a132aa6d66e4 793 * a MOVE or GOTO command (but it can be used during a RUN command).
davide.aliprandi@st.com 3:a132aa6d66e4 794 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 795 bool STSpin220::Stspin220_SetMinSpeed(uint16_t newMinSpeed)
davide.aliprandi@st.com 3:a132aa6d66e4 796 {
davide.aliprandi@st.com 3:a132aa6d66e4 797 bool cmdExecuted = false;
davide.aliprandi@st.com 3:a132aa6d66e4 798 if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&&
davide.aliprandi@st.com 3:a132aa6d66e4 799 (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) &&
davide.aliprandi@st.com 3:a132aa6d66e4 800 (newMinSpeed <= devicePrm.maxSpeed) &&
davide.aliprandi@st.com 3:a132aa6d66e4 801 (((devicePrm.motionState & INACTIVE) == INACTIVE)||
davide.aliprandi@st.com 3:a132aa6d66e4 802 (devicePrm.commandExecuted == RUN_CMD)))
davide.aliprandi@st.com 3:a132aa6d66e4 803 {
davide.aliprandi@st.com 3:a132aa6d66e4 804 devicePrm.minSpeed = newMinSpeed;
davide.aliprandi@st.com 3:a132aa6d66e4 805 cmdExecuted = true;
davide.aliprandi@st.com 3:a132aa6d66e4 806 }
davide.aliprandi@st.com 3:a132aa6d66e4 807 return cmdExecuted;
davide.aliprandi@st.com 3:a132aa6d66e4 808 }
davide.aliprandi@st.com 3:a132aa6d66e4 809
davide.aliprandi@st.com 3:a132aa6d66e4 810 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 811 * @brief Set the stepping mode
davide.aliprandi@st.com 3:a132aa6d66e4 812 * @param[in] stepMode from full step to 1/256 microstep
davide.aliprandi@st.com 3:a132aa6d66e4 813 * as specified in enum motorStepMode_t
davide.aliprandi@st.com 3:a132aa6d66e4 814 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 815 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 816 bool STSpin220::Stspin220_SetStepMode(motorStepMode_t stepMode)
davide.aliprandi@st.com 3:a132aa6d66e4 817 {
davide.aliprandi@st.com 3:a132aa6d66e4 818 /* Eventually deactivate motor */
davide.aliprandi@st.com 3:a132aa6d66e4 819 if ((devicePrm.motionState != INACTIVE)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 820 (devicePrm.motionState != STANDBY))
davide.aliprandi@st.com 3:a132aa6d66e4 821 {
davide.aliprandi@st.com 3:a132aa6d66e4 822 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 823 }
davide.aliprandi@st.com 3:a132aa6d66e4 824
davide.aliprandi@st.com 3:a132aa6d66e4 825 /* Enter standby */
davide.aliprandi@st.com 3:a132aa6d66e4 826 Stspin220_Board_Reset();
davide.aliprandi@st.com 3:a132aa6d66e4 827
davide.aliprandi@st.com 3:a132aa6d66e4 828 /* Reset the microstepping sequencer position */
davide.aliprandi@st.com 3:a132aa6d66e4 829 devicePrm.sequencerPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 830
davide.aliprandi@st.com 3:a132aa6d66e4 831 /* Reset current and mark positions */
davide.aliprandi@st.com 3:a132aa6d66e4 832 devicePrm.currentPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 833 devicePrm.markPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 834
davide.aliprandi@st.com 3:a132aa6d66e4 835 /* Set the step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 836 return (Stspin220_SetStepModeWithoutReset(stepMode));
davide.aliprandi@st.com 3:a132aa6d66e4 837 }
davide.aliprandi@st.com 3:a132aa6d66e4 838
davide.aliprandi@st.com 3:a132aa6d66e4 839 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 840 * @brief Select the mode to stop the motor.
davide.aliprandi@st.com 3:a132aa6d66e4 841 * @param[in] stopMode HOLD_MODE to let power bridge enabled
davide.aliprandi@st.com 3:a132aa6d66e4 842 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 843 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 844 void STSpin220::Stspin220_SetStopMode(motorStopMode_t stopMode)
davide.aliprandi@st.com 3:a132aa6d66e4 845 {
davide.aliprandi@st.com 3:a132aa6d66e4 846 devicePrm.stopMode = stopMode;
davide.aliprandi@st.com 3:a132aa6d66e4 847 }
davide.aliprandi@st.com 3:a132aa6d66e4 848
davide.aliprandi@st.com 3:a132aa6d66e4 849 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 850 * @brief Set the torque
davide.aliprandi@st.com 3:a132aa6d66e4 851 * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t
davide.aliprandi@st.com 3:a132aa6d66e4 852 * @param[in] torqueValue in % (from 0 to 100)
davide.aliprandi@st.com 3:a132aa6d66e4 853 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 854 * @note
davide.aliprandi@st.com 3:a132aa6d66e4 855 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 856 void STSpin220::Stspin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue)
davide.aliprandi@st.com 3:a132aa6d66e4 857 {
davide.aliprandi@st.com 3:a132aa6d66e4 858 devicePrm.updateTorque = true;
davide.aliprandi@st.com 3:a132aa6d66e4 859 if (torqueValue>100) torqueValue = 100;
davide.aliprandi@st.com 3:a132aa6d66e4 860 switch(torqueMode)
davide.aliprandi@st.com 3:a132aa6d66e4 861 {
davide.aliprandi@st.com 3:a132aa6d66e4 862 case ACC_TORQUE:
davide.aliprandi@st.com 3:a132aa6d66e4 863 devicePrm.accelTorque = torqueValue;
davide.aliprandi@st.com 3:a132aa6d66e4 864 break;
davide.aliprandi@st.com 3:a132aa6d66e4 865 case DEC_TORQUE:
davide.aliprandi@st.com 3:a132aa6d66e4 866 devicePrm.decelTorque = torqueValue;
davide.aliprandi@st.com 3:a132aa6d66e4 867 break;
davide.aliprandi@st.com 3:a132aa6d66e4 868 case RUN_TORQUE:
davide.aliprandi@st.com 3:a132aa6d66e4 869 devicePrm.runTorque = torqueValue;
davide.aliprandi@st.com 3:a132aa6d66e4 870 break;
davide.aliprandi@st.com 3:a132aa6d66e4 871 case HOLD_TORQUE:
davide.aliprandi@st.com 3:a132aa6d66e4 872 devicePrm.holdTorque = torqueValue;
davide.aliprandi@st.com 3:a132aa6d66e4 873 if (devicePrm.motionState != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 874 {
davide.aliprandi@st.com 3:a132aa6d66e4 875 break;
davide.aliprandi@st.com 3:a132aa6d66e4 876 }
davide.aliprandi@st.com 3:a132aa6d66e4 877 case CURRENT_TORQUE:
davide.aliprandi@st.com 3:a132aa6d66e4 878 devicePrm.currentTorque = torqueValue;
davide.aliprandi@st.com 3:a132aa6d66e4 879 Stspin220_Board_PwmRefSetDutyCycle(torqueValue);
davide.aliprandi@st.com 3:a132aa6d66e4 880 default:
davide.aliprandi@st.com 3:a132aa6d66e4 881 devicePrm.updateTorque = false;
davide.aliprandi@st.com 3:a132aa6d66e4 882 break; //ignore error
davide.aliprandi@st.com 3:a132aa6d66e4 883 }
davide.aliprandi@st.com 3:a132aa6d66e4 884 }
davide.aliprandi@st.com 3:a132aa6d66e4 885
davide.aliprandi@st.com 3:a132aa6d66e4 886 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 887 * @brief Enable or disable the torque boost feature
davide.aliprandi@st.com 3:a132aa6d66e4 888 * @param[in] enable true to enable torque boost, false to disable
davide.aliprandi@st.com 3:a132aa6d66e4 889 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 890 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 891 void STSpin220::Stspin220_SetTorqueBoostEnable(bool enable)
davide.aliprandi@st.com 3:a132aa6d66e4 892 {
davide.aliprandi@st.com 3:a132aa6d66e4 893 devicePrm.torqueBoostEnable = enable;
davide.aliprandi@st.com 3:a132aa6d66e4 894 }
davide.aliprandi@st.com 3:a132aa6d66e4 895
davide.aliprandi@st.com 3:a132aa6d66e4 896 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 897 * @brief Set the torque boost threshold
davide.aliprandi@st.com 3:a132aa6d66e4 898 * @param[in] speedThreshold speed threshold above which the step mode is
davide.aliprandi@st.com 3:a132aa6d66e4 899 * changed to full step
davide.aliprandi@st.com 3:a132aa6d66e4 900 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 901 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 902 void STSpin220::Stspin220_SetTorqueBoostThreshold(uint16_t speedThreshold)
davide.aliprandi@st.com 3:a132aa6d66e4 903 {
davide.aliprandi@st.com 3:a132aa6d66e4 904 devicePrm.torqueBoostSpeedThreshold = speedThreshold;
davide.aliprandi@st.com 3:a132aa6d66e4 905 }
davide.aliprandi@st.com 3:a132aa6d66e4 906
davide.aliprandi@st.com 3:a132aa6d66e4 907 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 908 * @brief Stops the motor by using the device deceleration
davide.aliprandi@st.com 3:a132aa6d66e4 909 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 910 * @note The command is not performed if the device is in INACTIVE,
davide.aliprandi@st.com 3:a132aa6d66e4 911 * STANDBYTOINACTIVE or STANDBY state.
davide.aliprandi@st.com 3:a132aa6d66e4 912 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 913 bool STSpin220::Stspin220_SoftStop(void)
davide.aliprandi@st.com 3:a132aa6d66e4 914 {
davide.aliprandi@st.com 3:a132aa6d66e4 915 bool cmdExecuted = false;
davide.aliprandi@st.com 3:a132aa6d66e4 916 if ((devicePrm.motionState & INACTIVE) != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 917 {
davide.aliprandi@st.com 3:a132aa6d66e4 918 devicePrm.commandExecuted=(deviceCommand_t)
davide.aliprandi@st.com 3:a132aa6d66e4 919 (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted);
davide.aliprandi@st.com 3:a132aa6d66e4 920 cmdExecuted = true;
davide.aliprandi@st.com 3:a132aa6d66e4 921 }
davide.aliprandi@st.com 3:a132aa6d66e4 922 return (cmdExecuted);
davide.aliprandi@st.com 3:a132aa6d66e4 923 }
davide.aliprandi@st.com 3:a132aa6d66e4 924
davide.aliprandi@st.com 3:a132aa6d66e4 925 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 926 * @brief Get the frequency of REF PWM
davide.aliprandi@st.com 3:a132aa6d66e4 927 * @retval the frequency of REF PWM in Hz
davide.aliprandi@st.com 3:a132aa6d66e4 928 * @note
davide.aliprandi@st.com 3:a132aa6d66e4 929 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 930 uint32_t STSpin220::Stspin220_VrefPwmGetFreq(void)
davide.aliprandi@st.com 3:a132aa6d66e4 931 {
davide.aliprandi@st.com 3:a132aa6d66e4 932 return devicePrm.refPwmFreq;
davide.aliprandi@st.com 3:a132aa6d66e4 933 }
davide.aliprandi@st.com 3:a132aa6d66e4 934
davide.aliprandi@st.com 3:a132aa6d66e4 935 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 936 * @brief Set the frequency of REF PWM
davide.aliprandi@st.com 3:a132aa6d66e4 937 * @param[in] newFreq in Hz
davide.aliprandi@st.com 3:a132aa6d66e4 938 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 939 * @note
davide.aliprandi@st.com 3:a132aa6d66e4 940 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 941 void STSpin220::Stspin220_VrefPwmSetFreq(uint32_t newFreq)
davide.aliprandi@st.com 3:a132aa6d66e4 942 {
davide.aliprandi@st.com 3:a132aa6d66e4 943 devicePrm.refPwmFreq = newFreq;
davide.aliprandi@st.com 3:a132aa6d66e4 944 Stspin220_Board_PwmRefSetFreq(newFreq);
davide.aliprandi@st.com 3:a132aa6d66e4 945 }
davide.aliprandi@st.com 3:a132aa6d66e4 946
davide.aliprandi@st.com 3:a132aa6d66e4 947 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 948 * @brief Locks until the device state becomes Inactive
davide.aliprandi@st.com 3:a132aa6d66e4 949 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 950 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 951 void STSpin220::Stspin220_WaitWhileActive(void)
davide.aliprandi@st.com 3:a132aa6d66e4 952 {
davide.aliprandi@st.com 3:a132aa6d66e4 953 /* Wait while motor is running */
davide.aliprandi@st.com 3:a132aa6d66e4 954 while (((Stspin220_get_device_state()&INACTIVE)!=INACTIVE)||\
davide.aliprandi@st.com 3:a132aa6d66e4 955 (((Stspin220_get_device_state()&INACTIVE)==INACTIVE)&&(toggleOdd!=0)));
davide.aliprandi@st.com 3:a132aa6d66e4 956 }
davide.aliprandi@st.com 3:a132aa6d66e4 957
davide.aliprandi@st.com 3:a132aa6d66e4 958 /* ------------------------------------------------------------------------- */
davide.aliprandi@st.com 3:a132aa6d66e4 959 /* Internal functions ------------------------------------------------------ */
davide.aliprandi@st.com 3:a132aa6d66e4 960 /* ------------------------------------------------------------------------- */
davide.aliprandi@st.com 3:a132aa6d66e4 961 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 962 * @brief Updates the current speed of the device
davide.aliprandi@st.com 3:a132aa6d66e4 963 * @param[in] newSpeed in pps
davide.aliprandi@st.com 3:a132aa6d66e4 964 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 965 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 966 void STSpin220::Stspin220_ApplySpeed(uint16_t newSpeed)
davide.aliprandi@st.com 3:a132aa6d66e4 967 {
davide.aliprandi@st.com 3:a132aa6d66e4 968 if (devicePrm.torqueBoostEnable != false)
davide.aliprandi@st.com 3:a132aa6d66e4 969 {
davide.aliprandi@st.com 3:a132aa6d66e4 970 if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256)
davide.aliprandi@st.com 3:a132aa6d66e4 971 {
davide.aliprandi@st.com 3:a132aa6d66e4 972 Stspin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED);
davide.aliprandi@st.com 3:a132aa6d66e4 973 }
davide.aliprandi@st.com 3:a132aa6d66e4 974 if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL)
davide.aliprandi@st.com 3:a132aa6d66e4 975 {
davide.aliprandi@st.com 3:a132aa6d66e4 976 if (((newSpeed>>devicePrm.stepModeLatched)>\
davide.aliprandi@st.com 3:a132aa6d66e4 977 devicePrm.torqueBoostSpeedThreshold)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 978 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\
davide.aliprandi@st.com 3:a132aa6d66e4 979 ((devicePrm.stepsToTake-devicePrm.relativePos)>=\
davide.aliprandi@st.com 3:a132aa6d66e4 980 (1<<devicePrm.stepModeLatched))))
davide.aliprandi@st.com 3:a132aa6d66e4 981 {
davide.aliprandi@st.com 3:a132aa6d66e4 982 if ((devicePrm.sequencerPosition & 0xFF) == 0X80)
davide.aliprandi@st.com 3:a132aa6d66e4 983 {
davide.aliprandi@st.com 3:a132aa6d66e4 984 Stspin220_Board_SetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 985 devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL;
davide.aliprandi@st.com 3:a132aa6d66e4 986 devicePrm.accu >>= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 987 newSpeed >>= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 988 }
davide.aliprandi@st.com 3:a132aa6d66e4 989 }
davide.aliprandi@st.com 3:a132aa6d66e4 990 }
davide.aliprandi@st.com 3:a132aa6d66e4 991 else if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\
davide.aliprandi@st.com 3:a132aa6d66e4 992 (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\
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 Stspin220_Board_UnsetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 998 devicePrm.stepMode = devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 999 devicePrm.accu <<= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 1000 newSpeed <<= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 1001 }
davide.aliprandi@st.com 3:a132aa6d66e4 1002 }
davide.aliprandi@st.com 3:a132aa6d66e4 1003 else if (devicePrm.stepMode != devicePrm.stepModeLatched)
davide.aliprandi@st.com 3:a132aa6d66e4 1004 {
davide.aliprandi@st.com 3:a132aa6d66e4 1005 //torqueBoostEnable has just been disabled
davide.aliprandi@st.com 3:a132aa6d66e4 1006 Stspin220_Board_UnsetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 1007 devicePrm.stepMode = devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 1008 devicePrm.accu <<= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 1009 newSpeed <<= devicePrm.stepModeLatched;
davide.aliprandi@st.com 3:a132aa6d66e4 1010 }
davide.aliprandi@st.com 3:a132aa6d66e4 1011
davide.aliprandi@st.com 3:a132aa6d66e4 1012 if (newSpeed < STSPIN220_MIN_STCK_FREQ)
davide.aliprandi@st.com 3:a132aa6d66e4 1013 {
davide.aliprandi@st.com 3:a132aa6d66e4 1014 newSpeed = STSPIN220_MIN_STCK_FREQ;
davide.aliprandi@st.com 3:a132aa6d66e4 1015 }
davide.aliprandi@st.com 3:a132aa6d66e4 1016 if (newSpeed > STSPIN220_MAX_STCK_FREQ)
davide.aliprandi@st.com 3:a132aa6d66e4 1017 {
davide.aliprandi@st.com 3:a132aa6d66e4 1018 newSpeed = STSPIN220_MAX_STCK_FREQ;
davide.aliprandi@st.com 3:a132aa6d66e4 1019 }
davide.aliprandi@st.com 3:a132aa6d66e4 1020
davide.aliprandi@st.com 3:a132aa6d66e4 1021 devicePrm.speed = newSpeed;
davide.aliprandi@st.com 3:a132aa6d66e4 1022 Stspin220_Board_TimStckSetFreq(newSpeed);
davide.aliprandi@st.com 3:a132aa6d66e4 1023
davide.aliprandi@st.com 3:a132aa6d66e4 1024 }
davide.aliprandi@st.com 3:a132aa6d66e4 1025
davide.aliprandi@st.com 3:a132aa6d66e4 1026 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1027 * @brief Computes the speed profile according to the number of steps to move
davide.aliprandi@st.com 3:a132aa6d66e4 1028 * @param[in] nbSteps number of steps to perform
davide.aliprandi@st.com 3:a132aa6d66e4 1029 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1030 * @note Using the acceleration and deceleration of the device,
davide.aliprandi@st.com 3:a132aa6d66e4 1031 * this function determines the duration in steps of the acceleration,
davide.aliprandi@st.com 3:a132aa6d66e4 1032 * steady and deceleration phases.
davide.aliprandi@st.com 3:a132aa6d66e4 1033 * If the total number of steps to perform is big enough, a trapezoidal move
davide.aliprandi@st.com 3:a132aa6d66e4 1034 * is performed (i.e. there is a steady phase where the motor runs at the maximum
davide.aliprandi@st.com 3:a132aa6d66e4 1035 * speed.
davide.aliprandi@st.com 3:a132aa6d66e4 1036 * Else, a triangular move is performed (no steady phase: the maximum speed is never
davide.aliprandi@st.com 3:a132aa6d66e4 1037 * reached.
davide.aliprandi@st.com 3:a132aa6d66e4 1038 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1039 void STSpin220::Stspin220_ComputeSpeedProfile(uint32_t nbSteps)
davide.aliprandi@st.com 3:a132aa6d66e4 1040 {
davide.aliprandi@st.com 3:a132aa6d66e4 1041 uint32_t reqAccSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1042 uint32_t reqDecSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1043
davide.aliprandi@st.com 3:a132aa6d66e4 1044 /* compute the number of steps to get the targeted speed */
davide.aliprandi@st.com 3:a132aa6d66e4 1045 uint16_t minSpeed = devicePrm.minSpeed;
davide.aliprandi@st.com 3:a132aa6d66e4 1046 reqAccSteps = (devicePrm.maxSpeed - minSpeed);
davide.aliprandi@st.com 3:a132aa6d66e4 1047 reqAccSteps *= (devicePrm.maxSpeed + minSpeed);
davide.aliprandi@st.com 3:a132aa6d66e4 1048 reqDecSteps = reqAccSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1049 reqAccSteps /= (uint32_t)devicePrm.acceleration;
davide.aliprandi@st.com 3:a132aa6d66e4 1050 reqAccSteps /= 2;
davide.aliprandi@st.com 3:a132aa6d66e4 1051
davide.aliprandi@st.com 3:a132aa6d66e4 1052 /* compute the number of steps to stop */
davide.aliprandi@st.com 3:a132aa6d66e4 1053 reqDecSteps /= (uint32_t)devicePrm.deceleration;
davide.aliprandi@st.com 3:a132aa6d66e4 1054 reqDecSteps /= 2;
davide.aliprandi@st.com 3:a132aa6d66e4 1055
davide.aliprandi@st.com 3:a132aa6d66e4 1056 if(( reqAccSteps + reqDecSteps ) > nbSteps)
davide.aliprandi@st.com 3:a132aa6d66e4 1057 {
davide.aliprandi@st.com 3:a132aa6d66e4 1058 /* Triangular move */
davide.aliprandi@st.com 3:a132aa6d66e4 1059 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
davide.aliprandi@st.com 3:a132aa6d66e4 1060 uint32_t dec = devicePrm.deceleration;
davide.aliprandi@st.com 3:a132aa6d66e4 1061 uint32_t acc = devicePrm.acceleration;
davide.aliprandi@st.com 3:a132aa6d66e4 1062
davide.aliprandi@st.com 3:a132aa6d66e4 1063 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec);
davide.aliprandi@st.com 3:a132aa6d66e4 1064 if (reqDecSteps > 1)
davide.aliprandi@st.com 3:a132aa6d66e4 1065 {
davide.aliprandi@st.com 3:a132aa6d66e4 1066 reqAccSteps = reqDecSteps - 1;
davide.aliprandi@st.com 3:a132aa6d66e4 1067 if(reqAccSteps == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1068 {
davide.aliprandi@st.com 3:a132aa6d66e4 1069 reqAccSteps = 1;
davide.aliprandi@st.com 3:a132aa6d66e4 1070 }
davide.aliprandi@st.com 3:a132aa6d66e4 1071 }
davide.aliprandi@st.com 3:a132aa6d66e4 1072 else
davide.aliprandi@st.com 3:a132aa6d66e4 1073 {
davide.aliprandi@st.com 3:a132aa6d66e4 1074 reqAccSteps = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1075 }
davide.aliprandi@st.com 3:a132aa6d66e4 1076 devicePrm.endAccPos = reqAccSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1077 devicePrm.startDecPos = reqDecSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1078 }
davide.aliprandi@st.com 3:a132aa6d66e4 1079 else
davide.aliprandi@st.com 3:a132aa6d66e4 1080 {
davide.aliprandi@st.com 3:a132aa6d66e4 1081 /* Trapezoidal move */
davide.aliprandi@st.com 3:a132aa6d66e4 1082 /* accelerating phase to endAccPos */
davide.aliprandi@st.com 3:a132aa6d66e4 1083 /* steady phase from endAccPos to startDecPos */
davide.aliprandi@st.com 3:a132aa6d66e4 1084 /* decelerating from startDecPos to stepsToTake*/
davide.aliprandi@st.com 3:a132aa6d66e4 1085 devicePrm.endAccPos = reqAccSteps;
davide.aliprandi@st.com 3:a132aa6d66e4 1086 devicePrm.startDecPos = nbSteps - reqDecSteps - 1;
davide.aliprandi@st.com 3:a132aa6d66e4 1087 }
davide.aliprandi@st.com 3:a132aa6d66e4 1088 }
davide.aliprandi@st.com 3:a132aa6d66e4 1089
davide.aliprandi@st.com 3:a132aa6d66e4 1090 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1091 * @brief Set the parameters of the device whose values are not defined in
davide.aliprandi@st.com 3:a132aa6d66e4 1092 * stspin220_target_config.h
davide.aliprandi@st.com 3:a132aa6d66e4 1093 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1094 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1095 void STSpin220::Stspin220_SetDeviceParamsOtherValues(void)
davide.aliprandi@st.com 3:a132aa6d66e4 1096 {
davide.aliprandi@st.com 3:a132aa6d66e4 1097 uint16_t tmp;
davide.aliprandi@st.com 3:a132aa6d66e4 1098
davide.aliprandi@st.com 3:a132aa6d66e4 1099 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1100 devicePrm.currentPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1101 devicePrm.sequencerPosition = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1102 devicePrm.endAccPos = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1103 devicePrm.relativePos = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1104 devicePrm.startDecPos = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1105 devicePrm.stepsToTake = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1106 devicePrm.updateTorque = false;
davide.aliprandi@st.com 3:a132aa6d66e4 1107 devicePrm.speed = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1108 devicePrm.commandExecuted = NO_CMD;
davide.aliprandi@st.com 3:a132aa6d66e4 1109 devicePrm.direction = FORWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 1110 tmp = devicePrm.minSpeed;
davide.aliprandi@st.com 3:a132aa6d66e4 1111 if (((devicePrm.torqueBoostEnable != false)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 1112 (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\
davide.aliprandi@st.com 3:a132aa6d66e4 1113 (tmp>devicePrm.maxSpeed))
davide.aliprandi@st.com 3:a132aa6d66e4 1114 {
davide.aliprandi@st.com 3:a132aa6d66e4 1115 Stspin220_ErrorHandler(STSPIN220_ERROR_INIT);
davide.aliprandi@st.com 3:a132aa6d66e4 1116 }
davide.aliprandi@st.com 3:a132aa6d66e4 1117 }
davide.aliprandi@st.com 3:a132aa6d66e4 1118
davide.aliprandi@st.com 3:a132aa6d66e4 1119 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1120 * @brief Set the parameters of the device to values of the structure pointed
davide.aliprandi@st.com 3:a132aa6d66e4 1121 * by pInitDevicePrm. Set GPIO according to these values.
davide.aliprandi@st.com 3:a132aa6d66e4 1122 * @param pInitDevicePrm pointer onto the structure containing values to
davide.aliprandi@st.com 3:a132aa6d66e4 1123 * initialize the device parameters.
davide.aliprandi@st.com 3:a132aa6d66e4 1124 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1125 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1126 void STSpin220::Stspin220_SetDeviceParamsToGivenValues(Stspin220_init_t* pInitDevicePrm)
davide.aliprandi@st.com 3:a132aa6d66e4 1127 {
davide.aliprandi@st.com 3:a132aa6d66e4 1128 devicePrm.motionState = STANDBY;
davide.aliprandi@st.com 3:a132aa6d66e4 1129
davide.aliprandi@st.com 3:a132aa6d66e4 1130 if (Stspin220_SetAcceleration(pInitDevicePrm->acceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
davide.aliprandi@st.com 3:a132aa6d66e4 1131 if (Stspin220_SetDeceleration(pInitDevicePrm->deceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
davide.aliprandi@st.com 3:a132aa6d66e4 1132 if (Stspin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
davide.aliprandi@st.com 3:a132aa6d66e4 1133 if (Stspin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
davide.aliprandi@st.com 3:a132aa6d66e4 1134
davide.aliprandi@st.com 3:a132aa6d66e4 1135 Stspin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq);
davide.aliprandi@st.com 3:a132aa6d66e4 1136 Stspin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque);
davide.aliprandi@st.com 3:a132aa6d66e4 1137 Stspin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque);
davide.aliprandi@st.com 3:a132aa6d66e4 1138 Stspin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque);
davide.aliprandi@st.com 3:a132aa6d66e4 1139 Stspin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque);
davide.aliprandi@st.com 3:a132aa6d66e4 1140 devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
davide.aliprandi@st.com 3:a132aa6d66e4 1141 devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
davide.aliprandi@st.com 3:a132aa6d66e4 1142 Stspin220_SetStopMode(pInitDevicePrm->stopMode);
davide.aliprandi@st.com 3:a132aa6d66e4 1143
davide.aliprandi@st.com 3:a132aa6d66e4 1144 Stspin220_SetDeviceParamsOtherValues();
davide.aliprandi@st.com 3:a132aa6d66e4 1145
davide.aliprandi@st.com 3:a132aa6d66e4 1146 /* Set predefined step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 1147 /* Standby-reset deactivation included to latch the MODEX inputs */
davide.aliprandi@st.com 3:a132aa6d66e4 1148 Stspin220_SetStepMode(pInitDevicePrm->stepMode);
davide.aliprandi@st.com 3:a132aa6d66e4 1149 }
davide.aliprandi@st.com 3:a132aa6d66e4 1150
davide.aliprandi@st.com 3:a132aa6d66e4 1151 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1152 * @brief Sets the parameters of the device to predefined values
davide.aliprandi@st.com 3:a132aa6d66e4 1153 * from stspin220_target_config.h
davide.aliprandi@st.com 3:a132aa6d66e4 1154 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1155 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1156 void STSpin220::Stspin220_SetDeviceParamsToPredefinedValues(void)
davide.aliprandi@st.com 3:a132aa6d66e4 1157 {
davide.aliprandi@st.com 3:a132aa6d66e4 1158 devicePrm.motionState = STANDBY;
davide.aliprandi@st.com 3:a132aa6d66e4 1159
davide.aliprandi@st.com 3:a132aa6d66e4 1160 if (Stspin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION);
davide.aliprandi@st.com 3:a132aa6d66e4 1161 if (Stspin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION);
davide.aliprandi@st.com 3:a132aa6d66e4 1162 if (Stspin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED);
davide.aliprandi@st.com 3:a132aa6d66e4 1163 if (Stspin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED);
davide.aliprandi@st.com 3:a132aa6d66e4 1164
davide.aliprandi@st.com 3:a132aa6d66e4 1165 Stspin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY);
davide.aliprandi@st.com 3:a132aa6d66e4 1166 Stspin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1167 Stspin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1168 Stspin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1169 Stspin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1170 devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN;
davide.aliprandi@st.com 3:a132aa6d66e4 1171 devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH;
davide.aliprandi@st.com 3:a132aa6d66e4 1172 Stspin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP);
davide.aliprandi@st.com 3:a132aa6d66e4 1173
davide.aliprandi@st.com 3:a132aa6d66e4 1174 Stspin220_SetDeviceParamsOtherValues();
davide.aliprandi@st.com 3:a132aa6d66e4 1175
davide.aliprandi@st.com 3:a132aa6d66e4 1176 /* Set predefined step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 1177 /* Standby-reset deactivation included to latch the MODEX inputs */
davide.aliprandi@st.com 3:a132aa6d66e4 1178 Stspin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE);
davide.aliprandi@st.com 3:a132aa6d66e4 1179 }
davide.aliprandi@st.com 3:a132aa6d66e4 1180
davide.aliprandi@st.com 3:a132aa6d66e4 1181 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1182 * @brief Set the stepping mode without reset
davide.aliprandi@st.com 3:a132aa6d66e4 1183 * @param[in] stepMode from full step to 1/256 microstep
davide.aliprandi@st.com 3:a132aa6d66e4 1184 * as specified in enum motorStepMode_t
davide.aliprandi@st.com 3:a132aa6d66e4 1185 * @retval true if the command is successfully executed, else false
davide.aliprandi@st.com 3:a132aa6d66e4 1186 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1187 bool STSpin220::Stspin220_SetStepModeWithoutReset(motorStepMode_t stepMode)
davide.aliprandi@st.com 3:a132aa6d66e4 1188 {
davide.aliprandi@st.com 3:a132aa6d66e4 1189 /* Store step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 1190 devicePrm.stepMode = stepMode;
davide.aliprandi@st.com 3:a132aa6d66e4 1191 devicePrm.stepModeLatched = stepMode;
davide.aliprandi@st.com 3:a132aa6d66e4 1192
davide.aliprandi@st.com 3:a132aa6d66e4 1193 /* Set the mode pins to the levels corresponding to the selected step mode */
davide.aliprandi@st.com 3:a132aa6d66e4 1194 switch (stepMode)
davide.aliprandi@st.com 3:a132aa6d66e4 1195 {
davide.aliprandi@st.com 3:a132aa6d66e4 1196 case STEP_MODE_FULL:
davide.aliprandi@st.com 3:a132aa6d66e4 1197 Stspin220_Board_SetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 1198 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1199 case STEP_MODE_HALF:
davide.aliprandi@st.com 3:a132aa6d66e4 1200 Stspin220_Board_SetModePins(1, 0, 1, 0);
davide.aliprandi@st.com 3:a132aa6d66e4 1201 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1202 case STEP_MODE_1_4:
davide.aliprandi@st.com 3:a132aa6d66e4 1203 Stspin220_Board_SetModePins(0, 1, 0, 1);
davide.aliprandi@st.com 3:a132aa6d66e4 1204 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1205 case STEP_MODE_1_8:
davide.aliprandi@st.com 3:a132aa6d66e4 1206 Stspin220_Board_SetModePins(1, 1, 1, 0);
davide.aliprandi@st.com 3:a132aa6d66e4 1207 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1208 case STEP_MODE_1_16:
davide.aliprandi@st.com 3:a132aa6d66e4 1209 Stspin220_Board_SetModePins(1, 1, 1, 1);
davide.aliprandi@st.com 3:a132aa6d66e4 1210 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1211 case STEP_MODE_1_32:
davide.aliprandi@st.com 3:a132aa6d66e4 1212 Stspin220_Board_SetModePins(0, 0, 0, 1);
davide.aliprandi@st.com 3:a132aa6d66e4 1213 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1214 case STEP_MODE_1_64:
davide.aliprandi@st.com 3:a132aa6d66e4 1215 Stspin220_Board_SetModePins(1, 1, 0, 1);
davide.aliprandi@st.com 3:a132aa6d66e4 1216 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1217 case STEP_MODE_1_128:
davide.aliprandi@st.com 3:a132aa6d66e4 1218 Stspin220_Board_SetModePins(0, 0, 1, 0);
davide.aliprandi@st.com 3:a132aa6d66e4 1219 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1220 case STEP_MODE_1_256:
davide.aliprandi@st.com 3:a132aa6d66e4 1221 Stspin220_Board_SetModePins(1, 1, 0, 0);
davide.aliprandi@st.com 3:a132aa6d66e4 1222 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1223 default:
davide.aliprandi@st.com 3:a132aa6d66e4 1224 return false;
davide.aliprandi@st.com 3:a132aa6d66e4 1225 }
davide.aliprandi@st.com 3:a132aa6d66e4 1226
davide.aliprandi@st.com 3:a132aa6d66e4 1227 /* Wait */
davide.aliprandi@st.com 3:a132aa6d66e4 1228 Stspin220_Board_Delay(SELECT_STEP_MODE_DELAY);
davide.aliprandi@st.com 3:a132aa6d66e4 1229
davide.aliprandi@st.com 3:a132aa6d66e4 1230 /* Exit standby, selected step mode is latched */
davide.aliprandi@st.com 3:a132aa6d66e4 1231 Stspin220_Board_ReleaseReset();
davide.aliprandi@st.com 3:a132aa6d66e4 1232
davide.aliprandi@st.com 3:a132aa6d66e4 1233 /* Let a delay after reset release and step mode latching*/
davide.aliprandi@st.com 3:a132aa6d66e4 1234 Stspin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
davide.aliprandi@st.com 3:a132aa6d66e4 1235
davide.aliprandi@st.com 3:a132aa6d66e4 1236 /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */
davide.aliprandi@st.com 3:a132aa6d66e4 1237 /* after the device quit the standby condition */
davide.aliprandi@st.com 3:a132aa6d66e4 1238 if (stepMode!=(motorStepMode_t)STEP_MODE_FULL)
davide.aliprandi@st.com 3:a132aa6d66e4 1239 {
davide.aliprandi@st.com 3:a132aa6d66e4 1240 Stspin220_Board_UnsetFullStep();
davide.aliprandi@st.com 3:a132aa6d66e4 1241 }
davide.aliprandi@st.com 3:a132aa6d66e4 1242
davide.aliprandi@st.com 3:a132aa6d66e4 1243 return true;
davide.aliprandi@st.com 3:a132aa6d66e4 1244 }
davide.aliprandi@st.com 3:a132aa6d66e4 1245
davide.aliprandi@st.com 3:a132aa6d66e4 1246 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1247 * @brief Initialises the bridge parameters to start the movement
davide.aliprandi@st.com 3:a132aa6d66e4 1248 * and enable the power bridge
davide.aliprandi@st.com 3:a132aa6d66e4 1249 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1250 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1251 void STSpin220::Stspin220_StartMovement(void)
davide.aliprandi@st.com 3:a132aa6d66e4 1252 {
davide.aliprandi@st.com 3:a132aa6d66e4 1253 /* Enable STSpin220 powerstage */
davide.aliprandi@st.com 3:a132aa6d66e4 1254 Stspin220_Enable();
davide.aliprandi@st.com 3:a132aa6d66e4 1255 toggleOdd = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1256 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1257 devicePrm.relativePos = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1258 Stspin220_Board_TimStckInit(true);
davide.aliprandi@st.com 3:a132aa6d66e4 1259 if ((devicePrm.endAccPos == 0)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 1260 (devicePrm.commandExecuted != RUN_CMD))
davide.aliprandi@st.com 3:a132aa6d66e4 1261 {
davide.aliprandi@st.com 3:a132aa6d66e4 1262 devicePrm.motionState = DECELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1263 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1264 }
davide.aliprandi@st.com 3:a132aa6d66e4 1265 else
davide.aliprandi@st.com 3:a132aa6d66e4 1266 {
davide.aliprandi@st.com 3:a132aa6d66e4 1267 devicePrm.motionState = ACCELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1268 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1269 }
davide.aliprandi@st.com 3:a132aa6d66e4 1270 /* Program the step clock */
davide.aliprandi@st.com 3:a132aa6d66e4 1271 Stspin220_ApplySpeed(devicePrm.minSpeed);
davide.aliprandi@st.com 3:a132aa6d66e4 1272 }
davide.aliprandi@st.com 3:a132aa6d66e4 1273
davide.aliprandi@st.com 3:a132aa6d66e4 1274 /******************************************************//**
davide.aliprandi@st.com 3:a132aa6d66e4 1275 * @brief Handles the device state machine at each pulse
davide.aliprandi@st.com 3:a132aa6d66e4 1276 * @retval None
davide.aliprandi@st.com 3:a132aa6d66e4 1277 * @note Must only be called by the timer ISR
davide.aliprandi@st.com 3:a132aa6d66e4 1278 **********************************************************/
davide.aliprandi@st.com 3:a132aa6d66e4 1279 void STSpin220::Stspin220_StepClockHandler(void)
davide.aliprandi@st.com 3:a132aa6d66e4 1280 {
davide.aliprandi@st.com 3:a132aa6d66e4 1281 uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode;
davide.aliprandi@st.com 3:a132aa6d66e4 1282 uint16_t tmp;
davide.aliprandi@st.com 3:a132aa6d66e4 1283 Stspin220_Board_Monitor_Set();
davide.aliprandi@st.com 3:a132aa6d66e4 1284 if (devicePrm.motionState == STANDBYTOINACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 1285 {
davide.aliprandi@st.com 3:a132aa6d66e4 1286 if (toggleOdd != 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1287 {
davide.aliprandi@st.com 3:a132aa6d66e4 1288 Stspin220_Board_StckMode3_Reset();
davide.aliprandi@st.com 3:a132aa6d66e4 1289 toggleOdd = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1290 if (devicePrm.sequencerPosition == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1291 {
davide.aliprandi@st.com 3:a132aa6d66e4 1292 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1293 {
davide.aliprandi@st.com 3:a132aa6d66e4 1294 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
davide.aliprandi@st.com 3:a132aa6d66e4 1295 }
davide.aliprandi@st.com 3:a132aa6d66e4 1296 return;
davide.aliprandi@st.com 3:a132aa6d66e4 1297 }
davide.aliprandi@st.com 3:a132aa6d66e4 1298 }
davide.aliprandi@st.com 3:a132aa6d66e4 1299 else
davide.aliprandi@st.com 3:a132aa6d66e4 1300 {
davide.aliprandi@st.com 3:a132aa6d66e4 1301 Stspin220_Board_StckMode3_Set();
davide.aliprandi@st.com 3:a132aa6d66e4 1302 toggleOdd = 1;
davide.aliprandi@st.com 3:a132aa6d66e4 1303 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
davide.aliprandi@st.com 3:a132aa6d66e4 1304 devicePrm.sequencerPosition -= tmp;
davide.aliprandi@st.com 3:a132aa6d66e4 1305 }
davide.aliprandi@st.com 3:a132aa6d66e4 1306 Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ);
davide.aliprandi@st.com 3:a132aa6d66e4 1307 return;
davide.aliprandi@st.com 3:a132aa6d66e4 1308 }
davide.aliprandi@st.com 3:a132aa6d66e4 1309
davide.aliprandi@st.com 3:a132aa6d66e4 1310 if (toggleOdd == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1311 {
davide.aliprandi@st.com 3:a132aa6d66e4 1312 Stspin220_Board_StckMode3_Set();
davide.aliprandi@st.com 3:a132aa6d66e4 1313 toggleOdd = 1;
davide.aliprandi@st.com 3:a132aa6d66e4 1314 }
davide.aliprandi@st.com 3:a132aa6d66e4 1315 else
davide.aliprandi@st.com 3:a132aa6d66e4 1316 {
davide.aliprandi@st.com 3:a132aa6d66e4 1317 Stspin220_Board_StckMode3_Reset();
davide.aliprandi@st.com 3:a132aa6d66e4 1318 toggleOdd = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1319 /* Incrementation of the relative position */
davide.aliprandi@st.com 3:a132aa6d66e4 1320 devicePrm.relativePos += (1 << stepModeShift);
davide.aliprandi@st.com 3:a132aa6d66e4 1321
davide.aliprandi@st.com 3:a132aa6d66e4 1322 /* Incrementation of the current position */
davide.aliprandi@st.com 3:a132aa6d66e4 1323 if (devicePrm.direction != BACKWARD)
davide.aliprandi@st.com 3:a132aa6d66e4 1324 {
davide.aliprandi@st.com 3:a132aa6d66e4 1325 devicePrm.currentPosition += (1 << stepModeShift);
davide.aliprandi@st.com 3:a132aa6d66e4 1326 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
davide.aliprandi@st.com 3:a132aa6d66e4 1327 devicePrm.sequencerPosition += tmp;
davide.aliprandi@st.com 3:a132aa6d66e4 1328 if (devicePrm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
davide.aliprandi@st.com 3:a132aa6d66e4 1329 {
davide.aliprandi@st.com 3:a132aa6d66e4 1330 devicePrm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
davide.aliprandi@st.com 3:a132aa6d66e4 1331 }
davide.aliprandi@st.com 3:a132aa6d66e4 1332 }
davide.aliprandi@st.com 3:a132aa6d66e4 1333 else
davide.aliprandi@st.com 3:a132aa6d66e4 1334 {
davide.aliprandi@st.com 3:a132aa6d66e4 1335 devicePrm.currentPosition -= (1 << stepModeShift);
davide.aliprandi@st.com 3:a132aa6d66e4 1336 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode));
davide.aliprandi@st.com 3:a132aa6d66e4 1337 devicePrm.sequencerPosition -= tmp;
davide.aliprandi@st.com 3:a132aa6d66e4 1338 if (devicePrm.sequencerPosition < 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1339 {
davide.aliprandi@st.com 3:a132aa6d66e4 1340 devicePrm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
davide.aliprandi@st.com 3:a132aa6d66e4 1341 }
davide.aliprandi@st.com 3:a132aa6d66e4 1342 }
davide.aliprandi@st.com 3:a132aa6d66e4 1343
davide.aliprandi@st.com 3:a132aa6d66e4 1344 switch (devicePrm.motionState)
davide.aliprandi@st.com 3:a132aa6d66e4 1345 {
davide.aliprandi@st.com 3:a132aa6d66e4 1346 case ACCELERATING:
davide.aliprandi@st.com 3:a132aa6d66e4 1347 {
davide.aliprandi@st.com 3:a132aa6d66e4 1348 uint32_t relPos = devicePrm.relativePos;
davide.aliprandi@st.com 3:a132aa6d66e4 1349 uint32_t endAccPos = devicePrm.endAccPos;
davide.aliprandi@st.com 3:a132aa6d66e4 1350 uint16_t speed = devicePrm.speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1351 uint32_t acc = ((uint32_t)devicePrm.acceleration << 16)>>stepModeShift;
davide.aliprandi@st.com 3:a132aa6d66e4 1352
davide.aliprandi@st.com 3:a132aa6d66e4 1353 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
davide.aliprandi@st.com 3:a132aa6d66e4 1354 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos)))
davide.aliprandi@st.com 3:a132aa6d66e4 1355 {
davide.aliprandi@st.com 3:a132aa6d66e4 1356 devicePrm.motionState = DECELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1357 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1358 /* Apply decelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1359 Stspin220_ApplyTorque(DEC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1360 }
davide.aliprandi@st.com 3:a132aa6d66e4 1361 else if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\
davide.aliprandi@st.com 3:a132aa6d66e4 1362 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
davide.aliprandi@st.com 3:a132aa6d66e4 1363 {
davide.aliprandi@st.com 3:a132aa6d66e4 1364 devicePrm.motionState = STEADY;
davide.aliprandi@st.com 3:a132aa6d66e4 1365 /* Apply running torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1366 Stspin220_ApplyTorque(RUN_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1367 }
davide.aliprandi@st.com 3:a132aa6d66e4 1368 else
davide.aliprandi@st.com 3:a132aa6d66e4 1369 {
davide.aliprandi@st.com 3:a132aa6d66e4 1370 bool speedUpdated = false;
davide.aliprandi@st.com 3:a132aa6d66e4 1371 /* Go on accelerating */
davide.aliprandi@st.com 3:a132aa6d66e4 1372 if (speed==0) speed =1;
davide.aliprandi@st.com 3:a132aa6d66e4 1373 devicePrm.accu += acc / speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1374 while (devicePrm.accu>=(0X10000L))
davide.aliprandi@st.com 3:a132aa6d66e4 1375 {
davide.aliprandi@st.com 3:a132aa6d66e4 1376 devicePrm.accu -= (0X10000L);
davide.aliprandi@st.com 3:a132aa6d66e4 1377 speed +=1;
davide.aliprandi@st.com 3:a132aa6d66e4 1378 speedUpdated = true;
davide.aliprandi@st.com 3:a132aa6d66e4 1379 }
davide.aliprandi@st.com 3:a132aa6d66e4 1380
davide.aliprandi@st.com 3:a132aa6d66e4 1381 if (speedUpdated)
davide.aliprandi@st.com 3:a132aa6d66e4 1382 {
davide.aliprandi@st.com 3:a132aa6d66e4 1383 if (speed>(devicePrm.maxSpeed>>stepModeShift))
davide.aliprandi@st.com 3:a132aa6d66e4 1384 {
davide.aliprandi@st.com 3:a132aa6d66e4 1385 speed = devicePrm.maxSpeed>>stepModeShift;
davide.aliprandi@st.com 3:a132aa6d66e4 1386 }
davide.aliprandi@st.com 3:a132aa6d66e4 1387 devicePrm.speed = speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1388 }
davide.aliprandi@st.com 3:a132aa6d66e4 1389
davide.aliprandi@st.com 3:a132aa6d66e4 1390 if (devicePrm.updateTorque!=false)
davide.aliprandi@st.com 3:a132aa6d66e4 1391 {
davide.aliprandi@st.com 3:a132aa6d66e4 1392 /* Apply accelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1393 Stspin220_ApplyTorque(ACC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1394 }
davide.aliprandi@st.com 3:a132aa6d66e4 1395 }
davide.aliprandi@st.com 3:a132aa6d66e4 1396 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1397 }
davide.aliprandi@st.com 3:a132aa6d66e4 1398 case STEADY:
davide.aliprandi@st.com 3:a132aa6d66e4 1399 {
davide.aliprandi@st.com 3:a132aa6d66e4 1400 uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift;
davide.aliprandi@st.com 3:a132aa6d66e4 1401 uint32_t relativePos = devicePrm.relativePos;
davide.aliprandi@st.com 3:a132aa6d66e4 1402 if (devicePrm.updateTorque!=false)
davide.aliprandi@st.com 3:a132aa6d66e4 1403 {
davide.aliprandi@st.com 3:a132aa6d66e4 1404 /* Apply accelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1405 Stspin220_ApplyTorque(RUN_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1406 }
davide.aliprandi@st.com 3:a132aa6d66e4 1407 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\
davide.aliprandi@st.com 3:a132aa6d66e4 1408 ((devicePrm.commandExecuted==MOVE_CMD)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 1409 (relativePos>=(devicePrm.startDecPos)))||\
davide.aliprandi@st.com 3:a132aa6d66e4 1410 ((devicePrm.commandExecuted==RUN_CMD)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 1411 (devicePrm.speed>maxSpeed)))
davide.aliprandi@st.com 3:a132aa6d66e4 1412 {
davide.aliprandi@st.com 3:a132aa6d66e4 1413 devicePrm.motionState = DECELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1414 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1415 /* Apply decelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1416 Stspin220_ApplyTorque(DEC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1417 }
davide.aliprandi@st.com 3:a132aa6d66e4 1418 else if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed))
davide.aliprandi@st.com 3:a132aa6d66e4 1419 {
davide.aliprandi@st.com 3:a132aa6d66e4 1420 devicePrm.motionState = ACCELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1421 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1422 /* Apply accelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1423 Stspin220_ApplyTorque(ACC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1424 }
davide.aliprandi@st.com 3:a132aa6d66e4 1425 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1426 }
davide.aliprandi@st.com 3:a132aa6d66e4 1427 case DECELERATING:
davide.aliprandi@st.com 3:a132aa6d66e4 1428 {
davide.aliprandi@st.com 3:a132aa6d66e4 1429 uint32_t relativePos = devicePrm.relativePos;
davide.aliprandi@st.com 3:a132aa6d66e4 1430 uint16_t speed = devicePrm.speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1431 uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift;
davide.aliprandi@st.com 3:a132aa6d66e4 1432 if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\
davide.aliprandi@st.com 3:a132aa6d66e4 1433 (speed<=(devicePrm.minSpeed>>stepModeShift)))||\
davide.aliprandi@st.com 3:a132aa6d66e4 1434 ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake)))
davide.aliprandi@st.com 3:a132aa6d66e4 1435 {
davide.aliprandi@st.com 3:a132aa6d66e4 1436 /* Motion process complete */
davide.aliprandi@st.com 3:a132aa6d66e4 1437 if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0)
davide.aliprandi@st.com 3:a132aa6d66e4 1438 {
davide.aliprandi@st.com 3:a132aa6d66e4 1439 devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted);
davide.aliprandi@st.com 3:a132aa6d66e4 1440 if (devicePrm.direction==BACKWARD) devicePrm.direction=FORWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 1441 else devicePrm.direction=BACKWARD;
davide.aliprandi@st.com 3:a132aa6d66e4 1442 Stspin220_Board_SetDirectionGpio(devicePrm.direction);
davide.aliprandi@st.com 3:a132aa6d66e4 1443 if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0)
davide.aliprandi@st.com 3:a132aa6d66e4 1444 {
davide.aliprandi@st.com 3:a132aa6d66e4 1445 devicePrm.motionState = ACCELERATING;
davide.aliprandi@st.com 3:a132aa6d66e4 1446 devicePrm.accu = 0;
davide.aliprandi@st.com 3:a132aa6d66e4 1447 /* Apply accelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1448 Stspin220_ApplyTorque(ACC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1449 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1450 }
davide.aliprandi@st.com 3:a132aa6d66e4 1451 }
davide.aliprandi@st.com 3:a132aa6d66e4 1452 if (devicePrm.stopMode==HOLD_MODE)
davide.aliprandi@st.com 3:a132aa6d66e4 1453 {
davide.aliprandi@st.com 3:a132aa6d66e4 1454 Stspin220_HardStop();
davide.aliprandi@st.com 3:a132aa6d66e4 1455 }
davide.aliprandi@st.com 3:a132aa6d66e4 1456 else if (devicePrm.stopMode==STANDBY_MODE)
davide.aliprandi@st.com 3:a132aa6d66e4 1457 {
davide.aliprandi@st.com 3:a132aa6d66e4 1458 Stspin220_PutDeviceInStandby();
davide.aliprandi@st.com 3:a132aa6d66e4 1459 }
davide.aliprandi@st.com 3:a132aa6d66e4 1460 else
davide.aliprandi@st.com 3:a132aa6d66e4 1461 {
davide.aliprandi@st.com 3:a132aa6d66e4 1462 Stspin220_HardHiZ();
davide.aliprandi@st.com 3:a132aa6d66e4 1463 }
davide.aliprandi@st.com 3:a132aa6d66e4 1464 }
davide.aliprandi@st.com 3:a132aa6d66e4 1465 else if ((devicePrm.commandExecuted==RUN_CMD)&&
davide.aliprandi@st.com 3:a132aa6d66e4 1466 (speed<=(devicePrm.maxSpeed>>stepModeShift)))
davide.aliprandi@st.com 3:a132aa6d66e4 1467 {
davide.aliprandi@st.com 3:a132aa6d66e4 1468 devicePrm.motionState = STEADY;
davide.aliprandi@st.com 3:a132aa6d66e4 1469 /* Apply running torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1470 Stspin220_ApplyTorque(RUN_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1471 }
davide.aliprandi@st.com 3:a132aa6d66e4 1472 else
davide.aliprandi@st.com 3:a132aa6d66e4 1473 {
davide.aliprandi@st.com 3:a132aa6d66e4 1474 /* Go on decelerating */
davide.aliprandi@st.com 3:a132aa6d66e4 1475 if (speed>(devicePrm.minSpeed>>stepModeShift))
davide.aliprandi@st.com 3:a132aa6d66e4 1476 {
davide.aliprandi@st.com 3:a132aa6d66e4 1477 bool speedUpdated = false;
davide.aliprandi@st.com 3:a132aa6d66e4 1478 if (speed==0) speed =1;
davide.aliprandi@st.com 3:a132aa6d66e4 1479 devicePrm.accu += dec / speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1480 while (devicePrm.accu>=(0X10000L))
davide.aliprandi@st.com 3:a132aa6d66e4 1481 {
davide.aliprandi@st.com 3:a132aa6d66e4 1482 devicePrm.accu -= (0X10000L);
davide.aliprandi@st.com 3:a132aa6d66e4 1483 if (speed>1)
davide.aliprandi@st.com 3:a132aa6d66e4 1484 {
davide.aliprandi@st.com 3:a132aa6d66e4 1485 speed -=1;
davide.aliprandi@st.com 3:a132aa6d66e4 1486 }
davide.aliprandi@st.com 3:a132aa6d66e4 1487 speedUpdated = true;
davide.aliprandi@st.com 3:a132aa6d66e4 1488 }
davide.aliprandi@st.com 3:a132aa6d66e4 1489
davide.aliprandi@st.com 3:a132aa6d66e4 1490 if (speedUpdated)
davide.aliprandi@st.com 3:a132aa6d66e4 1491 {
davide.aliprandi@st.com 3:a132aa6d66e4 1492 if (speed<(devicePrm.minSpeed>>stepModeShift))
davide.aliprandi@st.com 3:a132aa6d66e4 1493 {
davide.aliprandi@st.com 3:a132aa6d66e4 1494 speed = devicePrm.minSpeed>>stepModeShift;
davide.aliprandi@st.com 3:a132aa6d66e4 1495 }
davide.aliprandi@st.com 3:a132aa6d66e4 1496 devicePrm.speed = speed;
davide.aliprandi@st.com 3:a132aa6d66e4 1497 }
davide.aliprandi@st.com 3:a132aa6d66e4 1498
davide.aliprandi@st.com 3:a132aa6d66e4 1499 if (devicePrm.updateTorque!=false)
davide.aliprandi@st.com 3:a132aa6d66e4 1500 {
davide.aliprandi@st.com 3:a132aa6d66e4 1501 /* Apply decelerating torque */
davide.aliprandi@st.com 3:a132aa6d66e4 1502 Stspin220_ApplyTorque(DEC_TORQUE);
davide.aliprandi@st.com 3:a132aa6d66e4 1503 }
davide.aliprandi@st.com 3:a132aa6d66e4 1504 }
davide.aliprandi@st.com 3:a132aa6d66e4 1505 }
davide.aliprandi@st.com 3:a132aa6d66e4 1506 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1507 }
davide.aliprandi@st.com 3:a132aa6d66e4 1508 default:
davide.aliprandi@st.com 3:a132aa6d66e4 1509 {
davide.aliprandi@st.com 3:a132aa6d66e4 1510 break;
davide.aliprandi@st.com 3:a132aa6d66e4 1511 }
davide.aliprandi@st.com 3:a132aa6d66e4 1512 }
davide.aliprandi@st.com 3:a132aa6d66e4 1513 }
davide.aliprandi@st.com 3:a132aa6d66e4 1514 if ((devicePrm.motionState & INACTIVE) != INACTIVE)
davide.aliprandi@st.com 3:a132aa6d66e4 1515 {
davide.aliprandi@st.com 3:a132aa6d66e4 1516 Stspin220_ApplySpeed(devicePrm.speed);
davide.aliprandi@st.com 3:a132aa6d66e4 1517 }
davide.aliprandi@st.com 3:a132aa6d66e4 1518 else
davide.aliprandi@st.com 3:a132aa6d66e4 1519 {
davide.aliprandi@st.com 3:a132aa6d66e4 1520 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0)
davide.aliprandi@st.com 3:a132aa6d66e4 1521 {
davide.aliprandi@st.com 3:a132aa6d66e4 1522 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK);
davide.aliprandi@st.com 3:a132aa6d66e4 1523 }
davide.aliprandi@st.com 3:a132aa6d66e4 1524 }
davide.aliprandi@st.com 3:a132aa6d66e4 1525 Stspin220_Board_Monitor_Reset();
davide.aliprandi@st.com 3:a132aa6d66e4 1526 }
davide.aliprandi@st.com 3:a132aa6d66e4 1527
davide.aliprandi@st.com 3:a132aa6d66e4 1528 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/