Personal fork of the library for direct control instead of library control
Dependents: Thesis_Rotating_Platform
Fork of X_NUCLEO_IHM01A1 by
Components/l6474/l6474_class.cpp@25:9933118b7fc7, 2017-05-24 (annotated)
- Committer:
- Arkadi
- Date:
- Wed May 24 10:01:11 2017 +0000
- Revision:
- 25:9933118b7fc7
- Parent:
- 23:58264db10a17
Library changes to disable stepper control, only for initialization
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Davidroid | 0:2887415a46cd | 1 | /** |
Davidroid | 0:2887415a46cd | 2 | ****************************************************************************** |
Davidroid | 0:2887415a46cd | 3 | * @file l6474_class.cpp |
Davidroid | 0:2887415a46cd | 4 | * @author IPC Rennes |
Davidroid | 0:2887415a46cd | 5 | * @version V1.5.0 |
Davidroid | 0:2887415a46cd | 6 | * @date November 12, 2014 |
Davidroid | 0:2887415a46cd | 7 | * @brief L6474 driver (fully integrated microstepping motor driver) |
Davidroid | 0:2887415a46cd | 8 | * @note (C) COPYRIGHT 2014 STMicroelectronics |
Davidroid | 0:2887415a46cd | 9 | ****************************************************************************** |
Davidroid | 0:2887415a46cd | 10 | * @attention |
Davidroid | 0:2887415a46cd | 11 | * |
Davidroid | 0:2887415a46cd | 12 | * <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2> |
Davidroid | 0:2887415a46cd | 13 | * |
Davidroid | 0:2887415a46cd | 14 | * Redistribution and use in source and binary forms, with or without modification, |
Davidroid | 0:2887415a46cd | 15 | * are permitted provided that the following conditions are met: |
Davidroid | 0:2887415a46cd | 16 | * 1. Redistributions of source code must retain the above copyright notice, |
Davidroid | 0:2887415a46cd | 17 | * this list of conditions and the following disclaimer. |
Davidroid | 0:2887415a46cd | 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
Davidroid | 0:2887415a46cd | 19 | * this list of conditions and the following disclaimer in the documentation |
Davidroid | 0:2887415a46cd | 20 | * and/or other materials provided with the distribution. |
Davidroid | 0:2887415a46cd | 21 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
Davidroid | 0:2887415a46cd | 22 | * may be used to endorse or promote products derived from this software |
Davidroid | 0:2887415a46cd | 23 | * without specific prior written permission. |
Davidroid | 0:2887415a46cd | 24 | * |
Davidroid | 0:2887415a46cd | 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
Davidroid | 0:2887415a46cd | 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
Davidroid | 0:2887415a46cd | 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
Davidroid | 0:2887415a46cd | 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
Davidroid | 0:2887415a46cd | 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
Davidroid | 0:2887415a46cd | 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
Davidroid | 0:2887415a46cd | 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
Davidroid | 0:2887415a46cd | 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
Davidroid | 0:2887415a46cd | 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
Davidroid | 0:2887415a46cd | 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
Davidroid | 0:2887415a46cd | 35 | * |
Davidroid | 0:2887415a46cd | 36 | ****************************************************************************** |
Davidroid | 0:2887415a46cd | 37 | */ |
Davidroid | 0:2887415a46cd | 38 | |
Davidroid | 0:2887415a46cd | 39 | |
Davidroid | 23:58264db10a17 | 40 | /* Generated with STM32CubeTOO -----------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 41 | |
Davidroid | 0:2887415a46cd | 42 | |
Davidroid | 0:2887415a46cd | 43 | /* Revision ------------------------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 44 | /* |
Davidroid | 0:2887415a46cd | 45 | Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev |
Davidroid | 0:2887415a46cd | 46 | Branch/Trunk/Tag: trunk |
Davidroid | 0:2887415a46cd | 47 | Based on: X-CUBE-SPN1/trunk/Drivers/BSP/Components/l6474/l6474.c |
Davidroid | 14:73823a4344be | 48 | Revision: 0 |
Davidroid | 0:2887415a46cd | 49 | */ |
Davidroid | 0:2887415a46cd | 50 | |
Davidroid | 0:2887415a46cd | 51 | |
Davidroid | 0:2887415a46cd | 52 | /* Includes ------------------------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 53 | |
Davidroid | 0:2887415a46cd | 54 | #include "l6474_class.h" |
Davidroid | 0:2887415a46cd | 55 | |
Davidroid | 1:b38ebb8ea286 | 56 | |
Davidroid | 15:40470df81d9a | 57 | /* Definitions ---------------------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 58 | |
Davidroid | 15:40470df81d9a | 59 | /* Error while initialising the SPI. */ |
Davidroid | 5:d3c78f12a78d | 60 | #define L6474_ERROR_0 (0x8000) |
Davidroid | 15:40470df81d9a | 61 | |
Davidroid | 15:40470df81d9a | 62 | /* Error of bad SPI transaction. */ |
Davidroid | 5:d3c78f12a78d | 63 | #define L6474_ERROR_1 (0x8001) |
Davidroid | 0:2887415a46cd | 64 | |
Davidroid | 15:40470df81d9a | 65 | /* Maximum number of steps. */ |
Davidroid | 5:d3c78f12a78d | 66 | #define MAX_STEPS (0x7FFFFFFF) |
Davidroid | 0:2887415a46cd | 67 | |
Davidroid | 15:40470df81d9a | 68 | /* Maximum frequency of the PWMs in Hz. */ |
Davidroid | 0:2887415a46cd | 69 | #define L6474_MAX_PWM_FREQ (10000) |
Davidroid | 0:2887415a46cd | 70 | |
Davidroid | 15:40470df81d9a | 71 | /* Minimum frequency of the PWMs in Hz. */ |
Davidroid | 0:2887415a46cd | 72 | #define L6474_MIN_PWM_FREQ (2) |
Davidroid | 0:2887415a46cd | 73 | |
Davidroid | 0:2887415a46cd | 74 | |
Davidroid | 15:40470df81d9a | 75 | /* Variables ----------------------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 76 | |
Davidroid | 0:2887415a46cd | 77 | /* Number of devices. */ |
Davidroid | 18:2d6ab2b93685 | 78 | uint8_t L6474::number_of_devices = 0; |
Davidroid | 0:2887415a46cd | 79 | |
Davidroid | 6:a47569fc7534 | 80 | /* ISR flags used to restart an interrupted SPI transfer when an error is reported. */ |
Davidroid | 18:2d6ab2b93685 | 81 | bool L6474::spi_preemtion_by_isr = FALSE; |
Davidroid | 18:2d6ab2b93685 | 82 | bool L6474::isr_flag = FALSE; |
Davidroid | 6:a47569fc7534 | 83 | |
Davidroid | 5:d3c78f12a78d | 84 | /* SPI Transmission for Daisy-Chain Configuration. */ |
Davidroid | 18:2d6ab2b93685 | 85 | uint8_t L6474::spi_tx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; |
Davidroid | 18:2d6ab2b93685 | 86 | uint8_t L6474::spi_rx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; |
Davidroid | 5:d3c78f12a78d | 87 | |
Davidroid | 0:2887415a46cd | 88 | |
Davidroid | 1:b38ebb8ea286 | 89 | /* Methods -------------------------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 90 | |
Davidroid | 5:d3c78f12a78d | 91 | /********************************************************** |
Davidroid | 0:2887415a46cd | 92 | * @brief Attaches a user callback to the error Handler. |
Davidroid | 0:2887415a46cd | 93 | * The call back will be then called each time the library |
Davidroid | 0:2887415a46cd | 94 | * detects an error |
Davidroid | 0:2887415a46cd | 95 | * @param[in] callback Name of the callback to attach |
Davidroid | 0:2887415a46cd | 96 | * to the error Hanlder |
Davidroid | 0:2887415a46cd | 97 | * @retval None |
Davidroid | 0:2887415a46cd | 98 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 99 | void L6474::L6474_AttachErrorHandler(void (*callback)(uint16_t error)) |
Davidroid | 0:2887415a46cd | 100 | { |
Davidroid | 18:2d6ab2b93685 | 101 | error_handler_callback = (void (*)(uint16_t error)) callback; |
Davidroid | 0:2887415a46cd | 102 | } |
Davidroid | 0:2887415a46cd | 103 | |
Davidroid | 5:d3c78f12a78d | 104 | /********************************************************** |
Davidroid | 0:2887415a46cd | 105 | * @brief Starts the L6474 library |
Davidroid | 18:2d6ab2b93685 | 106 | * @param init Initialization structure. |
Davidroid | 18:2d6ab2b93685 | 107 | * @retval COMPONENT_OK in case of success. |
Davidroid | 0:2887415a46cd | 108 | **********************************************************/ |
Davidroid | 22:ed3a6990a6eb | 109 | Status_t L6474::L6474_Init(void *init) |
Davidroid | 0:2887415a46cd | 110 | { |
Davidroid | 0:2887415a46cd | 111 | /* Initialise the PWMs used for the Step clocks ----------------------------*/ |
Arkadi | 25:9933118b7fc7 | 112 | |
Arkadi | 25:9933118b7fc7 | 113 | ///////////////////////////////////////////////////////// !!!!!!!!!!!!!!!!!!!!!!!!!!! |
Arkadi | 25:9933118b7fc7 | 114 | // Disable Library control |
Arkadi | 25:9933118b7fc7 | 115 | //L6474_PwmInit(); |
Arkadi | 25:9933118b7fc7 | 116 | // Disable Library control |
Arkadi | 25:9933118b7fc7 | 117 | ////////////////////////////////////////////////////////// !!!!!!!!!!!!!!!!!!!!!!!!!! |
Arkadi | 25:9933118b7fc7 | 118 | |
Davidroid | 0:2887415a46cd | 119 | /* Initialise the L6474s ------------------------------------------------*/ |
Davidroid | 0:2887415a46cd | 120 | |
Davidroid | 0:2887415a46cd | 121 | /* Standby-reset deactivation */ |
Davidroid | 0:2887415a46cd | 122 | L6474_ReleaseReset(); |
Davidroid | 0:2887415a46cd | 123 | |
Davidroid | 0:2887415a46cd | 124 | /* Let a delay after reset */ |
Arkadi | 25:9933118b7fc7 | 125 | L6474_Delay(0.25); // original 1sec delay |
Davidroid | 0:2887415a46cd | 126 | |
Davidroid | 18:2d6ab2b93685 | 127 | /* Set device parameters to the predefined values from "l6474_target_config.h". */ |
Davidroid | 0:2887415a46cd | 128 | L6474_SetDeviceParamsToPredefinedValues(); |
Davidroid | 0:2887415a46cd | 129 | |
Davidroid | 18:2d6ab2b93685 | 130 | if (init == NULL) |
Davidroid | 18:2d6ab2b93685 | 131 | /* Set device registers to the predefined values from "l6474_target_config.h". */ |
Davidroid | 18:2d6ab2b93685 | 132 | L6474_SetRegisterToPredefinedValues(); |
Davidroid | 18:2d6ab2b93685 | 133 | else |
Davidroid | 18:2d6ab2b93685 | 134 | /* Set device registers to the passed initialization values. */ |
Davidroid | 22:ed3a6990a6eb | 135 | L6474_SetRegisterToInitializationValues((L6474_Init_t *) init); |
Davidroid | 18:2d6ab2b93685 | 136 | |
Davidroid | 0:2887415a46cd | 137 | /* Disable L6474 powerstage */ |
Davidroid | 0:2887415a46cd | 138 | L6474_CmdDisable(); |
Davidroid | 0:2887415a46cd | 139 | |
Davidroid | 0:2887415a46cd | 140 | /* Get Status to clear flags after start up */ |
Davidroid | 0:2887415a46cd | 141 | L6474_CmdGetStatus(); |
Davidroid | 0:2887415a46cd | 142 | |
Davidroid | 0:2887415a46cd | 143 | return COMPONENT_OK; |
Davidroid | 0:2887415a46cd | 144 | } |
Davidroid | 0:2887415a46cd | 145 | |
Davidroid | 5:d3c78f12a78d | 146 | /********************************************************** |
Davidroid | 0:2887415a46cd | 147 | * @brief Read id |
Davidroid | 0:2887415a46cd | 148 | * @param id pointer to the identifier to be read. |
Davidroid | 18:2d6ab2b93685 | 149 | * @retval COMPONENT_OK in case of success. |
Davidroid | 0:2887415a46cd | 150 | **********************************************************/ |
Davidroid | 22:ed3a6990a6eb | 151 | Status_t L6474::L6474_ReadID(uint8_t *id) |
Davidroid | 0:2887415a46cd | 152 | { |
Davidroid | 18:2d6ab2b93685 | 153 | *id = device_instance; |
Davidroid | 0:2887415a46cd | 154 | |
Davidroid | 0:2887415a46cd | 155 | return COMPONENT_OK; |
Davidroid | 0:2887415a46cd | 156 | } |
Davidroid | 0:2887415a46cd | 157 | |
Davidroid | 5:d3c78f12a78d | 158 | /********************************************************** |
Davidroid | 0:2887415a46cd | 159 | * @brief Returns the acceleration of the specified device |
Davidroid | 0:2887415a46cd | 160 | * @retval Acceleration in pps^2 |
Davidroid | 0:2887415a46cd | 161 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 162 | uint16_t L6474::L6474_GetAcceleration(void) |
Davidroid | 0:2887415a46cd | 163 | { |
Davidroid | 18:2d6ab2b93685 | 164 | return (device_prm.acceleration); |
Davidroid | 0:2887415a46cd | 165 | } |
Davidroid | 0:2887415a46cd | 166 | |
Davidroid | 5:d3c78f12a78d | 167 | /********************************************************** |
Davidroid | 0:2887415a46cd | 168 | * @brief Returns the current speed of the specified device |
Davidroid | 0:2887415a46cd | 169 | * @retval Speed in pps |
Davidroid | 0:2887415a46cd | 170 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 171 | uint16_t L6474::L6474_GetCurrentSpeed(void) |
Davidroid | 0:2887415a46cd | 172 | { |
Davidroid | 18:2d6ab2b93685 | 173 | return device_prm.speed; |
Davidroid | 0:2887415a46cd | 174 | } |
Davidroid | 0:2887415a46cd | 175 | |
Davidroid | 5:d3c78f12a78d | 176 | /********************************************************** |
Davidroid | 0:2887415a46cd | 177 | * @brief Returns the deceleration of the specified device |
Davidroid | 0:2887415a46cd | 178 | * @retval Deceleration in pps^2 |
Davidroid | 0:2887415a46cd | 179 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 180 | uint16_t L6474::L6474_GetDeceleration(void) |
Davidroid | 0:2887415a46cd | 181 | { |
Davidroid | 18:2d6ab2b93685 | 182 | return (device_prm.deceleration); |
Davidroid | 0:2887415a46cd | 183 | } |
Davidroid | 0:2887415a46cd | 184 | |
Davidroid | 5:d3c78f12a78d | 185 | /********************************************************** |
Davidroid | 0:2887415a46cd | 186 | * @brief Returns the device state |
Davidroid | 0:2887415a46cd | 187 | * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) |
Davidroid | 0:2887415a46cd | 188 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 189 | motorState_t L6474::L6474_GetDeviceState(void) |
Davidroid | 0:2887415a46cd | 190 | { |
Davidroid | 18:2d6ab2b93685 | 191 | return device_prm.motionState; |
Davidroid | 0:2887415a46cd | 192 | } |
Davidroid | 0:2887415a46cd | 193 | |
Davidroid | 5:d3c78f12a78d | 194 | /********************************************************** |
Davidroid | 0:2887415a46cd | 195 | * @brief Returns the FW version of the library |
Davidroid | 0:2887415a46cd | 196 | * @param None |
Davidroid | 0:2887415a46cd | 197 | * @retval L6474_FW_VERSION |
Davidroid | 0:2887415a46cd | 198 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 199 | uint8_t L6474::L6474_GetFwVersion(void) |
Davidroid | 0:2887415a46cd | 200 | { |
Davidroid | 0:2887415a46cd | 201 | return (L6474_FW_VERSION); |
Davidroid | 0:2887415a46cd | 202 | } |
Davidroid | 0:2887415a46cd | 203 | |
Davidroid | 5:d3c78f12a78d | 204 | /********************************************************** |
Davidroid | 0:2887415a46cd | 205 | * @brief Returns the mark position of the specified device |
Davidroid | 0:2887415a46cd | 206 | * @retval Mark register value converted in a 32b signed integer |
Davidroid | 0:2887415a46cd | 207 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 208 | int32_t L6474::L6474_GetMark(void) |
Davidroid | 0:2887415a46cd | 209 | { |
Davidroid | 0:2887415a46cd | 210 | return L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); |
Davidroid | 0:2887415a46cd | 211 | } |
Davidroid | 0:2887415a46cd | 212 | |
Davidroid | 5:d3c78f12a78d | 213 | /********************************************************** |
Davidroid | 0:2887415a46cd | 214 | * @brief Returns the max speed of the specified device |
Davidroid | 0:2887415a46cd | 215 | * @retval maxSpeed in pps |
Davidroid | 0:2887415a46cd | 216 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 217 | uint16_t L6474::L6474_GetMaxSpeed(void) |
Davidroid | 0:2887415a46cd | 218 | { |
Davidroid | 18:2d6ab2b93685 | 219 | return (device_prm.maxSpeed); |
Davidroid | 0:2887415a46cd | 220 | } |
Davidroid | 0:2887415a46cd | 221 | |
Davidroid | 5:d3c78f12a78d | 222 | /********************************************************** |
Davidroid | 0:2887415a46cd | 223 | * @brief Returns the min speed of the specified device |
Davidroid | 0:2887415a46cd | 224 | * @retval minSpeed in pps |
Davidroid | 0:2887415a46cd | 225 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 226 | uint16_t L6474::L6474_GetMinSpeed(void) |
Davidroid | 0:2887415a46cd | 227 | { |
Davidroid | 18:2d6ab2b93685 | 228 | return (device_prm.minSpeed); |
Davidroid | 0:2887415a46cd | 229 | } |
Davidroid | 0:2887415a46cd | 230 | |
Davidroid | 5:d3c78f12a78d | 231 | /********************************************************** |
Davidroid | 0:2887415a46cd | 232 | * @brief Returns the ABS_POSITION of the specified device |
Davidroid | 0:2887415a46cd | 233 | * @retval ABS_POSITION register value converted in a 32b signed integer |
Davidroid | 0:2887415a46cd | 234 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 235 | int32_t L6474::L6474_GetPosition(void) |
Davidroid | 0:2887415a46cd | 236 | { |
Davidroid | 0:2887415a46cd | 237 | return L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); |
Davidroid | 0:2887415a46cd | 238 | } |
Davidroid | 0:2887415a46cd | 239 | |
Davidroid | 5:d3c78f12a78d | 240 | /********************************************************** |
Davidroid | 0:2887415a46cd | 241 | * @brief Requests the motor to move to the home position (ABS_POSITION = 0) |
Davidroid | 0:2887415a46cd | 242 | * @retval None |
Davidroid | 0:2887415a46cd | 243 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 244 | void L6474::L6474_GoHome(void) |
Davidroid | 0:2887415a46cd | 245 | { |
Davidroid | 0:2887415a46cd | 246 | L6474_GoTo(0); |
Davidroid | 0:2887415a46cd | 247 | } |
Davidroid | 0:2887415a46cd | 248 | |
Davidroid | 5:d3c78f12a78d | 249 | /********************************************************** |
Davidroid | 0:2887415a46cd | 250 | * @brief Requests the motor to move to the mark position |
Davidroid | 0:2887415a46cd | 251 | * @retval None |
Davidroid | 0:2887415a46cd | 252 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 253 | void L6474::L6474_GoMark(void) |
Davidroid | 0:2887415a46cd | 254 | { |
Davidroid | 0:2887415a46cd | 255 | uint32_t mark; |
Davidroid | 0:2887415a46cd | 256 | |
Davidroid | 0:2887415a46cd | 257 | mark = L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); |
Davidroid | 0:2887415a46cd | 258 | L6474_GoTo(mark); |
Davidroid | 0:2887415a46cd | 259 | } |
Davidroid | 0:2887415a46cd | 260 | |
Davidroid | 5:d3c78f12a78d | 261 | /********************************************************** |
Davidroid | 0:2887415a46cd | 262 | * @brief Requests the motor to move to the specified position |
Davidroid | 0:2887415a46cd | 263 | * @param[in] targetPosition absolute position in steps |
Davidroid | 0:2887415a46cd | 264 | * @retval None |
Davidroid | 0:2887415a46cd | 265 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 266 | void L6474::L6474_GoTo(int32_t targetPosition) |
Davidroid | 0:2887415a46cd | 267 | { |
Davidroid | 5:d3c78f12a78d | 268 | motorDir_t direction; |
Davidroid | 0:2887415a46cd | 269 | int32_t steps; |
Davidroid | 0:2887415a46cd | 270 | |
Davidroid | 0:2887415a46cd | 271 | /* Eventually deactivate motor */ |
Davidroid | 18:2d6ab2b93685 | 272 | if (device_prm.motionState != INACTIVE) |
Davidroid | 0:2887415a46cd | 273 | { |
Davidroid | 0:2887415a46cd | 274 | L6474_HardStop(); |
Davidroid | 0:2887415a46cd | 275 | } |
Davidroid | 0:2887415a46cd | 276 | |
Davidroid | 0:2887415a46cd | 277 | /* Get current position */ |
Davidroid | 18:2d6ab2b93685 | 278 | device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); |
Davidroid | 0:2887415a46cd | 279 | |
Davidroid | 0:2887415a46cd | 280 | /* Compute the number of steps to perform */ |
Davidroid | 18:2d6ab2b93685 | 281 | steps = targetPosition - device_prm.currentPosition; |
Davidroid | 0:2887415a46cd | 282 | |
Davidroid | 0:2887415a46cd | 283 | if (steps >= 0) |
Davidroid | 0:2887415a46cd | 284 | { |
Davidroid | 18:2d6ab2b93685 | 285 | device_prm.stepsToTake = steps; |
Davidroid | 5:d3c78f12a78d | 286 | direction = FORWARD; |
Davidroid | 0:2887415a46cd | 287 | } |
Davidroid | 0:2887415a46cd | 288 | else |
Davidroid | 0:2887415a46cd | 289 | { |
Davidroid | 18:2d6ab2b93685 | 290 | device_prm.stepsToTake = -steps; |
Davidroid | 5:d3c78f12a78d | 291 | direction = BACKWARD; |
Davidroid | 0:2887415a46cd | 292 | } |
Davidroid | 0:2887415a46cd | 293 | |
Davidroid | 0:2887415a46cd | 294 | if (steps != 0) |
Davidroid | 0:2887415a46cd | 295 | { |
Davidroid | 18:2d6ab2b93685 | 296 | device_prm.commandExecuted = MOVE_CMD; |
Davidroid | 0:2887415a46cd | 297 | |
Davidroid | 0:2887415a46cd | 298 | /* Direction setup */ |
Davidroid | 0:2887415a46cd | 299 | L6474_SetDirection(direction); |
Davidroid | 0:2887415a46cd | 300 | |
Davidroid | 18:2d6ab2b93685 | 301 | L6474_ComputeSpeedProfile(device_prm.stepsToTake); |
Davidroid | 0:2887415a46cd | 302 | |
Davidroid | 0:2887415a46cd | 303 | /* Motor activation */ |
Davidroid | 0:2887415a46cd | 304 | L6474_StartMovement(); |
Davidroid | 0:2887415a46cd | 305 | } |
Davidroid | 0:2887415a46cd | 306 | } |
Davidroid | 0:2887415a46cd | 307 | |
Davidroid | 5:d3c78f12a78d | 308 | /********************************************************** |
Davidroid | 0:2887415a46cd | 309 | * @brief Immediatly stops the motor and disable the power bridge |
Davidroid | 0:2887415a46cd | 310 | * @retval None |
Davidroid | 0:2887415a46cd | 311 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 312 | void L6474::L6474_HardStop(void) |
Davidroid | 0:2887415a46cd | 313 | { |
Davidroid | 0:2887415a46cd | 314 | /* Disable corresponding PWM */ |
Davidroid | 0:2887415a46cd | 315 | L6474_PwmStop(); |
Davidroid | 0:2887415a46cd | 316 | |
Davidroid | 0:2887415a46cd | 317 | /* Set inactive state */ |
Davidroid | 18:2d6ab2b93685 | 318 | device_prm.motionState = INACTIVE; |
Davidroid | 18:2d6ab2b93685 | 319 | device_prm.commandExecuted = NO_CMD; |
Davidroid | 18:2d6ab2b93685 | 320 | device_prm.stepsToTake = MAX_STEPS; |
Davidroid | 0:2887415a46cd | 321 | } |
Davidroid | 0:2887415a46cd | 322 | |
Davidroid | 5:d3c78f12a78d | 323 | /********************************************************** |
Davidroid | 0:2887415a46cd | 324 | * @brief Moves the motor of the specified number of steps |
Davidroid | 0:2887415a46cd | 325 | * @param[in] direction FORWARD or BACKWARD |
Davidroid | 0:2887415a46cd | 326 | * @param[in] stepCount Number of steps to perform |
Davidroid | 0:2887415a46cd | 327 | * @retval None |
Davidroid | 0:2887415a46cd | 328 | **********************************************************/ |
Davidroid | 5:d3c78f12a78d | 329 | void L6474::L6474_Move(motorDir_t direction, uint32_t stepCount) |
Davidroid | 0:2887415a46cd | 330 | { |
Davidroid | 0:2887415a46cd | 331 | /* Eventually deactivate motor */ |
Davidroid | 18:2d6ab2b93685 | 332 | if (device_prm.motionState != INACTIVE) |
Davidroid | 0:2887415a46cd | 333 | { |
Davidroid | 0:2887415a46cd | 334 | L6474_HardStop(); |
Davidroid | 0:2887415a46cd | 335 | } |
Davidroid | 0:2887415a46cd | 336 | |
Davidroid | 0:2887415a46cd | 337 | if (stepCount != 0) |
Davidroid | 0:2887415a46cd | 338 | { |
Davidroid | 18:2d6ab2b93685 | 339 | device_prm.stepsToTake = stepCount; |
Davidroid | 0:2887415a46cd | 340 | |
Davidroid | 18:2d6ab2b93685 | 341 | device_prm.commandExecuted = MOVE_CMD; |
Davidroid | 0:2887415a46cd | 342 | |
Davidroid | 18:2d6ab2b93685 | 343 | device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); |
Davidroid | 0:2887415a46cd | 344 | |
Davidroid | 0:2887415a46cd | 345 | /* Direction setup */ |
Davidroid | 0:2887415a46cd | 346 | L6474_SetDirection(direction); |
Davidroid | 0:2887415a46cd | 347 | |
Davidroid | 0:2887415a46cd | 348 | L6474_ComputeSpeedProfile(stepCount); |
Davidroid | 0:2887415a46cd | 349 | |
Davidroid | 0:2887415a46cd | 350 | /* Motor activation */ |
Davidroid | 0:2887415a46cd | 351 | L6474_StartMovement(); |
Davidroid | 0:2887415a46cd | 352 | } |
Davidroid | 0:2887415a46cd | 353 | } |
Davidroid | 0:2887415a46cd | 354 | |
Davidroid | 5:d3c78f12a78d | 355 | /********************************************************** |
Davidroid | 0:2887415a46cd | 356 | * @brief Runs the motor. It will accelerate from the min |
Davidroid | 0:2887415a46cd | 357 | * speed up to the max speed by using the device acceleration. |
Davidroid | 0:2887415a46cd | 358 | * @param[in] direction FORWARD or BACKWARD |
Davidroid | 0:2887415a46cd | 359 | * @retval None |
Davidroid | 0:2887415a46cd | 360 | **********************************************************/ |
Davidroid | 5:d3c78f12a78d | 361 | void L6474::L6474_Run(motorDir_t direction) |
Davidroid | 0:2887415a46cd | 362 | { |
Davidroid | 0:2887415a46cd | 363 | /* Eventually deactivate motor */ |
Davidroid | 18:2d6ab2b93685 | 364 | if (device_prm.motionState != INACTIVE) |
Davidroid | 0:2887415a46cd | 365 | { |
Davidroid | 0:2887415a46cd | 366 | L6474_HardStop(); |
Davidroid | 0:2887415a46cd | 367 | } |
Davidroid | 0:2887415a46cd | 368 | |
Davidroid | 0:2887415a46cd | 369 | /* Direction setup */ |
Davidroid | 0:2887415a46cd | 370 | L6474_SetDirection(direction); |
Davidroid | 0:2887415a46cd | 371 | |
Davidroid | 18:2d6ab2b93685 | 372 | device_prm.commandExecuted = RUN_CMD; |
Davidroid | 0:2887415a46cd | 373 | |
Davidroid | 0:2887415a46cd | 374 | /* Motor activation */ |
Davidroid | 0:2887415a46cd | 375 | L6474_StartMovement(); |
Davidroid | 0:2887415a46cd | 376 | } |
Davidroid | 0:2887415a46cd | 377 | |
Davidroid | 5:d3c78f12a78d | 378 | /********************************************************** |
Davidroid | 0:2887415a46cd | 379 | * @brief Changes the acceleration of the specified device |
Davidroid | 0:2887415a46cd | 380 | * @param[in] newAcc New acceleration to apply in pps^2 |
Davidroid | 0:2887415a46cd | 381 | * @retval true if the command is successfully executed, else false |
Davidroid | 0:2887415a46cd | 382 | * @note The command is not performed is the device is executing |
Davidroid | 0:2887415a46cd | 383 | * a MOVE or GOTO command (but it can be used during a RUN command) |
Davidroid | 0:2887415a46cd | 384 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 385 | bool L6474::L6474_SetAcceleration(uint16_t newAcc) |
Davidroid | 0:2887415a46cd | 386 | { |
Davidroid | 0:2887415a46cd | 387 | bool cmdExecuted = FALSE; |
Davidroid | 0:2887415a46cd | 388 | if ((newAcc != 0)&& |
Davidroid | 18:2d6ab2b93685 | 389 | ((device_prm.motionState == INACTIVE)|| |
Davidroid | 18:2d6ab2b93685 | 390 | (device_prm.commandExecuted == RUN_CMD))) |
Davidroid | 0:2887415a46cd | 391 | { |
Davidroid | 18:2d6ab2b93685 | 392 | device_prm.acceleration = newAcc; |
Davidroid | 0:2887415a46cd | 393 | cmdExecuted = TRUE; |
Davidroid | 0:2887415a46cd | 394 | } |
Davidroid | 0:2887415a46cd | 395 | return cmdExecuted; |
Davidroid | 0:2887415a46cd | 396 | } |
Davidroid | 0:2887415a46cd | 397 | |
Davidroid | 5:d3c78f12a78d | 398 | /********************************************************** |
Davidroid | 0:2887415a46cd | 399 | * @brief Changes the deceleration of the specified device |
Davidroid | 0:2887415a46cd | 400 | * @param[in] newDec New deceleration to apply in pps^2 |
Davidroid | 0:2887415a46cd | 401 | * @retval true if the command is successfully executed, else false |
Davidroid | 0:2887415a46cd | 402 | * @note The command is not performed is the device is executing |
Davidroid | 0:2887415a46cd | 403 | * a MOVE or GOTO command (but it can be used during a RUN command) |
Davidroid | 0:2887415a46cd | 404 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 405 | bool L6474::L6474_SetDeceleration(uint16_t newDec) |
Davidroid | 0:2887415a46cd | 406 | { |
Davidroid | 0:2887415a46cd | 407 | bool cmdExecuted = FALSE; |
Davidroid | 0:2887415a46cd | 408 | if ((newDec != 0)&& |
Davidroid | 18:2d6ab2b93685 | 409 | ((device_prm.motionState == INACTIVE)|| |
Davidroid | 18:2d6ab2b93685 | 410 | (device_prm.commandExecuted == RUN_CMD))) |
Davidroid | 0:2887415a46cd | 411 | { |
Davidroid | 18:2d6ab2b93685 | 412 | device_prm.deceleration = newDec; |
Davidroid | 0:2887415a46cd | 413 | cmdExecuted = TRUE; |
Davidroid | 0:2887415a46cd | 414 | } |
Davidroid | 0:2887415a46cd | 415 | return cmdExecuted; |
Davidroid | 0:2887415a46cd | 416 | } |
Davidroid | 0:2887415a46cd | 417 | |
Davidroid | 5:d3c78f12a78d | 418 | /********************************************************** |
Davidroid | 0:2887415a46cd | 419 | * @brief Set current position to be the Home position (ABS pos set to 0) |
Davidroid | 0:2887415a46cd | 420 | * @retval None |
Davidroid | 0:2887415a46cd | 421 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 422 | void L6474::L6474_SetHome(void) |
Davidroid | 0:2887415a46cd | 423 | { |
Davidroid | 0:2887415a46cd | 424 | L6474_CmdSetParam(L6474_ABS_POS, 0); |
Davidroid | 0:2887415a46cd | 425 | } |
Davidroid | 0:2887415a46cd | 426 | |
Davidroid | 5:d3c78f12a78d | 427 | /********************************************************** |
Davidroid | 0:2887415a46cd | 428 | * @brief Sets current position to be the Mark position |
Davidroid | 0:2887415a46cd | 429 | * @retval None |
Davidroid | 0:2887415a46cd | 430 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 431 | void L6474::L6474_SetMark(void) |
Davidroid | 0:2887415a46cd | 432 | { |
Davidroid | 0:2887415a46cd | 433 | uint32_t mark = L6474_CmdGetParam(L6474_ABS_POS); |
Davidroid | 0:2887415a46cd | 434 | L6474_CmdSetParam(L6474_MARK, mark); |
Davidroid | 0:2887415a46cd | 435 | } |
Davidroid | 0:2887415a46cd | 436 | |
Davidroid | 5:d3c78f12a78d | 437 | /********************************************************** |
Davidroid | 0:2887415a46cd | 438 | * @brief Changes the max speed of the specified device |
Davidroid | 0:2887415a46cd | 439 | * @param[in] newMaxSpeed New max speed to apply in pps |
Davidroid | 0:2887415a46cd | 440 | * @retval true if the command is successfully executed, else false |
Davidroid | 0:2887415a46cd | 441 | * @note The command is not performed is the device is executing |
Davidroid | 0:2887415a46cd | 442 | * a MOVE or GOTO command (but it can be used during a RUN command). |
Davidroid | 0:2887415a46cd | 443 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 444 | bool L6474::L6474_SetMaxSpeed(uint16_t newMaxSpeed) |
Davidroid | 0:2887415a46cd | 445 | { |
Davidroid | 0:2887415a46cd | 446 | bool cmdExecuted = FALSE; |
Davidroid | 0:2887415a46cd | 447 | if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&& |
Davidroid | 0:2887415a46cd | 448 | (newMaxSpeed <= L6474_MAX_PWM_FREQ) && |
Davidroid | 18:2d6ab2b93685 | 449 | (device_prm.minSpeed <= newMaxSpeed) && |
Davidroid | 18:2d6ab2b93685 | 450 | ((device_prm.motionState == INACTIVE)|| |
Davidroid | 18:2d6ab2b93685 | 451 | (device_prm.commandExecuted == RUN_CMD))) |
Davidroid | 0:2887415a46cd | 452 | { |
Davidroid | 18:2d6ab2b93685 | 453 | device_prm.maxSpeed = newMaxSpeed; |
Davidroid | 0:2887415a46cd | 454 | cmdExecuted = TRUE; |
Davidroid | 0:2887415a46cd | 455 | } |
Davidroid | 0:2887415a46cd | 456 | return cmdExecuted; |
Davidroid | 0:2887415a46cd | 457 | } |
Davidroid | 0:2887415a46cd | 458 | |
Davidroid | 5:d3c78f12a78d | 459 | /********************************************************** |
Davidroid | 0:2887415a46cd | 460 | * @brief Changes the min speed of the specified device |
Davidroid | 0:2887415a46cd | 461 | * @param[in] newMinSpeed New min speed to apply in pps |
Davidroid | 0:2887415a46cd | 462 | * @retval true if the command is successfully executed, else false |
Davidroid | 0:2887415a46cd | 463 | * @note The command is not performed is the device is executing |
Davidroid | 0:2887415a46cd | 464 | * a MOVE or GOTO command (but it can be used during a RUN command). |
Davidroid | 0:2887415a46cd | 465 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 466 | bool L6474::L6474_SetMinSpeed(uint16_t newMinSpeed) |
Davidroid | 0:2887415a46cd | 467 | { |
Davidroid | 0:2887415a46cd | 468 | bool cmdExecuted = FALSE; |
Davidroid | 0:2887415a46cd | 469 | if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&& |
Davidroid | 0:2887415a46cd | 470 | (newMinSpeed <= L6474_MAX_PWM_FREQ) && |
Davidroid | 18:2d6ab2b93685 | 471 | (newMinSpeed <= device_prm.maxSpeed) && |
Davidroid | 18:2d6ab2b93685 | 472 | ((device_prm.motionState == INACTIVE)|| |
Davidroid | 18:2d6ab2b93685 | 473 | (device_prm.commandExecuted == RUN_CMD))) |
Davidroid | 0:2887415a46cd | 474 | { |
Davidroid | 18:2d6ab2b93685 | 475 | device_prm.minSpeed = newMinSpeed; |
Davidroid | 0:2887415a46cd | 476 | cmdExecuted = TRUE; |
Davidroid | 0:2887415a46cd | 477 | } |
Davidroid | 0:2887415a46cd | 478 | return cmdExecuted; |
Davidroid | 0:2887415a46cd | 479 | } |
Davidroid | 0:2887415a46cd | 480 | |
Davidroid | 5:d3c78f12a78d | 481 | /********************************************************** |
Davidroid | 0:2887415a46cd | 482 | * @brief Stops the motor by using the device deceleration |
Davidroid | 0:2887415a46cd | 483 | * @retval true if the command is successfully executed, else false |
Davidroid | 0:2887415a46cd | 484 | * @note The command is not performed is the device is in INACTIVE state. |
Davidroid | 0:2887415a46cd | 485 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 486 | bool L6474::L6474_SoftStop(void) |
Davidroid | 0:2887415a46cd | 487 | { |
Davidroid | 0:2887415a46cd | 488 | bool cmdExecuted = FALSE; |
Davidroid | 18:2d6ab2b93685 | 489 | if (device_prm.motionState != INACTIVE) |
Davidroid | 0:2887415a46cd | 490 | { |
Davidroid | 18:2d6ab2b93685 | 491 | device_prm.commandExecuted = SOFT_STOP_CMD; |
Davidroid | 0:2887415a46cd | 492 | cmdExecuted = TRUE; |
Davidroid | 0:2887415a46cd | 493 | } |
Davidroid | 0:2887415a46cd | 494 | return (cmdExecuted); |
Davidroid | 0:2887415a46cd | 495 | } |
Davidroid | 0:2887415a46cd | 496 | |
Davidroid | 5:d3c78f12a78d | 497 | /********************************************************** |
Davidroid | 1:b38ebb8ea286 | 498 | * @brief Locks until the device state becomes Inactive |
Davidroid | 1:b38ebb8ea286 | 499 | * @retval None |
Davidroid | 1:b38ebb8ea286 | 500 | **********************************************************/ |
Davidroid | 1:b38ebb8ea286 | 501 | void L6474::L6474_WaitWhileActive(void) |
Davidroid | 1:b38ebb8ea286 | 502 | { |
Davidroid | 1:b38ebb8ea286 | 503 | /* Wait while motor is running */ |
Davidroid | 1:b38ebb8ea286 | 504 | while (L6474_GetDeviceState() != INACTIVE); |
Davidroid | 1:b38ebb8ea286 | 505 | } |
Davidroid | 1:b38ebb8ea286 | 506 | |
Davidroid | 5:d3c78f12a78d | 507 | /********************************************************** |
Davidroid | 0:2887415a46cd | 508 | * @brief Issue the Disable command to the L6474 of the specified device |
Davidroid | 0:2887415a46cd | 509 | * @retval None |
Davidroid | 0:2887415a46cd | 510 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 511 | void L6474::L6474_CmdDisable(void) |
Davidroid | 0:2887415a46cd | 512 | { |
Davidroid | 0:2887415a46cd | 513 | L6474_SendCommand(L6474_DISABLE); |
Davidroid | 0:2887415a46cd | 514 | } |
Davidroid | 0:2887415a46cd | 515 | |
Davidroid | 5:d3c78f12a78d | 516 | /********************************************************** |
Davidroid | 0:2887415a46cd | 517 | * @brief Issues the Enable command to the L6474 of the specified device |
Davidroid | 0:2887415a46cd | 518 | * @retval None |
Davidroid | 0:2887415a46cd | 519 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 520 | void L6474::L6474_CmdEnable(void) |
Davidroid | 0:2887415a46cd | 521 | { |
Davidroid | 0:2887415a46cd | 522 | L6474_SendCommand(L6474_ENABLE); |
Davidroid | 0:2887415a46cd | 523 | } |
Davidroid | 0:2887415a46cd | 524 | |
Davidroid | 5:d3c78f12a78d | 525 | /********************************************************** |
Davidroid | 0:2887415a46cd | 526 | * @brief Issues the GetParam command to the L6474 of the specified device |
Davidroid | 7:f7e0c3621f77 | 527 | * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...) |
Davidroid | 0:2887415a46cd | 528 | * @retval Register value |
Davidroid | 0:2887415a46cd | 529 | **********************************************************/ |
Davidroid | 7:f7e0c3621f77 | 530 | uint32_t L6474::L6474_CmdGetParam(L6474_Registers_t parameter) |
Davidroid | 0:2887415a46cd | 531 | { |
Davidroid | 0:2887415a46cd | 532 | uint32_t i; |
Davidroid | 0:2887415a46cd | 533 | uint32_t spiRxData; |
Davidroid | 0:2887415a46cd | 534 | uint8_t maxArgumentNbBytes = 0; |
Davidroid | 18:2d6ab2b93685 | 535 | uint8_t spiIndex = number_of_devices - device_instance - 1; |
Davidroid | 0:2887415a46cd | 536 | bool itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 537 | |
Davidroid | 0:2887415a46cd | 538 | do |
Davidroid | 0:2887415a46cd | 539 | { |
Davidroid | 18:2d6ab2b93685 | 540 | spi_preemtion_by_isr = FALSE; |
Davidroid | 0:2887415a46cd | 541 | if (itDisable) |
Davidroid | 0:2887415a46cd | 542 | { |
Davidroid | 0:2887415a46cd | 543 | /* re-enable L6474_EnableIrq if disable in previous iteration */ |
Davidroid | 0:2887415a46cd | 544 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 545 | itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 546 | } |
Davidroid | 0:2887415a46cd | 547 | |
Davidroid | 18:2d6ab2b93685 | 548 | for (i = 0; i < number_of_devices; i++) |
Davidroid | 5:d3c78f12a78d | 549 | { |
Davidroid | 18:2d6ab2b93685 | 550 | spi_tx_bursts[0][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 551 | spi_tx_bursts[1][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 552 | spi_tx_bursts[2][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 553 | spi_tx_bursts[3][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 554 | spi_rx_bursts[1][i] = 0; |
Davidroid | 18:2d6ab2b93685 | 555 | spi_rx_bursts[2][i] = 0; |
Davidroid | 18:2d6ab2b93685 | 556 | spi_rx_bursts[3][i] = 0; |
Davidroid | 5:d3c78f12a78d | 557 | } |
Davidroid | 0:2887415a46cd | 558 | |
Davidroid | 7:f7e0c3621f77 | 559 | switch (parameter) |
Davidroid | 0:2887415a46cd | 560 | { |
Davidroid | 0:2887415a46cd | 561 | case L6474_ABS_POS: ; |
Davidroid | 0:2887415a46cd | 562 | case L6474_MARK: |
Davidroid | 18:2d6ab2b93685 | 563 | spi_tx_bursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); |
Davidroid | 0:2887415a46cd | 564 | maxArgumentNbBytes = 3; |
Davidroid | 0:2887415a46cd | 565 | break; |
Davidroid | 0:2887415a46cd | 566 | case L6474_EL_POS: ; |
Davidroid | 0:2887415a46cd | 567 | case L6474_CONFIG: ; |
Davidroid | 0:2887415a46cd | 568 | case L6474_STATUS: |
Davidroid | 18:2d6ab2b93685 | 569 | spi_tx_bursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); |
Davidroid | 0:2887415a46cd | 570 | maxArgumentNbBytes = 2; |
Davidroid | 0:2887415a46cd | 571 | break; |
Davidroid | 0:2887415a46cd | 572 | default: |
Davidroid | 18:2d6ab2b93685 | 573 | spi_tx_bursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); |
Davidroid | 0:2887415a46cd | 574 | maxArgumentNbBytes = 1; |
Davidroid | 0:2887415a46cd | 575 | } |
Davidroid | 0:2887415a46cd | 576 | |
Davidroid | 0:2887415a46cd | 577 | /* Disable interruption before checking */ |
Davidroid | 0:2887415a46cd | 578 | /* pre-emption by ISR and SPI transfers*/ |
Davidroid | 0:2887415a46cd | 579 | L6474_DisableIrq(); |
Davidroid | 0:2887415a46cd | 580 | itDisable = TRUE; |
Davidroid | 18:2d6ab2b93685 | 581 | } while (spi_preemtion_by_isr); // check pre-emption by ISR |
Davidroid | 0:2887415a46cd | 582 | |
Davidroid | 0:2887415a46cd | 583 | for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
Davidroid | 0:2887415a46cd | 584 | i < L6474_CMD_ARG_MAX_NB_BYTES; |
Davidroid | 0:2887415a46cd | 585 | i++) |
Davidroid | 0:2887415a46cd | 586 | { |
Davidroid | 18:2d6ab2b93685 | 587 | L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]); |
Davidroid | 0:2887415a46cd | 588 | } |
Davidroid | 0:2887415a46cd | 589 | |
Davidroid | 18:2d6ab2b93685 | 590 | spiRxData = ((uint32_t)spi_rx_bursts[1][spiIndex] << 16) | |
Davidroid | 18:2d6ab2b93685 | 591 | (spi_rx_bursts[2][spiIndex] << 8) | |
Davidroid | 18:2d6ab2b93685 | 592 | (spi_rx_bursts[3][spiIndex]); |
Davidroid | 0:2887415a46cd | 593 | |
Davidroid | 0:2887415a46cd | 594 | /* re-enable L6474_EnableIrq after SPI transfers*/ |
Davidroid | 0:2887415a46cd | 595 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 596 | |
Davidroid | 0:2887415a46cd | 597 | return (spiRxData); |
Davidroid | 0:2887415a46cd | 598 | } |
Davidroid | 0:2887415a46cd | 599 | |
Davidroid | 5:d3c78f12a78d | 600 | /********************************************************** |
Davidroid | 0:2887415a46cd | 601 | * @brief Issues the GetStatus command to the L6474 of the specified device |
Davidroid | 0:2887415a46cd | 602 | * @retval Status Register value |
Davidroid | 0:2887415a46cd | 603 | * @note Once the GetStatus command is performed, the flags of the status register |
Davidroid | 0:2887415a46cd | 604 | * are reset. This is not the case when the status register is read with the |
Davidroid | 7:f7e0c3621f77 | 605 | * GetParam command (via the functions L6474ReadStatusRegister or L6474_CmdGetParam). |
Davidroid | 0:2887415a46cd | 606 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 607 | uint16_t L6474::L6474_CmdGetStatus(void) |
Davidroid | 0:2887415a46cd | 608 | { |
Davidroid | 0:2887415a46cd | 609 | uint32_t i; |
Davidroid | 0:2887415a46cd | 610 | uint16_t status; |
Davidroid | 18:2d6ab2b93685 | 611 | uint8_t spiIndex = number_of_devices - device_instance - 1; |
Davidroid | 0:2887415a46cd | 612 | bool itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 613 | |
Davidroid | 0:2887415a46cd | 614 | do |
Davidroid | 0:2887415a46cd | 615 | { |
Davidroid | 18:2d6ab2b93685 | 616 | spi_preemtion_by_isr = FALSE; |
Davidroid | 0:2887415a46cd | 617 | if (itDisable) |
Davidroid | 0:2887415a46cd | 618 | { |
Davidroid | 0:2887415a46cd | 619 | /* re-enable L6474_EnableIrq if disable in previous iteration */ |
Davidroid | 0:2887415a46cd | 620 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 621 | itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 622 | } |
Davidroid | 0:2887415a46cd | 623 | |
Davidroid | 18:2d6ab2b93685 | 624 | for (i = 0; i < number_of_devices; i++) |
Davidroid | 5:d3c78f12a78d | 625 | { |
Davidroid | 18:2d6ab2b93685 | 626 | spi_tx_bursts[0][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 627 | spi_tx_bursts[1][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 628 | spi_tx_bursts[2][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 629 | spi_rx_bursts[1][i] = 0; |
Davidroid | 18:2d6ab2b93685 | 630 | spi_rx_bursts[2][i] = 0; |
Davidroid | 5:d3c78f12a78d | 631 | } |
Davidroid | 18:2d6ab2b93685 | 632 | spi_tx_bursts[0][spiIndex] = L6474_GET_STATUS; |
Davidroid | 0:2887415a46cd | 633 | |
Davidroid | 0:2887415a46cd | 634 | /* Disable interruption before checking */ |
Davidroid | 0:2887415a46cd | 635 | /* pre-emption by ISR and SPI transfers*/ |
Davidroid | 0:2887415a46cd | 636 | L6474_DisableIrq(); |
Davidroid | 0:2887415a46cd | 637 | itDisable = TRUE; |
Davidroid | 18:2d6ab2b93685 | 638 | } while (spi_preemtion_by_isr); // check pre-emption by ISR |
Davidroid | 0:2887415a46cd | 639 | |
Davidroid | 0:2887415a46cd | 640 | for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++) |
Davidroid | 0:2887415a46cd | 641 | { |
Davidroid | 18:2d6ab2b93685 | 642 | L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]); |
Davidroid | 0:2887415a46cd | 643 | } |
Davidroid | 18:2d6ab2b93685 | 644 | status = (spi_rx_bursts[1][spiIndex] << 8) | (spi_rx_bursts[2][spiIndex]); |
Davidroid | 0:2887415a46cd | 645 | |
Davidroid | 0:2887415a46cd | 646 | /* re-enable L6474_EnableIrq after SPI transfers*/ |
Davidroid | 0:2887415a46cd | 647 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 648 | |
Davidroid | 0:2887415a46cd | 649 | return (status); |
Davidroid | 0:2887415a46cd | 650 | } |
Davidroid | 0:2887415a46cd | 651 | |
Davidroid | 5:d3c78f12a78d | 652 | /********************************************************** |
Davidroid | 0:2887415a46cd | 653 | * @brief Issues the Nop command to the L6474 of the specified device |
Davidroid | 0:2887415a46cd | 654 | * @retval None |
Davidroid | 0:2887415a46cd | 655 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 656 | void L6474::L6474_CmdNop(void) |
Davidroid | 0:2887415a46cd | 657 | { |
Davidroid | 0:2887415a46cd | 658 | L6474_SendCommand(L6474_NOP); |
Davidroid | 0:2887415a46cd | 659 | } |
Davidroid | 0:2887415a46cd | 660 | |
Davidroid | 5:d3c78f12a78d | 661 | /********************************************************** |
Davidroid | 0:2887415a46cd | 662 | * @brief Issues the SetParam command to the L6474 of the specified device |
Davidroid | 7:f7e0c3621f77 | 663 | * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...) |
Davidroid | 0:2887415a46cd | 664 | * @param[in] value Value to set in the register |
Davidroid | 0:2887415a46cd | 665 | * @retval None |
Davidroid | 0:2887415a46cd | 666 | **********************************************************/ |
Davidroid | 7:f7e0c3621f77 | 667 | void L6474::L6474_CmdSetParam(L6474_Registers_t parameter, uint32_t value) |
Davidroid | 0:2887415a46cd | 668 | { |
Davidroid | 0:2887415a46cd | 669 | uint32_t i; |
Davidroid | 0:2887415a46cd | 670 | uint8_t maxArgumentNbBytes = 0; |
Davidroid | 18:2d6ab2b93685 | 671 | uint8_t spiIndex = number_of_devices - device_instance - 1; |
Davidroid | 0:2887415a46cd | 672 | bool itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 673 | do |
Davidroid | 0:2887415a46cd | 674 | { |
Davidroid | 18:2d6ab2b93685 | 675 | spi_preemtion_by_isr = FALSE; |
Davidroid | 0:2887415a46cd | 676 | if (itDisable) |
Davidroid | 0:2887415a46cd | 677 | { |
Davidroid | 0:2887415a46cd | 678 | /* re-enable L6474_EnableIrq if disable in previous iteration */ |
Davidroid | 0:2887415a46cd | 679 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 680 | itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 681 | } |
Davidroid | 0:2887415a46cd | 682 | |
Davidroid | 18:2d6ab2b93685 | 683 | for (i = 0; i < number_of_devices; i++) |
Davidroid | 5:d3c78f12a78d | 684 | { |
Davidroid | 18:2d6ab2b93685 | 685 | spi_tx_bursts[0][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 686 | spi_tx_bursts[1][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 687 | spi_tx_bursts[2][i] = L6474_NOP; |
Davidroid | 18:2d6ab2b93685 | 688 | spi_tx_bursts[3][i] = L6474_NOP; |
Davidroid | 5:d3c78f12a78d | 689 | } |
Davidroid | 0:2887415a46cd | 690 | |
Davidroid | 7:f7e0c3621f77 | 691 | switch (parameter) |
Davidroid | 0:2887415a46cd | 692 | { |
Davidroid | 0:2887415a46cd | 693 | case L6474_ABS_POS: ; |
Davidroid | 0:2887415a46cd | 694 | case L6474_MARK: |
Davidroid | 18:2d6ab2b93685 | 695 | spi_tx_bursts[0][spiIndex] = parameter; |
Davidroid | 18:2d6ab2b93685 | 696 | spi_tx_bursts[1][spiIndex] = (uint8_t)(value >> 16); |
Davidroid | 18:2d6ab2b93685 | 697 | spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8); |
Davidroid | 0:2887415a46cd | 698 | maxArgumentNbBytes = 3; |
Davidroid | 0:2887415a46cd | 699 | break; |
Davidroid | 0:2887415a46cd | 700 | case L6474_EL_POS: ; |
Davidroid | 0:2887415a46cd | 701 | case L6474_CONFIG: |
Davidroid | 18:2d6ab2b93685 | 702 | spi_tx_bursts[1][spiIndex] = parameter; |
Davidroid | 18:2d6ab2b93685 | 703 | spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8); |
Davidroid | 0:2887415a46cd | 704 | maxArgumentNbBytes = 2; |
Davidroid | 0:2887415a46cd | 705 | break; |
Davidroid | 0:2887415a46cd | 706 | default: |
Davidroid | 18:2d6ab2b93685 | 707 | spi_tx_bursts[2][spiIndex] = parameter; |
Davidroid | 0:2887415a46cd | 708 | maxArgumentNbBytes = 1; |
Davidroid | 0:2887415a46cd | 709 | break; |
Davidroid | 0:2887415a46cd | 710 | } |
Davidroid | 18:2d6ab2b93685 | 711 | spi_tx_bursts[3][spiIndex] = (uint8_t)(value); |
Davidroid | 0:2887415a46cd | 712 | |
Davidroid | 0:2887415a46cd | 713 | /* Disable interruption before checking */ |
Davidroid | 0:2887415a46cd | 714 | /* pre-emption by ISR and SPI transfers*/ |
Davidroid | 0:2887415a46cd | 715 | L6474_DisableIrq(); |
Davidroid | 0:2887415a46cd | 716 | itDisable = TRUE; |
Davidroid | 18:2d6ab2b93685 | 717 | } while (spi_preemtion_by_isr); // check pre-emption by ISR |
Davidroid | 0:2887415a46cd | 718 | |
Davidroid | 0:2887415a46cd | 719 | /* SPI transfer */ |
Davidroid | 0:2887415a46cd | 720 | for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
Davidroid | 0:2887415a46cd | 721 | i < L6474_CMD_ARG_MAX_NB_BYTES; |
Davidroid | 0:2887415a46cd | 722 | i++) |
Davidroid | 0:2887415a46cd | 723 | { |
Davidroid | 18:2d6ab2b93685 | 724 | L6474_WriteBytes(&spi_tx_bursts[i][0],&spi_rx_bursts[i][0]); |
Davidroid | 0:2887415a46cd | 725 | } |
Davidroid | 0:2887415a46cd | 726 | /* re-enable L6474_EnableIrq after SPI transfers*/ |
Davidroid | 0:2887415a46cd | 727 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 728 | } |
Davidroid | 0:2887415a46cd | 729 | |
Davidroid | 5:d3c78f12a78d | 730 | /********************************************************** |
Davidroid | 0:2887415a46cd | 731 | * @brief Reads the Status Register value |
Davidroid | 0:2887415a46cd | 732 | * @retval Status register valued |
Davidroid | 0:2887415a46cd | 733 | * @note The status register flags are not cleared |
Davidroid | 0:2887415a46cd | 734 | * at the difference with L6474CmdGetStatus() |
Davidroid | 0:2887415a46cd | 735 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 736 | uint16_t L6474::L6474_ReadStatusRegister(void) |
Davidroid | 0:2887415a46cd | 737 | { |
Davidroid | 0:2887415a46cd | 738 | return (L6474_CmdGetParam(L6474_STATUS)); |
Davidroid | 0:2887415a46cd | 739 | } |
Davidroid | 0:2887415a46cd | 740 | |
Davidroid | 5:d3c78f12a78d | 741 | /********************************************************** |
Davidroid | 0:2887415a46cd | 742 | * @brief Set the stepping mode |
Davidroid | 0:2887415a46cd | 743 | * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t |
Davidroid | 0:2887415a46cd | 744 | * @retval None |
Davidroid | 0:2887415a46cd | 745 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 746 | void L6474::L6474_SelectStepMode(motorStepMode_t stepMod) |
Davidroid | 0:2887415a46cd | 747 | { |
Davidroid | 0:2887415a46cd | 748 | uint8_t stepModeRegister; |
Davidroid | 0:2887415a46cd | 749 | L6474_STEP_SEL_t l6474StepMod; |
Davidroid | 0:2887415a46cd | 750 | |
Davidroid | 0:2887415a46cd | 751 | switch (stepMod) |
Davidroid | 0:2887415a46cd | 752 | { |
Davidroid | 0:2887415a46cd | 753 | case STEP_MODE_FULL: |
Davidroid | 0:2887415a46cd | 754 | l6474StepMod = L6474_STEP_SEL_1; |
Davidroid | 0:2887415a46cd | 755 | break; |
Davidroid | 0:2887415a46cd | 756 | case STEP_MODE_HALF: |
Davidroid | 0:2887415a46cd | 757 | l6474StepMod = L6474_STEP_SEL_1_2; |
Davidroid | 0:2887415a46cd | 758 | break; |
Davidroid | 0:2887415a46cd | 759 | case STEP_MODE_1_4: |
Davidroid | 0:2887415a46cd | 760 | l6474StepMod = L6474_STEP_SEL_1_4; |
Davidroid | 0:2887415a46cd | 761 | break; |
Davidroid | 0:2887415a46cd | 762 | case STEP_MODE_1_8: |
Davidroid | 0:2887415a46cd | 763 | l6474StepMod = L6474_STEP_SEL_1_8; |
Davidroid | 0:2887415a46cd | 764 | break; |
Davidroid | 0:2887415a46cd | 765 | case STEP_MODE_1_16: |
Davidroid | 0:2887415a46cd | 766 | default: |
Davidroid | 0:2887415a46cd | 767 | l6474StepMod = L6474_STEP_SEL_1_16; |
Davidroid | 0:2887415a46cd | 768 | break; |
Davidroid | 0:2887415a46cd | 769 | } |
Davidroid | 0:2887415a46cd | 770 | |
Davidroid | 0:2887415a46cd | 771 | /* Eventually deactivate motor */ |
Davidroid | 18:2d6ab2b93685 | 772 | if (device_prm.motionState != INACTIVE) |
Davidroid | 0:2887415a46cd | 773 | { |
Davidroid | 0:2887415a46cd | 774 | L6474_HardStop(); |
Davidroid | 0:2887415a46cd | 775 | } |
Davidroid | 0:2887415a46cd | 776 | |
Davidroid | 0:2887415a46cd | 777 | /* Read Step mode register and clear STEP_SEL field */ |
Davidroid | 0:2887415a46cd | 778 | stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(L6474_STEP_MODE)) ; |
Davidroid | 0:2887415a46cd | 779 | |
Davidroid | 0:2887415a46cd | 780 | /* Apply new step mode */ |
Davidroid | 0:2887415a46cd | 781 | L6474_CmdSetParam(L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod); |
Davidroid | 0:2887415a46cd | 782 | |
Davidroid | 0:2887415a46cd | 783 | /* Reset abs pos register */ |
Davidroid | 0:2887415a46cd | 784 | L6474_SetHome(); |
Davidroid | 0:2887415a46cd | 785 | } |
Davidroid | 0:2887415a46cd | 786 | |
Davidroid | 5:d3c78f12a78d | 787 | /********************************************************** |
Davidroid | 5:d3c78f12a78d | 788 | * @brief Get the direction |
Davidroid | 5:d3c78f12a78d | 789 | * @param None |
Davidroid | 5:d3c78f12a78d | 790 | * @retval direction FORWARD or BACKWARD |
Davidroid | 4:83a1eb397a65 | 791 | **********************************************************/ |
Davidroid | 5:d3c78f12a78d | 792 | motorDir_t L6474::L6474_GetDirection(void) |
Davidroid | 4:83a1eb397a65 | 793 | { |
Davidroid | 18:2d6ab2b93685 | 794 | return device_prm.direction; |
Davidroid | 4:83a1eb397a65 | 795 | } |
Davidroid | 4:83a1eb397a65 | 796 | |
Davidroid | 5:d3c78f12a78d | 797 | /********************************************************** |
Davidroid | 0:2887415a46cd | 798 | * @brief Specifies the direction |
Davidroid | 0:2887415a46cd | 799 | * @param[in] dir FORWARD or BACKWARD |
Davidroid | 0:2887415a46cd | 800 | * @note The direction change is only applied if the device |
Davidroid | 0:2887415a46cd | 801 | * is in INACTIVE state |
Davidroid | 0:2887415a46cd | 802 | * @retval None |
Davidroid | 0:2887415a46cd | 803 | **********************************************************/ |
Davidroid | 5:d3c78f12a78d | 804 | void L6474::L6474_SetDirection(motorDir_t direction) |
Davidroid | 0:2887415a46cd | 805 | { |
Davidroid | 18:2d6ab2b93685 | 806 | if (device_prm.motionState == INACTIVE) |
Davidroid | 0:2887415a46cd | 807 | { |
Davidroid | 18:2d6ab2b93685 | 808 | device_prm.direction = direction; |
Davidroid | 5:d3c78f12a78d | 809 | L6474_SetDirectionGpio(direction); |
Davidroid | 0:2887415a46cd | 810 | } |
Davidroid | 0:2887415a46cd | 811 | } |
Davidroid | 0:2887415a46cd | 812 | |
Davidroid | 5:d3c78f12a78d | 813 | /********************************************************** |
Davidroid | 0:2887415a46cd | 814 | * @brief Updates the current speed of the device |
Davidroid | 0:2887415a46cd | 815 | * @param[in] newSpeed in pps |
Davidroid | 0:2887415a46cd | 816 | * @retval None |
Davidroid | 0:2887415a46cd | 817 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 818 | void L6474::L6474_ApplySpeed(uint16_t newSpeed) |
Davidroid | 0:2887415a46cd | 819 | { |
Davidroid | 0:2887415a46cd | 820 | if (newSpeed < L6474_MIN_PWM_FREQ) |
Davidroid | 0:2887415a46cd | 821 | { |
Davidroid | 0:2887415a46cd | 822 | newSpeed = L6474_MIN_PWM_FREQ; |
Davidroid | 0:2887415a46cd | 823 | } |
Davidroid | 0:2887415a46cd | 824 | if (newSpeed > L6474_MAX_PWM_FREQ) |
Davidroid | 0:2887415a46cd | 825 | { |
Davidroid | 0:2887415a46cd | 826 | newSpeed = L6474_MAX_PWM_FREQ; |
Davidroid | 0:2887415a46cd | 827 | } |
Davidroid | 0:2887415a46cd | 828 | |
Davidroid | 18:2d6ab2b93685 | 829 | device_prm.speed = newSpeed; |
Davidroid | 0:2887415a46cd | 830 | |
Davidroid | 0:2887415a46cd | 831 | L6474_PwmSetFreq(newSpeed); |
Davidroid | 0:2887415a46cd | 832 | } |
Davidroid | 0:2887415a46cd | 833 | |
Davidroid | 5:d3c78f12a78d | 834 | /********************************************************** |
Davidroid | 0:2887415a46cd | 835 | * @brief Computes the speed profile according to the number of steps to move |
Davidroid | 0:2887415a46cd | 836 | * @param[in] nbSteps number of steps to perform |
Davidroid | 0:2887415a46cd | 837 | * @retval None |
Davidroid | 0:2887415a46cd | 838 | * @note Using the acceleration and deceleration of the device, |
Davidroid | 0:2887415a46cd | 839 | * this function determines the duration in steps of the acceleration, |
Davidroid | 0:2887415a46cd | 840 | * steady and deceleration phases. |
Davidroid | 0:2887415a46cd | 841 | * If the total number of steps to perform is big enough, a trapezoidal move |
Davidroid | 0:2887415a46cd | 842 | * is performed (i.e. there is a steady phase where the motor runs at the maximum |
Davidroid | 0:2887415a46cd | 843 | * speed. |
Davidroid | 0:2887415a46cd | 844 | * Else, a triangular move is performed (no steady phase: the maximum speed is never |
Davidroid | 0:2887415a46cd | 845 | * reached. |
Davidroid | 0:2887415a46cd | 846 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 847 | void L6474::L6474_ComputeSpeedProfile(uint32_t nbSteps) |
Davidroid | 0:2887415a46cd | 848 | { |
Davidroid | 0:2887415a46cd | 849 | uint32_t reqAccSteps; |
Davidroid | 0:2887415a46cd | 850 | uint32_t reqDecSteps; |
Davidroid | 0:2887415a46cd | 851 | |
Davidroid | 0:2887415a46cd | 852 | /* compute the number of steps to get the targeted speed */ |
Davidroid | 18:2d6ab2b93685 | 853 | uint16_t minSpeed = device_prm.minSpeed; |
Davidroid | 18:2d6ab2b93685 | 854 | reqAccSteps = (device_prm.maxSpeed - minSpeed); |
Davidroid | 18:2d6ab2b93685 | 855 | reqAccSteps *= (device_prm.maxSpeed + minSpeed); |
Davidroid | 0:2887415a46cd | 856 | reqDecSteps = reqAccSteps; |
Davidroid | 18:2d6ab2b93685 | 857 | reqAccSteps /= (uint32_t)device_prm.acceleration; |
Davidroid | 0:2887415a46cd | 858 | reqAccSteps /= 2; |
Davidroid | 0:2887415a46cd | 859 | |
Davidroid | 0:2887415a46cd | 860 | /* compute the number of steps to stop */ |
Davidroid | 18:2d6ab2b93685 | 861 | reqDecSteps /= (uint32_t)device_prm.deceleration; |
Davidroid | 0:2887415a46cd | 862 | reqDecSteps /= 2; |
Davidroid | 0:2887415a46cd | 863 | |
Davidroid | 0:2887415a46cd | 864 | if(( reqAccSteps + reqDecSteps ) > nbSteps) |
Davidroid | 0:2887415a46cd | 865 | { |
Davidroid | 0:2887415a46cd | 866 | /* Triangular move */ |
Davidroid | 0:2887415a46cd | 867 | /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ |
Davidroid | 18:2d6ab2b93685 | 868 | uint32_t dec = device_prm.deceleration; |
Davidroid | 18:2d6ab2b93685 | 869 | uint32_t acc = device_prm.acceleration; |
Davidroid | 0:2887415a46cd | 870 | |
Davidroid | 0:2887415a46cd | 871 | reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); |
Davidroid | 0:2887415a46cd | 872 | if (reqDecSteps > 1) |
Davidroid | 0:2887415a46cd | 873 | { |
Davidroid | 0:2887415a46cd | 874 | reqAccSteps = reqDecSteps - 1; |
Davidroid | 0:2887415a46cd | 875 | if(reqAccSteps == 0) |
Davidroid | 0:2887415a46cd | 876 | { |
Davidroid | 0:2887415a46cd | 877 | reqAccSteps = 1; |
Davidroid | 0:2887415a46cd | 878 | } |
Davidroid | 0:2887415a46cd | 879 | } |
Davidroid | 0:2887415a46cd | 880 | else |
Davidroid | 0:2887415a46cd | 881 | { |
Davidroid | 0:2887415a46cd | 882 | reqAccSteps = 0; |
Davidroid | 0:2887415a46cd | 883 | } |
Davidroid | 18:2d6ab2b93685 | 884 | device_prm.endAccPos = reqAccSteps; |
Davidroid | 18:2d6ab2b93685 | 885 | device_prm.startDecPos = reqDecSteps; |
Davidroid | 0:2887415a46cd | 886 | } |
Davidroid | 0:2887415a46cd | 887 | else |
Davidroid | 0:2887415a46cd | 888 | { |
Davidroid | 0:2887415a46cd | 889 | /* Trapezoidal move */ |
Davidroid | 0:2887415a46cd | 890 | /* accelerating phase to endAccPos */ |
Davidroid | 0:2887415a46cd | 891 | /* steady phase from endAccPos to startDecPos */ |
Davidroid | 0:2887415a46cd | 892 | /* decelerating from startDecPos to stepsToTake*/ |
Davidroid | 18:2d6ab2b93685 | 893 | device_prm.endAccPos = reqAccSteps; |
Davidroid | 18:2d6ab2b93685 | 894 | device_prm.startDecPos = nbSteps - reqDecSteps - 1; |
Davidroid | 0:2887415a46cd | 895 | } |
Davidroid | 0:2887415a46cd | 896 | } |
Davidroid | 0:2887415a46cd | 897 | |
Davidroid | 5:d3c78f12a78d | 898 | /********************************************************** |
Davidroid | 0:2887415a46cd | 899 | * @brief Converts the ABS_POSITION register value to a 32b signed integer |
Davidroid | 0:2887415a46cd | 900 | * @param[in] abs_position_reg value of the ABS_POSITION register |
Davidroid | 0:2887415a46cd | 901 | * @retval operation_result 32b signed integer corresponding to the absolute position |
Davidroid | 0:2887415a46cd | 902 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 903 | int32_t L6474::L6474_ConvertPosition(uint32_t abs_position_reg) |
Davidroid | 0:2887415a46cd | 904 | { |
Davidroid | 0:2887415a46cd | 905 | int32_t operation_result; |
Davidroid | 0:2887415a46cd | 906 | |
Davidroid | 0:2887415a46cd | 907 | if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) |
Davidroid | 0:2887415a46cd | 908 | { |
Davidroid | 0:2887415a46cd | 909 | /* Negative register value */ |
Davidroid | 0:2887415a46cd | 910 | abs_position_reg = ~abs_position_reg; |
Davidroid | 0:2887415a46cd | 911 | abs_position_reg += 1; |
Davidroid | 0:2887415a46cd | 912 | |
Davidroid | 0:2887415a46cd | 913 | operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK); |
Davidroid | 0:2887415a46cd | 914 | operation_result = -operation_result; |
Davidroid | 0:2887415a46cd | 915 | } |
Davidroid | 0:2887415a46cd | 916 | else |
Davidroid | 0:2887415a46cd | 917 | { |
Davidroid | 0:2887415a46cd | 918 | operation_result = (int32_t) abs_position_reg; |
Davidroid | 0:2887415a46cd | 919 | } |
Davidroid | 5:d3c78f12a78d | 920 | |
Davidroid | 0:2887415a46cd | 921 | return operation_result; |
Davidroid | 0:2887415a46cd | 922 | } |
Davidroid | 0:2887415a46cd | 923 | |
Davidroid | 5:d3c78f12a78d | 924 | /********************************************************** |
Davidroid | 0:2887415a46cd | 925 | * @brief Error handler which calls the user callback (if defined) |
Davidroid | 0:2887415a46cd | 926 | * @param[in] error Number of the error |
Davidroid | 0:2887415a46cd | 927 | * @retval None |
Davidroid | 0:2887415a46cd | 928 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 929 | void L6474::L6474_ErrorHandler(uint16_t error) |
Davidroid | 0:2887415a46cd | 930 | { |
Davidroid | 18:2d6ab2b93685 | 931 | if (error_handler_callback != 0) |
Davidroid | 0:2887415a46cd | 932 | { |
Davidroid | 18:2d6ab2b93685 | 933 | (void) error_handler_callback(error); |
Davidroid | 0:2887415a46cd | 934 | } |
Davidroid | 0:2887415a46cd | 935 | else |
Davidroid | 0:2887415a46cd | 936 | { |
Davidroid | 7:f7e0c3621f77 | 937 | /* Aborting the program. */ |
Davidroid | 7:f7e0c3621f77 | 938 | exit(EXIT_FAILURE); |
Davidroid | 0:2887415a46cd | 939 | } |
Davidroid | 0:2887415a46cd | 940 | } |
Davidroid | 0:2887415a46cd | 941 | |
Davidroid | 5:d3c78f12a78d | 942 | /********************************************************** |
Davidroid | 0:2887415a46cd | 943 | * @brief Sends a command without arguments to the L6474 via the SPI |
Davidroid | 0:2887415a46cd | 944 | * @param[in] param Command to send |
Davidroid | 0:2887415a46cd | 945 | * @retval None |
Davidroid | 0:2887415a46cd | 946 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 947 | void L6474::L6474_SendCommand(uint8_t param) |
Davidroid | 0:2887415a46cd | 948 | { |
Davidroid | 5:d3c78f12a78d | 949 | uint32_t i; |
Davidroid | 5:d3c78f12a78d | 950 | bool itDisable = FALSE; |
Davidroid | 18:2d6ab2b93685 | 951 | uint8_t spiIndex = number_of_devices - device_instance - 1; |
Davidroid | 0:2887415a46cd | 952 | |
Davidroid | 0:2887415a46cd | 953 | do |
Davidroid | 0:2887415a46cd | 954 | { |
Davidroid | 18:2d6ab2b93685 | 955 | spi_preemtion_by_isr = FALSE; |
Davidroid | 0:2887415a46cd | 956 | if (itDisable) |
Davidroid | 0:2887415a46cd | 957 | { |
Davidroid | 0:2887415a46cd | 958 | /* re-enable L6474_EnableIrq if disable in previous iteration */ |
Davidroid | 0:2887415a46cd | 959 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 960 | itDisable = FALSE; |
Davidroid | 0:2887415a46cd | 961 | } |
Davidroid | 0:2887415a46cd | 962 | |
Davidroid | 18:2d6ab2b93685 | 963 | for (i = 0; i < number_of_devices; i++) |
Davidroid | 5:d3c78f12a78d | 964 | { |
Davidroid | 18:2d6ab2b93685 | 965 | spi_tx_bursts[3][i] = L6474_NOP; |
Davidroid | 5:d3c78f12a78d | 966 | } |
Davidroid | 18:2d6ab2b93685 | 967 | spi_tx_bursts[3][spiIndex] = param; |
Davidroid | 0:2887415a46cd | 968 | |
Davidroid | 0:2887415a46cd | 969 | /* Disable interruption before checking */ |
Davidroid | 0:2887415a46cd | 970 | /* pre-emption by ISR and SPI transfers*/ |
Davidroid | 0:2887415a46cd | 971 | L6474_DisableIrq(); |
Davidroid | 0:2887415a46cd | 972 | itDisable = TRUE; |
Davidroid | 18:2d6ab2b93685 | 973 | } while (spi_preemtion_by_isr); // check pre-emption by ISR |
Davidroid | 0:2887415a46cd | 974 | |
Davidroid | 18:2d6ab2b93685 | 975 | L6474_WriteBytes(&spi_tx_bursts[3][0], &spi_rx_bursts[3][0]); |
Davidroid | 0:2887415a46cd | 976 | |
Davidroid | 0:2887415a46cd | 977 | /* re-enable L6474_EnableIrq after SPI transfers*/ |
Davidroid | 0:2887415a46cd | 978 | L6474_EnableIrq(); |
Davidroid | 0:2887415a46cd | 979 | } |
Davidroid | 0:2887415a46cd | 980 | |
Davidroid | 5:d3c78f12a78d | 981 | /********************************************************** |
Davidroid | 0:2887415a46cd | 982 | * @brief Sets the registers of the L6474 to their predefined values |
Davidroid | 0:2887415a46cd | 983 | * from l6474_target_config.h |
Davidroid | 0:2887415a46cd | 984 | * @retval None |
Davidroid | 0:2887415a46cd | 985 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 986 | void L6474::L6474_SetRegisterToPredefinedValues(void) |
Davidroid | 0:2887415a46cd | 987 | { |
Davidroid | 0:2887415a46cd | 988 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 989 | L6474_ABS_POS, |
Davidroid | 0:2887415a46cd | 990 | 0); |
Davidroid | 0:2887415a46cd | 991 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 992 | L6474_EL_POS, |
Davidroid | 0:2887415a46cd | 993 | 0); |
Davidroid | 0:2887415a46cd | 994 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 995 | L6474_MARK, |
Davidroid | 0:2887415a46cd | 996 | 0); |
Davidroid | 18:2d6ab2b93685 | 997 | switch (device_instance) |
Davidroid | 0:2887415a46cd | 998 | { |
Davidroid | 0:2887415a46cd | 999 | case 0: |
Davidroid | 0:2887415a46cd | 1000 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1001 | L6474_TVAL, |
Davidroid | 0:2887415a46cd | 1002 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); |
Davidroid | 0:2887415a46cd | 1003 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1004 | L6474_T_FAST, |
Davidroid | 0:2887415a46cd | 1005 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1006 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0); |
Davidroid | 0:2887415a46cd | 1007 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1008 | L6474_TON_MIN, |
Davidroid | 0:2887415a46cd | 1009 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0) |
Davidroid | 0:2887415a46cd | 1010 | ); |
Davidroid | 0:2887415a46cd | 1011 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1012 | L6474_TOFF_MIN, |
Davidroid | 0:2887415a46cd | 1013 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0)); |
Davidroid | 0:2887415a46cd | 1014 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1015 | L6474_OCD_TH, |
Davidroid | 0:2887415a46cd | 1016 | L6474_CONF_PARAM_OCD_TH_DEVICE_0); |
Davidroid | 0:2887415a46cd | 1017 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1018 | L6474_STEP_MODE, |
Davidroid | 0:2887415a46cd | 1019 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1020 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0); |
Davidroid | 0:2887415a46cd | 1021 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1022 | L6474_ALARM_EN, |
Davidroid | 0:2887415a46cd | 1023 | L6474_CONF_PARAM_ALARM_EN_DEVICE_0); |
Davidroid | 0:2887415a46cd | 1024 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1025 | L6474_CONFIG, |
Davidroid | 0:2887415a46cd | 1026 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1027 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1028 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1029 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 | |
Davidroid | 0:2887415a46cd | 1030 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0); |
Davidroid | 0:2887415a46cd | 1031 | break; |
Davidroid | 0:2887415a46cd | 1032 | case 1: |
Davidroid | 0:2887415a46cd | 1033 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1034 | L6474_TVAL, |
Davidroid | 0:2887415a46cd | 1035 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); |
Davidroid | 0:2887415a46cd | 1036 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1037 | L6474_T_FAST, |
Davidroid | 0:2887415a46cd | 1038 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1039 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1); |
Davidroid | 0:2887415a46cd | 1040 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1041 | L6474_TON_MIN, |
Davidroid | 0:2887415a46cd | 1042 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1)); |
Davidroid | 0:2887415a46cd | 1043 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1044 | L6474_TOFF_MIN, |
Davidroid | 0:2887415a46cd | 1045 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1)); |
Davidroid | 0:2887415a46cd | 1046 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1047 | L6474_OCD_TH, |
Davidroid | 0:2887415a46cd | 1048 | L6474_CONF_PARAM_OCD_TH_DEVICE_1); |
Davidroid | 0:2887415a46cd | 1049 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1050 | L6474_STEP_MODE, |
Davidroid | 0:2887415a46cd | 1051 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1052 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1); |
Davidroid | 0:2887415a46cd | 1053 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1054 | L6474_ALARM_EN, |
Davidroid | 0:2887415a46cd | 1055 | L6474_CONF_PARAM_ALARM_EN_DEVICE_1); |
Davidroid | 0:2887415a46cd | 1056 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1057 | L6474_CONFIG, |
Davidroid | 0:2887415a46cd | 1058 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1059 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1060 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1061 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 | |
Davidroid | 0:2887415a46cd | 1062 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1); |
Davidroid | 0:2887415a46cd | 1063 | break; |
Davidroid | 0:2887415a46cd | 1064 | case 2: |
Davidroid | 0:2887415a46cd | 1065 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1066 | L6474_TVAL, |
Davidroid | 0:2887415a46cd | 1067 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); |
Davidroid | 0:2887415a46cd | 1068 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1069 | L6474_T_FAST, |
Davidroid | 0:2887415a46cd | 1070 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1071 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2); |
Davidroid | 0:2887415a46cd | 1072 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1073 | L6474_TON_MIN, |
Davidroid | 0:2887415a46cd | 1074 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2)); |
Davidroid | 0:2887415a46cd | 1075 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1076 | L6474_TOFF_MIN, |
Davidroid | 0:2887415a46cd | 1077 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2)); |
Davidroid | 0:2887415a46cd | 1078 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1079 | L6474_OCD_TH, |
Davidroid | 0:2887415a46cd | 1080 | L6474_CONF_PARAM_OCD_TH_DEVICE_2); |
Davidroid | 0:2887415a46cd | 1081 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1082 | L6474_STEP_MODE, |
Davidroid | 0:2887415a46cd | 1083 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1084 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2); |
Davidroid | 0:2887415a46cd | 1085 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1086 | L6474_ALARM_EN, |
Davidroid | 0:2887415a46cd | 1087 | L6474_CONF_PARAM_ALARM_EN_DEVICE_2); |
Davidroid | 0:2887415a46cd | 1088 | L6474_CmdSetParam( |
Davidroid | 0:2887415a46cd | 1089 | L6474_CONFIG, |
Davidroid | 0:2887415a46cd | 1090 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1091 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1092 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1093 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 | |
Davidroid | 0:2887415a46cd | 1094 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2); |
Davidroid | 0:2887415a46cd | 1095 | break; |
Davidroid | 0:2887415a46cd | 1096 | default: ; |
Davidroid | 0:2887415a46cd | 1097 | } |
Davidroid | 0:2887415a46cd | 1098 | } |
Davidroid | 0:2887415a46cd | 1099 | |
Davidroid | 5:d3c78f12a78d | 1100 | /********************************************************** |
Davidroid | 18:2d6ab2b93685 | 1101 | * @brief Sets the registers of the L6474 to initialization values. |
Davidroid | 18:2d6ab2b93685 | 1102 | * @param init Initialization structure. |
Davidroid | 18:2d6ab2b93685 | 1103 | * @retval None. |
Davidroid | 18:2d6ab2b93685 | 1104 | **********************************************************/ |
Davidroid | 22:ed3a6990a6eb | 1105 | void L6474::L6474_SetRegisterToInitializationValues(L6474_Init_t *init) |
Davidroid | 18:2d6ab2b93685 | 1106 | { |
Davidroid | 18:2d6ab2b93685 | 1107 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1108 | L6474_ABS_POS, |
Davidroid | 18:2d6ab2b93685 | 1109 | 0 |
Davidroid | 18:2d6ab2b93685 | 1110 | ); |
Davidroid | 18:2d6ab2b93685 | 1111 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1112 | L6474_EL_POS, |
Davidroid | 18:2d6ab2b93685 | 1113 | 0 |
Davidroid | 18:2d6ab2b93685 | 1114 | ); |
Davidroid | 18:2d6ab2b93685 | 1115 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1116 | L6474_MARK, |
Davidroid | 18:2d6ab2b93685 | 1117 | 0 |
Davidroid | 18:2d6ab2b93685 | 1118 | ); |
Davidroid | 18:2d6ab2b93685 | 1119 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1120 | L6474_TVAL, |
Davidroid | 18:2d6ab2b93685 | 1121 | L6474_Tval_Current_to_Par(init->torque_regulation_current_mA) |
Davidroid | 18:2d6ab2b93685 | 1122 | ); |
Davidroid | 18:2d6ab2b93685 | 1123 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1124 | L6474_T_FAST, |
Davidroid | 18:2d6ab2b93685 | 1125 | (uint8_t) init->maximum_fast_decay_time | |
Davidroid | 18:2d6ab2b93685 | 1126 | (uint8_t) init->fall_time |
Davidroid | 18:2d6ab2b93685 | 1127 | ); |
Davidroid | 18:2d6ab2b93685 | 1128 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1129 | L6474_TON_MIN, |
Davidroid | 18:2d6ab2b93685 | 1130 | L6474_Tmin_Time_to_Par(init->minimum_ON_time_us) |
Davidroid | 18:2d6ab2b93685 | 1131 | ); |
Davidroid | 18:2d6ab2b93685 | 1132 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1133 | L6474_TOFF_MIN, |
Davidroid | 18:2d6ab2b93685 | 1134 | L6474_Tmin_Time_to_Par(init->minimum_OFF_time_us) |
Davidroid | 18:2d6ab2b93685 | 1135 | ); |
Davidroid | 18:2d6ab2b93685 | 1136 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1137 | L6474_OCD_TH, |
Davidroid | 18:2d6ab2b93685 | 1138 | init->overcurrent_threshold |
Davidroid | 18:2d6ab2b93685 | 1139 | ); |
Davidroid | 18:2d6ab2b93685 | 1140 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1141 | L6474_STEP_MODE, |
Davidroid | 18:2d6ab2b93685 | 1142 | (uint8_t) init->step_selection | |
Davidroid | 18:2d6ab2b93685 | 1143 | (uint8_t) init->sync_selection |
Davidroid | 18:2d6ab2b93685 | 1144 | ); |
Davidroid | 18:2d6ab2b93685 | 1145 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1146 | L6474_ALARM_EN, |
Davidroid | 18:2d6ab2b93685 | 1147 | init->alarm |
Davidroid | 18:2d6ab2b93685 | 1148 | ); |
Davidroid | 18:2d6ab2b93685 | 1149 | L6474_CmdSetParam( |
Davidroid | 18:2d6ab2b93685 | 1150 | L6474_CONFIG, |
Davidroid | 18:2d6ab2b93685 | 1151 | (uint16_t) init->clock | |
Davidroid | 18:2d6ab2b93685 | 1152 | (uint16_t) init->torque_regulation_method | |
Davidroid | 18:2d6ab2b93685 | 1153 | (uint16_t) init->overcurrent_shutwdown | |
Davidroid | 18:2d6ab2b93685 | 1154 | (uint16_t) init->slew_rate | |
Davidroid | 18:2d6ab2b93685 | 1155 | (uint16_t) init->target_swicthing_period |
Davidroid | 18:2d6ab2b93685 | 1156 | ); |
Davidroid | 18:2d6ab2b93685 | 1157 | } |
Davidroid | 18:2d6ab2b93685 | 1158 | |
Davidroid | 18:2d6ab2b93685 | 1159 | /********************************************************** |
Davidroid | 0:2887415a46cd | 1160 | * @brief Sets the parameters of the device to predefined values |
Davidroid | 0:2887415a46cd | 1161 | * from l6474_target_config.h |
Davidroid | 0:2887415a46cd | 1162 | * @param None |
Davidroid | 0:2887415a46cd | 1163 | * @retval None |
Davidroid | 0:2887415a46cd | 1164 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 1165 | void L6474::L6474_SetDeviceParamsToPredefinedValues(void) |
Davidroid | 0:2887415a46cd | 1166 | { |
Davidroid | 18:2d6ab2b93685 | 1167 | switch (device_instance) |
Davidroid | 0:2887415a46cd | 1168 | { |
Davidroid | 0:2887415a46cd | 1169 | case 0: |
Davidroid | 18:2d6ab2b93685 | 1170 | device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_0; |
Davidroid | 18:2d6ab2b93685 | 1171 | device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_0; |
Davidroid | 18:2d6ab2b93685 | 1172 | device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0; |
Davidroid | 18:2d6ab2b93685 | 1173 | device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0; |
Davidroid | 0:2887415a46cd | 1174 | break; |
Davidroid | 0:2887415a46cd | 1175 | |
Davidroid | 0:2887415a46cd | 1176 | case 1: |
Davidroid | 18:2d6ab2b93685 | 1177 | device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_1; |
Davidroid | 18:2d6ab2b93685 | 1178 | device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_1; |
Davidroid | 18:2d6ab2b93685 | 1179 | device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1; |
Davidroid | 18:2d6ab2b93685 | 1180 | device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1; |
Davidroid | 0:2887415a46cd | 1181 | break; |
Davidroid | 0:2887415a46cd | 1182 | |
Davidroid | 0:2887415a46cd | 1183 | case 2: |
Davidroid | 18:2d6ab2b93685 | 1184 | device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_2; |
Davidroid | 18:2d6ab2b93685 | 1185 | device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_2; |
Davidroid | 18:2d6ab2b93685 | 1186 | device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2; |
Davidroid | 18:2d6ab2b93685 | 1187 | device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2; |
Davidroid | 0:2887415a46cd | 1188 | break; |
Davidroid | 0:2887415a46cd | 1189 | } |
Davidroid | 0:2887415a46cd | 1190 | |
Davidroid | 18:2d6ab2b93685 | 1191 | device_prm.accu = 0; |
Davidroid | 18:2d6ab2b93685 | 1192 | device_prm.currentPosition = 0; |
Davidroid | 18:2d6ab2b93685 | 1193 | device_prm.endAccPos = 0; |
Davidroid | 18:2d6ab2b93685 | 1194 | device_prm.relativePos = 0; |
Davidroid | 18:2d6ab2b93685 | 1195 | device_prm.startDecPos = 0; |
Davidroid | 18:2d6ab2b93685 | 1196 | device_prm.stepsToTake = 0; |
Davidroid | 18:2d6ab2b93685 | 1197 | device_prm.speed = 0; |
Davidroid | 18:2d6ab2b93685 | 1198 | device_prm.commandExecuted = NO_CMD; |
Davidroid | 18:2d6ab2b93685 | 1199 | device_prm.direction = FORWARD; |
Davidroid | 18:2d6ab2b93685 | 1200 | device_prm.motionState = INACTIVE; |
Davidroid | 0:2887415a46cd | 1201 | } |
Davidroid | 0:2887415a46cd | 1202 | |
Davidroid | 5:d3c78f12a78d | 1203 | /********************************************************** |
Davidroid | 0:2887415a46cd | 1204 | * @brief Initialises the bridge parameters to start the movement |
Davidroid | 0:2887415a46cd | 1205 | * and enable the power bridge |
Davidroid | 0:2887415a46cd | 1206 | * @retval None |
Davidroid | 0:2887415a46cd | 1207 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 1208 | void L6474::L6474_StartMovement(void) |
Davidroid | 0:2887415a46cd | 1209 | { |
Davidroid | 0:2887415a46cd | 1210 | /* Enable L6474 powerstage */ |
Davidroid | 0:2887415a46cd | 1211 | L6474_CmdEnable(); |
Davidroid | 18:2d6ab2b93685 | 1212 | if (device_prm.endAccPos != 0) |
Davidroid | 0:2887415a46cd | 1213 | { |
Davidroid | 18:2d6ab2b93685 | 1214 | device_prm.motionState = ACCELERATING; |
Davidroid | 0:2887415a46cd | 1215 | } |
Davidroid | 0:2887415a46cd | 1216 | else |
Davidroid | 0:2887415a46cd | 1217 | { |
Davidroid | 18:2d6ab2b93685 | 1218 | device_prm.motionState = DECELERATING; |
Davidroid | 0:2887415a46cd | 1219 | } |
Davidroid | 18:2d6ab2b93685 | 1220 | device_prm.accu = 0; |
Davidroid | 18:2d6ab2b93685 | 1221 | device_prm.relativePos = 0; |
Davidroid | 18:2d6ab2b93685 | 1222 | L6474_ApplySpeed(device_prm.minSpeed); |
Davidroid | 0:2887415a46cd | 1223 | } |
Davidroid | 0:2887415a46cd | 1224 | |
Davidroid | 5:d3c78f12a78d | 1225 | /********************************************************** |
Davidroid | 0:2887415a46cd | 1226 | * @brief Handles the device state machine at each ste |
Davidroid | 0:2887415a46cd | 1227 | * @retval None |
Davidroid | 0:2887415a46cd | 1228 | * @note Must only be called by the timer ISR |
Davidroid | 0:2887415a46cd | 1229 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 1230 | void L6474::L6474_StepClockHandler(void) |
Davidroid | 0:2887415a46cd | 1231 | { |
Davidroid | 0:2887415a46cd | 1232 | /* Set isr flag */ |
Davidroid | 18:2d6ab2b93685 | 1233 | isr_flag = TRUE; |
Davidroid | 0:2887415a46cd | 1234 | |
Davidroid | 0:2887415a46cd | 1235 | /* Incrementation of the relative position */ |
Davidroid | 18:2d6ab2b93685 | 1236 | device_prm.relativePos++; |
Davidroid | 0:2887415a46cd | 1237 | |
Davidroid | 18:2d6ab2b93685 | 1238 | switch (device_prm.motionState) |
Davidroid | 0:2887415a46cd | 1239 | { |
Davidroid | 0:2887415a46cd | 1240 | case ACCELERATING: |
Davidroid | 0:2887415a46cd | 1241 | { |
Davidroid | 18:2d6ab2b93685 | 1242 | uint32_t relPos = device_prm.relativePos; |
Davidroid | 18:2d6ab2b93685 | 1243 | uint32_t endAccPos = device_prm.endAccPos; |
Davidroid | 18:2d6ab2b93685 | 1244 | uint16_t speed = device_prm.speed; |
Davidroid | 18:2d6ab2b93685 | 1245 | uint32_t acc = ((uint32_t)device_prm.acceleration << 16); |
Davidroid | 0:2887415a46cd | 1246 | |
Davidroid | 18:2d6ab2b93685 | 1247 | if ((device_prm.commandExecuted == SOFT_STOP_CMD)|| |
Davidroid | 18:2d6ab2b93685 | 1248 | ((device_prm.commandExecuted != RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1249 | (relPos == device_prm.startDecPos))) |
Davidroid | 0:2887415a46cd | 1250 | { |
Davidroid | 18:2d6ab2b93685 | 1251 | device_prm.motionState = DECELERATING; |
Davidroid | 18:2d6ab2b93685 | 1252 | device_prm.accu = 0; |
Davidroid | 0:2887415a46cd | 1253 | } |
Davidroid | 18:2d6ab2b93685 | 1254 | else if ((speed >= device_prm.maxSpeed)|| |
Davidroid | 18:2d6ab2b93685 | 1255 | ((device_prm.commandExecuted != RUN_CMD)&& |
Davidroid | 0:2887415a46cd | 1256 | (relPos == endAccPos))) |
Davidroid | 0:2887415a46cd | 1257 | { |
Davidroid | 18:2d6ab2b93685 | 1258 | device_prm.motionState = STEADY; |
Davidroid | 0:2887415a46cd | 1259 | } |
Davidroid | 0:2887415a46cd | 1260 | else |
Davidroid | 0:2887415a46cd | 1261 | { |
Davidroid | 0:2887415a46cd | 1262 | bool speedUpdated = FALSE; |
Davidroid | 0:2887415a46cd | 1263 | /* Go on accelerating */ |
Davidroid | 0:2887415a46cd | 1264 | if (speed == 0) speed =1; |
Davidroid | 18:2d6ab2b93685 | 1265 | device_prm.accu += acc / speed; |
Davidroid | 18:2d6ab2b93685 | 1266 | while (device_prm.accu >= (0X10000L)) |
Davidroid | 0:2887415a46cd | 1267 | { |
Davidroid | 18:2d6ab2b93685 | 1268 | device_prm.accu -= (0X10000L); |
Davidroid | 0:2887415a46cd | 1269 | speed +=1; |
Davidroid | 0:2887415a46cd | 1270 | speedUpdated = TRUE; |
Davidroid | 0:2887415a46cd | 1271 | } |
Davidroid | 0:2887415a46cd | 1272 | |
Davidroid | 0:2887415a46cd | 1273 | if (speedUpdated) |
Davidroid | 0:2887415a46cd | 1274 | { |
Davidroid | 18:2d6ab2b93685 | 1275 | if (speed > device_prm.maxSpeed) |
Davidroid | 0:2887415a46cd | 1276 | { |
Davidroid | 18:2d6ab2b93685 | 1277 | speed = device_prm.maxSpeed; |
Davidroid | 0:2887415a46cd | 1278 | } |
Davidroid | 18:2d6ab2b93685 | 1279 | device_prm.speed = speed; |
Davidroid | 18:2d6ab2b93685 | 1280 | L6474_ApplySpeed(device_prm.speed); |
Davidroid | 0:2887415a46cd | 1281 | } |
Davidroid | 0:2887415a46cd | 1282 | } |
Davidroid | 0:2887415a46cd | 1283 | break; |
Davidroid | 0:2887415a46cd | 1284 | } |
Davidroid | 0:2887415a46cd | 1285 | case STEADY: |
Davidroid | 0:2887415a46cd | 1286 | { |
Davidroid | 18:2d6ab2b93685 | 1287 | uint16_t maxSpeed = device_prm.maxSpeed; |
Davidroid | 18:2d6ab2b93685 | 1288 | uint32_t relativePos = device_prm.relativePos; |
Davidroid | 18:2d6ab2b93685 | 1289 | if ((device_prm.commandExecuted == SOFT_STOP_CMD)|| |
Davidroid | 18:2d6ab2b93685 | 1290 | ((device_prm.commandExecuted != RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1291 | (relativePos >= (device_prm.startDecPos))) || |
Davidroid | 18:2d6ab2b93685 | 1292 | ((device_prm.commandExecuted == RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1293 | (device_prm.speed > maxSpeed))) |
Davidroid | 0:2887415a46cd | 1294 | { |
Davidroid | 18:2d6ab2b93685 | 1295 | device_prm.motionState = DECELERATING; |
Davidroid | 18:2d6ab2b93685 | 1296 | device_prm.accu = 0; |
Davidroid | 0:2887415a46cd | 1297 | } |
Davidroid | 18:2d6ab2b93685 | 1298 | else if ((device_prm.commandExecuted == RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1299 | (device_prm.speed < maxSpeed)) |
Davidroid | 0:2887415a46cd | 1300 | { |
Davidroid | 18:2d6ab2b93685 | 1301 | device_prm.motionState = ACCELERATING; |
Davidroid | 18:2d6ab2b93685 | 1302 | device_prm.accu = 0; |
Davidroid | 0:2887415a46cd | 1303 | } |
Davidroid | 0:2887415a46cd | 1304 | break; |
Davidroid | 0:2887415a46cd | 1305 | } |
Davidroid | 0:2887415a46cd | 1306 | case DECELERATING: |
Davidroid | 0:2887415a46cd | 1307 | { |
Davidroid | 18:2d6ab2b93685 | 1308 | uint32_t relativePos = device_prm.relativePos; |
Davidroid | 18:2d6ab2b93685 | 1309 | uint16_t speed = device_prm.speed; |
Davidroid | 18:2d6ab2b93685 | 1310 | uint32_t deceleration = ((uint32_t)device_prm.deceleration << 16); |
Davidroid | 18:2d6ab2b93685 | 1311 | if (((device_prm.commandExecuted == SOFT_STOP_CMD)&&(speed <= device_prm.minSpeed))|| |
Davidroid | 18:2d6ab2b93685 | 1312 | ((device_prm.commandExecuted != RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1313 | (relativePos >= device_prm.stepsToTake))) |
Davidroid | 0:2887415a46cd | 1314 | { |
Davidroid | 0:2887415a46cd | 1315 | /* Motion process complete */ |
Davidroid | 0:2887415a46cd | 1316 | L6474_HardStop(); |
Davidroid | 0:2887415a46cd | 1317 | } |
Davidroid | 18:2d6ab2b93685 | 1318 | else if ((device_prm.commandExecuted == RUN_CMD)&& |
Davidroid | 18:2d6ab2b93685 | 1319 | (speed <= device_prm.maxSpeed)) |
Davidroid | 0:2887415a46cd | 1320 | { |
Davidroid | 18:2d6ab2b93685 | 1321 | device_prm.motionState = STEADY; |
Davidroid | 0:2887415a46cd | 1322 | } |
Davidroid | 0:2887415a46cd | 1323 | else |
Davidroid | 0:2887415a46cd | 1324 | { |
Davidroid | 0:2887415a46cd | 1325 | /* Go on decelerating */ |
Davidroid | 18:2d6ab2b93685 | 1326 | if (speed > device_prm.minSpeed) |
Davidroid | 0:2887415a46cd | 1327 | { |
Davidroid | 0:2887415a46cd | 1328 | bool speedUpdated = FALSE; |
Davidroid | 0:2887415a46cd | 1329 | if (speed == 0) speed =1; |
Davidroid | 18:2d6ab2b93685 | 1330 | device_prm.accu += deceleration / speed; |
Davidroid | 18:2d6ab2b93685 | 1331 | while (device_prm.accu >= (0X10000L)) |
Davidroid | 0:2887415a46cd | 1332 | { |
Davidroid | 18:2d6ab2b93685 | 1333 | device_prm.accu -= (0X10000L); |
Davidroid | 0:2887415a46cd | 1334 | if (speed > 1) |
Davidroid | 0:2887415a46cd | 1335 | { |
Davidroid | 0:2887415a46cd | 1336 | speed -=1; |
Davidroid | 0:2887415a46cd | 1337 | } |
Davidroid | 0:2887415a46cd | 1338 | speedUpdated = TRUE; |
Davidroid | 0:2887415a46cd | 1339 | } |
Davidroid | 0:2887415a46cd | 1340 | |
Davidroid | 0:2887415a46cd | 1341 | if (speedUpdated) |
Davidroid | 0:2887415a46cd | 1342 | { |
Davidroid | 18:2d6ab2b93685 | 1343 | if (speed < device_prm.minSpeed) |
Davidroid | 0:2887415a46cd | 1344 | { |
Davidroid | 18:2d6ab2b93685 | 1345 | speed = device_prm.minSpeed; |
Davidroid | 0:2887415a46cd | 1346 | } |
Davidroid | 18:2d6ab2b93685 | 1347 | device_prm.speed = speed; |
Davidroid | 18:2d6ab2b93685 | 1348 | L6474_ApplySpeed(device_prm.speed); |
Davidroid | 0:2887415a46cd | 1349 | } |
Davidroid | 0:2887415a46cd | 1350 | } |
Davidroid | 0:2887415a46cd | 1351 | } |
Davidroid | 0:2887415a46cd | 1352 | break; |
Davidroid | 0:2887415a46cd | 1353 | } |
Davidroid | 0:2887415a46cd | 1354 | default: |
Davidroid | 0:2887415a46cd | 1355 | { |
Davidroid | 0:2887415a46cd | 1356 | break; |
Davidroid | 0:2887415a46cd | 1357 | } |
Davidroid | 0:2887415a46cd | 1358 | } |
Davidroid | 0:2887415a46cd | 1359 | /* Set isr flag */ |
Davidroid | 18:2d6ab2b93685 | 1360 | isr_flag = FALSE; |
Davidroid | 18:2d6ab2b93685 | 1361 | } |
Davidroid | 18:2d6ab2b93685 | 1362 | |
Davidroid | 18:2d6ab2b93685 | 1363 | /********************************************************** |
Davidroid | 18:2d6ab2b93685 | 1364 | * @brief Converts current in mA to values for TVAL register |
Davidroid | 18:2d6ab2b93685 | 1365 | * @param[in] current_mA current in mA |
Davidroid | 18:2d6ab2b93685 | 1366 | * @retval value for TVAL register |
Davidroid | 18:2d6ab2b93685 | 1367 | **********************************************************/ |
Davidroid | 18:2d6ab2b93685 | 1368 | float L6474::L6474_Tval_Current_to_Par(float current_mA) |
Davidroid | 18:2d6ab2b93685 | 1369 | { |
Davidroid | 18:2d6ab2b93685 | 1370 | return ((float)(((current_mA - 31.25f) / 31.25f) + 0.5f)); |
Davidroid | 0:2887415a46cd | 1371 | } |
Davidroid | 0:2887415a46cd | 1372 | |
Davidroid | 5:d3c78f12a78d | 1373 | /********************************************************** |
Davidroid | 18:2d6ab2b93685 | 1374 | * @brief Converts values from TVAL register to mA |
Davidroid | 18:2d6ab2b93685 | 1375 | * @param[in] Tval value from TVAL register |
Davidroid | 18:2d6ab2b93685 | 1376 | * @retval current in mA |
Davidroid | 0:2887415a46cd | 1377 | **********************************************************/ |
Davidroid | 18:2d6ab2b93685 | 1378 | float L6474::L6474_Par_to_Tval_Current(float Tval) |
Davidroid | 0:2887415a46cd | 1379 | { |
Davidroid | 18:2d6ab2b93685 | 1380 | return ((float)((Tval - 0.5f) * 31.25f + 31.25f)); |
Davidroid | 0:2887415a46cd | 1381 | } |
Davidroid | 0:2887415a46cd | 1382 | |
Davidroid | 5:d3c78f12a78d | 1383 | /********************************************************** |
Davidroid | 18:2d6ab2b93685 | 1384 | * @brief Convert time in us to values for TON_MIN register |
Davidroid | 18:2d6ab2b93685 | 1385 | * @param[in] ton_min_us time in us |
Davidroid | 18:2d6ab2b93685 | 1386 | * @retval value for TON_MIN register |
Davidroid | 0:2887415a46cd | 1387 | **********************************************************/ |
Davidroid | 18:2d6ab2b93685 | 1388 | float L6474::L6474_Tmin_Time_to_Par(float ton_min_us) |
Davidroid | 0:2887415a46cd | 1389 | { |
Davidroid | 18:2d6ab2b93685 | 1390 | return ((float)(((ton_min_us - 0.5f) * 2.0f) + 0.5f)); |
Davidroid | 18:2d6ab2b93685 | 1391 | } |
Davidroid | 18:2d6ab2b93685 | 1392 | |
Davidroid | 18:2d6ab2b93685 | 1393 | /********************************************************** |
Davidroid | 18:2d6ab2b93685 | 1394 | * @brief Convert values for TON_MIN register to time in us |
Davidroid | 18:2d6ab2b93685 | 1395 | * @param[in] Tmin value from TON_MIN register |
Davidroid | 18:2d6ab2b93685 | 1396 | * @retval time in us |
Davidroid | 18:2d6ab2b93685 | 1397 | **********************************************************/ |
Davidroid | 18:2d6ab2b93685 | 1398 | float L6474::L6474_Par_to_Tmin_Time(float Tmin) |
Davidroid | 18:2d6ab2b93685 | 1399 | { |
Davidroid | 18:2d6ab2b93685 | 1400 | return ((float)(((Tmin - 0.5f) / 2.0f) + 0.5f)); |
Davidroid | 0:2887415a46cd | 1401 | } |
Davidroid | 0:2887415a46cd | 1402 | |
Davidroid | 5:d3c78f12a78d | 1403 | /********************************************************** |
Davidroid | 0:2887415a46cd | 1404 | * @brief Write and receive a byte via SPI |
Davidroid | 0:2887415a46cd | 1405 | * @param[in] pByteToTransmit pointer to the byte to transmit |
Davidroid | 0:2887415a46cd | 1406 | * @param[in] pReceivedByte pointer to the received byte |
Davidroid | 0:2887415a46cd | 1407 | * @retval None |
Davidroid | 0:2887415a46cd | 1408 | **********************************************************/ |
Davidroid | 0:2887415a46cd | 1409 | void L6474::L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) |
Davidroid | 0:2887415a46cd | 1410 | { |
Davidroid | 0:2887415a46cd | 1411 | if (L6474_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0) |
Davidroid | 0:2887415a46cd | 1412 | { |
Davidroid | 0:2887415a46cd | 1413 | L6474_ErrorHandler(L6474_ERROR_1); |
Davidroid | 0:2887415a46cd | 1414 | } |
Davidroid | 0:2887415a46cd | 1415 | |
Davidroid | 18:2d6ab2b93685 | 1416 | if (isr_flag) |
Davidroid | 0:2887415a46cd | 1417 | { |
Davidroid | 18:2d6ab2b93685 | 1418 | spi_preemtion_by_isr = TRUE; |
Davidroid | 0:2887415a46cd | 1419 | } |
Davidroid | 0:2887415a46cd | 1420 | } |
Davidroid | 0:2887415a46cd | 1421 | |
Davidroid | 0:2887415a46cd | 1422 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |