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