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