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