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
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.
Components/STSpin220/STSpin220.cpp@4:265c30b9112a, 2017-03-24 (annotated)
- Committer:
- Davidroid
- Date:
- Fri Mar 24 13:54:23 2017 +0000
- Revision:
- 4:265c30b9112a
- Parent:
- Components/stspin220/STSpin220.cpp@3:a132aa6d66e4
- Child:
- 5:fd1315beea32
Typos corrected.
Who changed what in which revision?
| User | Revision | Line number | New 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>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 13 | * |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 14 | * Redistribution and use in source and binary forms, with or without modification, |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 15 | * are permitted provided that the following conditions are met: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 16 | * 1. Redistributions of source code must retain the above copyright notice, |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 17 | * this list of conditions and the following disclaimer. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 19 | * this list of conditions and the following disclaimer in the documentation |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 20 | * and/or other materials provided with the distribution. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 21 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 22 | * may be used to endorse or promote products derived from this software |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 23 | * without specific prior written permission. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 24 | * |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 35 | * |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 36 | ****************************************************************************** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 37 | */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 38 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 39 | /* Includes ------------------------------------------------------------------*/ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 40 | #include "STSpin220.h" |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 41 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 42 | /* Definitions ---------------------------------------------------------------*/ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 43 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 44 | /* Variables ----------------------------------------------------------------*/ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 45 | /* Number of devices. */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 46 | uint8_t STSpin220::numberOfDevices = 0; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 47 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 48 | /* Methods -------------------------------------------------------------------*/ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 49 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 50 | * @brief Start the STSpin220 library |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 51 | * @param[in] pInit pointer to the initialization data |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 52 | * @retval COMPONENT_OK in case of success. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 53 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 54 | status_t STSpin220::STSpin220_Init(void* pInit) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 55 | { |
| Davidroid | 4:265c30b9112a | 56 | STSpin220_Disable(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 57 | if (pInit == NULL) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 58 | { |
| Davidroid | 4:265c30b9112a | 59 | /* Set context variables to the predefined values from STSpin220_target_config.h */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 60 | /* Set GPIO according to these values */ |
| Davidroid | 4:265c30b9112a | 61 | STSpin220_SetDeviceParamsToPredefinedValues(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 62 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 63 | else |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 64 | { |
| Davidroid | 4:265c30b9112a | 65 | STSpin220_SetDeviceParamsToGivenValues((STSpin220_init_t*) pInit); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 66 | } |
| Davidroid | 4:265c30b9112a | 67 | STSpin220_Board_TimStckInit(false); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 68 | return COMPONENT_OK; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 69 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 70 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 71 | /********************************************************** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 72 | * @brief Read id |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 73 | * @param id pointer to the identifier to be read. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 74 | * @retval COMPONENT_OK in case of success. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 75 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 76 | status_t STSpin220::STSpin220_ReadID(uint8_t *id) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 77 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 78 | *id = deviceInstance; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 79 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 80 | return COMPONENT_OK; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 81 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 82 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 83 | /********************************************************** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 84 | * @brief Attaches a user callback to the error Handler. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 85 | * The call back will be then called each time the library |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 86 | * detects an error |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 87 | * @param[in] callback Name of the callback to attach |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 88 | * to the error Hanlder |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 89 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 90 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 91 | void STSpin220::STSpin220_AttachErrorHandler(void (*callback)(uint16_t error)) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 92 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 93 | errorHandlerCallback = (void (*)(uint16_t error)) callback; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 94 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 95 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 96 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 97 | * @brief Apply the set torque |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 98 | * @param[in] torqueMode torque mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 99 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 100 | * @note |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 101 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 102 | void STSpin220::STSpin220_ApplyTorque(motorTorqueMode_t torqueMode) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 103 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 104 | uint8_t torqueValue = 0; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 105 | devicePrm.updateTorque = false; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 106 | switch(torqueMode) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 107 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 108 | case ACC_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 109 | devicePrm.currentTorque = devicePrm.accelTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 110 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 111 | case DEC_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 112 | devicePrm.currentTorque = devicePrm.decelTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 113 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 114 | case RUN_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 115 | devicePrm.currentTorque = devicePrm.runTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 116 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 117 | case HOLD_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 118 | devicePrm.currentTorque = devicePrm.holdTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 119 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 120 | case CURRENT_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 121 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 122 | default: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 123 | return; //ignore error |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 124 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 125 | torqueValue = devicePrm.currentTorque; |
| Davidroid | 4:265c30b9112a | 126 | STSpin220_Board_PwmRefSetDutyCycle(torqueValue); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 127 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 128 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 129 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 130 | * @brief Disable the power bridges (leave the output bridges HiZ) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 131 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 132 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 133 | void STSpin220::STSpin220_Disable(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 134 | { |
| Davidroid | 4:265c30b9112a | 135 | STSpin220_Board_Disable(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 136 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 137 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 138 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 139 | * @brief Enable the power bridges |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 140 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 141 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 142 | void STSpin220::STSpin220_Enable(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 143 | { |
| Davidroid | 4:265c30b9112a | 144 | STSpin220_Board_Enable(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 145 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 146 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 147 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 148 | * @brief Error handler which calls the user callback (if defined) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 149 | * @param[in] error Number of the error |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 150 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 151 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 152 | void STSpin220::STSpin220_ErrorHandler(uint16_t error) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 153 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 154 | if (errorHandlerCallback != 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 155 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 156 | errorHandlerCallback(error); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 157 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 158 | else |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 159 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 160 | while(1) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 161 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 162 | /* Infinite loop */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 163 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 164 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 165 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 166 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 167 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 168 | * @brief Exit STSpin220 device from standby (low power consumption) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 169 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 170 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 171 | void STSpin220::STSpin220_ExitDeviceFromStandby(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 172 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 173 | uint32_t sequencerPosition = devicePrm.sequencerPosition; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 174 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 175 | /* Exit standby and set step mode */ |
| Davidroid | 4:265c30b9112a | 176 | STSpin220_SetStepModeWithoutReset(devicePrm.stepMode); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 177 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 178 | if (devicePrm.sequencerPosition != 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 179 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 180 | /* Set direction to FORWARD to ensure the HW sequencer is increased at */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 181 | /* each step clock rising edge */ |
| Davidroid | 4:265c30b9112a | 182 | STSpin220_SetDirection(FORWARD); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 183 | /* Going out of standby */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 184 | devicePrm.motionState = STANDBYTOINACTIVE; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 185 | /* Program the step clock */ |
| Davidroid | 4:265c30b9112a | 186 | STSpin220_Board_TimStckInit(true); |
| Davidroid | 4:265c30b9112a | 187 | STSpin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 188 | toggleOdd = 0; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 189 | while (devicePrm.sequencerPosition != 0); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 190 | while (toggleOdd!=0); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 191 | devicePrm.sequencerPosition = sequencerPosition; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 192 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 193 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 194 | devicePrm.motionState = INACTIVE; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 195 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 196 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 197 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 198 | * @brief Return the acceleration |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 199 | * @retval Acceleration in pps^2 |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 200 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 201 | uint16_t STSpin220::STSpin220_GetAcceleration(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 202 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 203 | return (devicePrm.acceleration); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 204 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 205 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 206 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 207 | * @brief Return the current speed |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 208 | * @retval Speed in pps |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 209 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 210 | uint16_t STSpin220::STSpin220_GetCurrentSpeed(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 211 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 212 | return devicePrm.speed; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 213 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 214 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 215 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 216 | * @brief Return the deceleration |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 217 | * @retval Deceleration in pps^2 |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 218 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 219 | uint16_t STSpin220::STSpin220_GetDeceleration(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 220 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 221 | return (devicePrm.deceleration); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 222 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 223 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 224 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 225 | * @brief Return the device state |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 226 | * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 227 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 228 | motorState_t STSpin220::STSpin220_get_device_state(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 229 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 230 | return devicePrm.motionState; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 231 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 232 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 233 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 234 | * @brief Get the motor current direction |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 235 | * @retval direction |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 236 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 237 | motorDir_t STSpin220::STSpin220_GetDirection(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 238 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 239 | return devicePrm.direction; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 240 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 241 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 242 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 243 | * @brief Return the FW version. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 244 | * @retval FW version |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 245 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 246 | uint32_t STSpin220::STSpin220_GetFwVersion(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 247 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 248 | return (STSPIN220_FW_VERSION); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 249 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 250 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 251 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 252 | * @brief Get the mark position (32b signed) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 253 | * @retval mark position |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 254 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 255 | int32_t STSpin220::STSpin220_GetMark(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 256 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 257 | return devicePrm.markPosition; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 258 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 259 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 260 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 261 | * @brief Return the max speed |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 262 | * @retval maxSpeed in pps |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 263 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 264 | uint16_t STSpin220::STSpin220_GetMaxSpeed(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 265 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 266 | return (devicePrm.maxSpeed); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 267 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 268 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 269 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 270 | * @brief Get the min speed |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 271 | * in step/s for full, half and wave modes |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 272 | * in microsteps/s for microstep modes |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 273 | * @retval return the min speed in step/s or microstep/s |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 274 | * @note |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 275 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 276 | uint16_t STSpin220::STSpin220_GetMinSpeed(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 277 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 278 | return (devicePrm.minSpeed); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 279 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 280 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 281 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 282 | * @brief Get the current position (32b signed) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 283 | * @retval current position value |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 284 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 285 | int32_t STSpin220::STSpin220_GetPosition(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 286 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 287 | return devicePrm.currentPosition; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 288 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 289 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 290 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 291 | * @brief Get the motor step mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 292 | * @retval step mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 293 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 294 | motorStepMode_t STSpin220::STSpin220_GetStepMode(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 295 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 296 | return devicePrm.stepMode; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 297 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 298 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 299 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 300 | * @brief Get the selected stop mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 301 | * @retval the selected stop mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 302 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 303 | motorStopMode_t STSpin220::STSpin220_GetStopMode(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 304 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 305 | return devicePrm.stopMode; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 306 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 307 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 308 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 309 | * @brief Get the torque |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 310 | * @param[in] torqueMode torque mode |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 311 | * @retval the torqueValue in % (from 0 to 100) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 312 | * @note |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 313 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 314 | uint8_t STSpin220::STSpin220_GetTorque(motorTorqueMode_t torqueMode) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 315 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 316 | uint8_t torqueValue = 0; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 317 | switch(torqueMode) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 318 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 319 | case ACC_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 320 | torqueValue = devicePrm.accelTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 321 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 322 | case DEC_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 323 | torqueValue = devicePrm.decelTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 324 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 325 | case RUN_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 326 | torqueValue = devicePrm.runTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 327 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 328 | case HOLD_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 329 | torqueValue = devicePrm.holdTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 330 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 331 | case CURRENT_TORQUE: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 332 | torqueValue = devicePrm.currentTorque; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 333 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 334 | default: |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 335 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 336 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 337 | return torqueValue; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 338 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 339 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 340 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 341 | * @brief Get the torque boost feature status |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 342 | * @retval TRUE if enabled, FALSE if disabled |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 343 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 344 | bool STSpin220::STSpin220_GetTorqueBoostEnable(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 345 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 346 | return devicePrm.torqueBoostEnable; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 347 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 348 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 349 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 350 | * @brief Get the torque boost threshold |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 351 | * @retval The torque boost threshold above which the step mode is |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 352 | * changed to full step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 353 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 354 | uint16_t STSpin220::STSpin220_GetTorqueBoostThreshold(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 355 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 356 | return devicePrm.torqueBoostSpeedThreshold; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 357 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 358 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 359 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 360 | * @brief Go to the home position |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 361 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 362 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 363 | void STSpin220::STSpin220_GoHome(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 364 | { |
| Davidroid | 4:265c30b9112a | 365 | STSpin220_GoTo(0); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 366 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 367 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 368 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 369 | * @brief Go to the Mark position |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 370 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 371 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 372 | void STSpin220::STSpin220_GoMark(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 373 | { |
| Davidroid | 4:265c30b9112a | 374 | STSpin220_GoTo(devicePrm.markPosition); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 375 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 376 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 377 | /******************************************************//** |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 378 | * @brief Request the motor to move to the specified position |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 379 | * @param[in] targetPosition absolute position in steps |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 380 | * @retval None |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 381 | * @note The position is at the resolution corresponding to the |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 382 | * selected step mode. |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 383 | * STEP_MODE_FULL : step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 384 | * STEP_MODE_HALF : 1/2 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 385 | * STEP_MODE_1_4 : 1/4 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 386 | * STEP_MODE_1_8 : 1/8 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 387 | * STEP_MODE_1_16 : 1/16 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 388 | * STEP_MODE_1_32 : 1/32 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 389 | * STEP_MODE_1_64 : 1/64 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 390 | * STEP_MODE_1_128 : 1/128 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 391 | * STEP_MODE_1_256 : 1/256 step |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 392 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 393 | void STSpin220::STSpin220_GoTo(int32_t targetPosition) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 394 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 395 | motorDir_t direction; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 396 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 397 | /* Exit from standby if needed */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 398 | if (devicePrm.motionState == STANDBY) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 399 | { |
| Davidroid | 4:265c30b9112a | 400 | STSpin220_ExitDeviceFromStandby(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 401 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 402 | /* Deactivate motor if needed */ |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 403 | else if (devicePrm.motionState != INACTIVE) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 404 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 444 | STSpin220_SetDirection(direction); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 445 | |
| Davidroid | 4:265c30b9112a | 446 | STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 447 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 448 | /* Motor activation */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 517 | STSpin220_SetDirection(direction); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 518 | |
| Davidroid | 4:265c30b9112a | 519 | STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 520 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 521 | /* Motor activation */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 536 | if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 537 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 546 | STSpin220_SetTorque(CURRENT_TORQUE, 0); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 547 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 548 | /* Disable power bridges */ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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, |
| Davidroid | 4:265c30b9112a | 566 | * or call STSpin220_HardHiz function when stop mode is HIZ_MODE, |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 578 | if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 579 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 632 | STSpin220_SetDirection(direction); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 633 | |
| Davidroid | 4:265c30b9112a | 634 | STSpin220_ComputeSpeedProfile(stepCount); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 635 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 636 | /* Motor activation */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 645 | void STSpin220::STSpin220_PutDeviceInStandby(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 646 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 647 | /* Stop movement */ |
| Davidroid | 4:265c30b9112a | 648 | STSpin220_HardHiZ(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 649 | |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 650 | /* Enter standby */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 676 | STSpin220_SetDirection(direction); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 677 | devicePrm.commandExecuted = RUN_CMD; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 678 | /* Motor activation */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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)&&\ |
| Davidroid | 4:265c30b9112a | 777 | ((newMaxSpeed>>STSpin220_GetStepMode())<= 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 954 | while (((STSpin220_get_device_state()&INACTIVE)!=INACTIVE)||\ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | |
| Davidroid | 4:265c30b9112a | 1130 | if (STSpin220_SetAcceleration(pInitDevicePrm->acceleration)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); |
| Davidroid | 4:265c30b9112a | 1131 | if (STSpin220_SetDeceleration(pInitDevicePrm->deceleration)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); |
| Davidroid | 4:265c30b9112a | 1132 | if (STSpin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); |
| Davidroid | 4:265c30b9112a | 1133 | if (STSpin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1134 | |
| Davidroid | 4:265c30b9112a | 1135 | STSpin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq); |
| Davidroid | 4:265c30b9112a | 1136 | STSpin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque); |
| Davidroid | 4:265c30b9112a | 1137 | STSpin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque); |
| Davidroid | 4:265c30b9112a | 1138 | STSpin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque); |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 1142 | STSpin220_SetStopMode(pInitDevicePrm->stopMode); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1143 | |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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 | |
| Davidroid | 4:265c30b9112a | 1160 | if (STSpin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); |
| Davidroid | 4:265c30b9112a | 1161 | if (STSpin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); |
| Davidroid | 4:265c30b9112a | 1162 | if (STSpin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); |
| Davidroid | 4:265c30b9112a | 1163 | if (STSpin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1164 | |
| Davidroid | 4:265c30b9112a | 1165 | STSpin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY); |
| Davidroid | 4:265c30b9112a | 1166 | STSpin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE); |
| Davidroid | 4:265c30b9112a | 1167 | STSpin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE); |
| Davidroid | 4:265c30b9112a | 1168 | STSpin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE); |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 1172 | STSpin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1173 | |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 1197 | STSpin220_Board_SetFullStep(); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1198 | break; |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1199 | case STEP_MODE_HALF: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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: |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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*/ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 1251 | void STSpin220::STSpin220_StartMovement(void) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1252 | { |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1253 | /* Enable STSpin220 powerstage */ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | **********************************************************/ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 1292 | if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1293 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | } |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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; |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 */ |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 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 | { |
| Davidroid | 4:265c30b9112a | 1520 | if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1521 | { |
| Davidroid | 4:265c30b9112a | 1522 | STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1523 | } |
| davide.aliprandi@st.com | 3:a132aa6d66e4 | 1524 | } |
| Davidroid | 4:265c30b9112a | 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****/ |

X-NUCLEO-IHM06A1 Low Voltage Stepper Motor Driver