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