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