X-CUBE-SPN1-20150128 example source code for one motor compiled under mbed. Tested OK on Nucleo F401. l6474.cpp is modified from original with defines in l6474_target_config.h to select the original behaviour (motor de-energised when halted), or new mode to continue powering with a (reduced) current in the coils (braking/position hold capability). On F401 avoid using mbed's InterruptIn on pins 10-15 (any port). Beware of other conflicts! L0 & F0 are included but untested.
IHM01A1/l6474.cpp@6:19c1b4a04c24, 2015-10-13 (annotated)
- Committer:
- gregeric
- Date:
- Tue Oct 13 10:46:01 2015 +0000
- Revision:
- 6:19c1b4a04c24
- Parent:
- 0:b9444a40a999
Ensure bridge is disabled before resetting the L6474.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gregeric | 0:b9444a40a999 | 1 | /** |
gregeric | 0:b9444a40a999 | 2 | ****************************************************************************** |
gregeric | 0:b9444a40a999 | 3 | * @file l6474.c |
gregeric | 0:b9444a40a999 | 4 | * @author IPC Rennes |
gregeric | 0:b9444a40a999 | 5 | * @version V1.5.0 |
gregeric | 0:b9444a40a999 | 6 | * @date November 12, 2014 |
gregeric | 0:b9444a40a999 | 7 | * @brief L6474 driver (fully integrated microstepping motor driver) |
gregeric | 0:b9444a40a999 | 8 | * @note (C) COPYRIGHT 2014 STMicroelectronics |
gregeric | 0:b9444a40a999 | 9 | ****************************************************************************** |
gregeric | 0:b9444a40a999 | 10 | * @attention |
gregeric | 0:b9444a40a999 | 11 | * |
gregeric | 0:b9444a40a999 | 12 | * <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2> |
gregeric | 0:b9444a40a999 | 13 | * |
gregeric | 0:b9444a40a999 | 14 | * Redistribution and use in source and binary forms, with or without modification, |
gregeric | 0:b9444a40a999 | 15 | * are permitted provided that the following conditions are met: |
gregeric | 0:b9444a40a999 | 16 | * 1. Redistributions of source code must retain the above copyright notice, |
gregeric | 0:b9444a40a999 | 17 | * this list of conditions and the following disclaimer. |
gregeric | 0:b9444a40a999 | 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
gregeric | 0:b9444a40a999 | 19 | * this list of conditions and the following disclaimer in the documentation |
gregeric | 0:b9444a40a999 | 20 | * and/or other materials provided with the distribution. |
gregeric | 0:b9444a40a999 | 21 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
gregeric | 0:b9444a40a999 | 22 | * may be used to endorse or promote products derived from this software |
gregeric | 0:b9444a40a999 | 23 | * without specific prior written permission. |
gregeric | 0:b9444a40a999 | 24 | * |
gregeric | 0:b9444a40a999 | 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
gregeric | 0:b9444a40a999 | 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
gregeric | 0:b9444a40a999 | 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
gregeric | 0:b9444a40a999 | 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
gregeric | 0:b9444a40a999 | 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
gregeric | 0:b9444a40a999 | 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
gregeric | 0:b9444a40a999 | 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
gregeric | 0:b9444a40a999 | 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
gregeric | 0:b9444a40a999 | 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
gregeric | 0:b9444a40a999 | 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
gregeric | 0:b9444a40a999 | 35 | * |
gregeric | 0:b9444a40a999 | 36 | ****************************************************************************** |
gregeric | 0:b9444a40a999 | 37 | */ |
gregeric | 0:b9444a40a999 | 38 | |
gregeric | 0:b9444a40a999 | 39 | /* Includes ------------------------------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 40 | #include "l6474.h" |
gregeric | 0:b9444a40a999 | 41 | |
gregeric | 0:b9444a40a999 | 42 | /* Private constants ---------------------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 43 | |
gregeric | 0:b9444a40a999 | 44 | |
gregeric | 0:b9444a40a999 | 45 | /** @addtogroup BSP |
gregeric | 0:b9444a40a999 | 46 | * @{ |
gregeric | 0:b9444a40a999 | 47 | */ |
gregeric | 0:b9444a40a999 | 48 | |
gregeric | 0:b9444a40a999 | 49 | /** @defgroup L6474 |
gregeric | 0:b9444a40a999 | 50 | * @{ |
gregeric | 0:b9444a40a999 | 51 | */ |
gregeric | 0:b9444a40a999 | 52 | |
gregeric | 0:b9444a40a999 | 53 | /* Private constants ---------------------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 54 | |
gregeric | 0:b9444a40a999 | 55 | /** @defgroup L6474_Private_Constants |
gregeric | 0:b9444a40a999 | 56 | * @{ |
gregeric | 0:b9444a40a999 | 57 | */ |
gregeric | 0:b9444a40a999 | 58 | |
gregeric | 0:b9444a40a999 | 59 | /// Error while initialising the SPI |
gregeric | 0:b9444a40a999 | 60 | #define L6474_ERROR_0 (0x8000) |
gregeric | 0:b9444a40a999 | 61 | /// Error: Bad SPI transaction |
gregeric | 0:b9444a40a999 | 62 | #define L6474_ERROR_1 (0x8001) |
gregeric | 0:b9444a40a999 | 63 | |
gregeric | 0:b9444a40a999 | 64 | /// Maximum number of steps |
gregeric | 0:b9444a40a999 | 65 | #define MAX_STEPS (0x7FFFFFFF) |
gregeric | 0:b9444a40a999 | 66 | |
gregeric | 0:b9444a40a999 | 67 | /// Maximum frequency of the PWMs in Hz |
gregeric | 0:b9444a40a999 | 68 | #define L6474_MAX_PWM_FREQ (10000) |
gregeric | 0:b9444a40a999 | 69 | |
gregeric | 0:b9444a40a999 | 70 | /// Minimum frequency of the PWMs in Hz |
gregeric | 0:b9444a40a999 | 71 | #define L6474_MIN_PWM_FREQ (2) |
gregeric | 0:b9444a40a999 | 72 | |
gregeric | 0:b9444a40a999 | 73 | /** |
gregeric | 0:b9444a40a999 | 74 | * @} |
gregeric | 0:b9444a40a999 | 75 | */ |
gregeric | 0:b9444a40a999 | 76 | |
gregeric | 0:b9444a40a999 | 77 | /* Private variables ---------------------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 78 | |
gregeric | 0:b9444a40a999 | 79 | /** @defgroup L6474_Private_Variables |
gregeric | 0:b9444a40a999 | 80 | * @{ |
gregeric | 0:b9444a40a999 | 81 | */ |
gregeric | 0:b9444a40a999 | 82 | |
gregeric | 0:b9444a40a999 | 83 | /// Function pointer to flag interrupt call back |
gregeric | 0:b9444a40a999 | 84 | void (*flagInterruptCallback)(void); |
gregeric | 0:b9444a40a999 | 85 | /// Function pointer to error handler call back |
gregeric | 0:b9444a40a999 | 86 | void (*errorHandlerCallback)(uint16_t); |
gregeric | 0:b9444a40a999 | 87 | static volatile uint8_t numberOfDevices; |
gregeric | 0:b9444a40a999 | 88 | static uint8_t spiTxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; |
gregeric | 0:b9444a40a999 | 89 | static uint8_t spiRxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; |
gregeric | 0:b9444a40a999 | 90 | static volatile bool spiPreemtionByIsr = FALSE; |
gregeric | 0:b9444a40a999 | 91 | static volatile bool isrFlag = FALSE; |
gregeric | 0:b9444a40a999 | 92 | static uint16_t l6474DriverInstance = 0; |
gregeric | 0:b9444a40a999 | 93 | |
gregeric | 0:b9444a40a999 | 94 | /// L6474 Device Paramaters structure |
gregeric | 0:b9444a40a999 | 95 | deviceParams_t devicePrm[MAX_NUMBER_OF_DEVICES]; |
gregeric | 0:b9444a40a999 | 96 | |
gregeric | 0:b9444a40a999 | 97 | |
gregeric | 0:b9444a40a999 | 98 | /** |
gregeric | 0:b9444a40a999 | 99 | * @} |
gregeric | 0:b9444a40a999 | 100 | */ |
gregeric | 0:b9444a40a999 | 101 | |
gregeric | 0:b9444a40a999 | 102 | /* Private function prototypes -----------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 103 | |
gregeric | 0:b9444a40a999 | 104 | /** @defgroup L6474_Private_functions |
gregeric | 0:b9444a40a999 | 105 | * @{ |
gregeric | 0:b9444a40a999 | 106 | */ |
gregeric | 0:b9444a40a999 | 107 | void L6474_ApplySpeed(uint8_t pwmId, uint16_t newSpeed); |
gregeric | 0:b9444a40a999 | 108 | void L6474_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps); |
gregeric | 0:b9444a40a999 | 109 | int32_t L6474_ConvertPosition(uint32_t abs_position_reg); |
gregeric | 0:b9444a40a999 | 110 | void L6474_ErrorHandler(uint16_t error); |
gregeric | 0:b9444a40a999 | 111 | void L6474_FlagInterruptHandler(void); |
gregeric | 0:b9444a40a999 | 112 | void L6474_SendCommand(uint8_t deviceId, uint8_t param); |
gregeric | 0:b9444a40a999 | 113 | void L6474_SetRegisterToPredefinedValues(uint8_t deviceId); |
gregeric | 0:b9444a40a999 | 114 | void L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte); |
gregeric | 0:b9444a40a999 | 115 | void L6474_SetDeviceParamsToPredefinedValues(void); |
gregeric | 0:b9444a40a999 | 116 | void L6474_StartMovement(uint8_t deviceId); |
gregeric | 0:b9444a40a999 | 117 | void L6474_StepClockHandler(uint8_t deviceId); |
gregeric | 0:b9444a40a999 | 118 | uint8_t L6474_Tval_Current_to_Par(double Tval); |
gregeric | 0:b9444a40a999 | 119 | uint8_t L6474_Tmin_Time_to_Par(double Tmin); |
gregeric | 0:b9444a40a999 | 120 | |
gregeric | 0:b9444a40a999 | 121 | /** |
gregeric | 0:b9444a40a999 | 122 | * @} |
gregeric | 0:b9444a40a999 | 123 | */ |
gregeric | 0:b9444a40a999 | 124 | |
gregeric | 0:b9444a40a999 | 125 | |
gregeric | 0:b9444a40a999 | 126 | /** @defgroup L6474_Exported_Variables |
gregeric | 0:b9444a40a999 | 127 | * @{ |
gregeric | 0:b9444a40a999 | 128 | */ |
gregeric | 0:b9444a40a999 | 129 | |
gregeric | 0:b9444a40a999 | 130 | /// L6474 motor driver functions pointer structure |
gregeric | 0:b9444a40a999 | 131 | motorDrv_t l6474Drv = |
gregeric | 0:b9444a40a999 | 132 | { |
gregeric | 0:b9444a40a999 | 133 | L6474_Init, |
gregeric | 0:b9444a40a999 | 134 | L6474_ReadId, |
gregeric | 0:b9444a40a999 | 135 | L6474_AttachErrorHandler, |
gregeric | 0:b9444a40a999 | 136 | L6474_AttachFlagInterrupt, |
gregeric | 0:b9444a40a999 | 137 | 0, |
gregeric | 0:b9444a40a999 | 138 | L6474_FlagInterruptHandler, |
gregeric | 0:b9444a40a999 | 139 | L6474_GetAcceleration, |
gregeric | 0:b9444a40a999 | 140 | L6474_GetCurrentSpeed, |
gregeric | 0:b9444a40a999 | 141 | L6474_GetDeceleration, |
gregeric | 0:b9444a40a999 | 142 | L6474_GetDeviceState, |
gregeric | 0:b9444a40a999 | 143 | L6474_GetFwVersion, |
gregeric | 0:b9444a40a999 | 144 | L6474_GetMark, |
gregeric | 0:b9444a40a999 | 145 | L6474_GetMaxSpeed, |
gregeric | 0:b9444a40a999 | 146 | L6474_GetMinSpeed, |
gregeric | 0:b9444a40a999 | 147 | L6474_GetPosition, |
gregeric | 0:b9444a40a999 | 148 | L6474_GoHome, |
gregeric | 0:b9444a40a999 | 149 | L6474_GoMark, |
gregeric | 0:b9444a40a999 | 150 | L6474_GoTo, |
gregeric | 0:b9444a40a999 | 151 | L6474_HardStop, |
gregeric | 0:b9444a40a999 | 152 | L6474_Move, |
gregeric | 0:b9444a40a999 | 153 | L6474_ResetAllDevices, |
gregeric | 0:b9444a40a999 | 154 | L6474_Run, |
gregeric | 0:b9444a40a999 | 155 | L6474_SetAcceleration, |
gregeric | 0:b9444a40a999 | 156 | L6474_SetDeceleration, |
gregeric | 0:b9444a40a999 | 157 | L6474_SetHome, |
gregeric | 0:b9444a40a999 | 158 | L6474_SetMark, |
gregeric | 0:b9444a40a999 | 159 | L6474_SetMaxSpeed, |
gregeric | 0:b9444a40a999 | 160 | L6474_SetMinSpeed, |
gregeric | 0:b9444a40a999 | 161 | L6474_SoftStop, |
gregeric | 0:b9444a40a999 | 162 | L6474_StepClockHandler, |
gregeric | 0:b9444a40a999 | 163 | L6474_WaitWhileActive, |
gregeric | 0:b9444a40a999 | 164 | L6474_CmdDisable, |
gregeric | 0:b9444a40a999 | 165 | L6474_CmdEnable, |
gregeric | 0:b9444a40a999 | 166 | L6474_CmdGetParam, |
gregeric | 0:b9444a40a999 | 167 | L6474_CmdGetStatus, |
gregeric | 0:b9444a40a999 | 168 | L6474_CmdNop, |
gregeric | 0:b9444a40a999 | 169 | L6474_CmdSetParam, |
gregeric | 0:b9444a40a999 | 170 | L6474_ReadStatusRegister, |
gregeric | 0:b9444a40a999 | 171 | L6474_ReleaseReset, |
gregeric | 0:b9444a40a999 | 172 | L6474_Reset, |
gregeric | 0:b9444a40a999 | 173 | L6474_SelectStepMode, |
gregeric | 0:b9444a40a999 | 174 | L6474_SetDirection, |
gregeric | 0:b9444a40a999 | 175 | 0, |
gregeric | 0:b9444a40a999 | 176 | 0, |
gregeric | 0:b9444a40a999 | 177 | 0, |
gregeric | 0:b9444a40a999 | 178 | 0, |
gregeric | 0:b9444a40a999 | 179 | 0, |
gregeric | 0:b9444a40a999 | 180 | 0, |
gregeric | 0:b9444a40a999 | 181 | 0, |
gregeric | 0:b9444a40a999 | 182 | 0, |
gregeric | 0:b9444a40a999 | 183 | 0, |
gregeric | 0:b9444a40a999 | 184 | 0, |
gregeric | 0:b9444a40a999 | 185 | 0, |
gregeric | 0:b9444a40a999 | 186 | 0, |
gregeric | 0:b9444a40a999 | 187 | 0, |
gregeric | 0:b9444a40a999 | 188 | 0, |
gregeric | 0:b9444a40a999 | 189 | 0, |
gregeric | 0:b9444a40a999 | 190 | 0, |
gregeric | 0:b9444a40a999 | 191 | 0, |
gregeric | 0:b9444a40a999 | 192 | 0, |
gregeric | 0:b9444a40a999 | 193 | L6474_ErrorHandler, |
gregeric | 0:b9444a40a999 | 194 | 0 |
gregeric | 0:b9444a40a999 | 195 | }; |
gregeric | 0:b9444a40a999 | 196 | |
gregeric | 0:b9444a40a999 | 197 | /** |
gregeric | 0:b9444a40a999 | 198 | * @} |
gregeric | 0:b9444a40a999 | 199 | */ |
gregeric | 0:b9444a40a999 | 200 | |
gregeric | 0:b9444a40a999 | 201 | /** @defgroup Device_Control_Functions |
gregeric | 0:b9444a40a999 | 202 | * @{ |
gregeric | 0:b9444a40a999 | 203 | */ |
gregeric | 0:b9444a40a999 | 204 | |
gregeric | 0:b9444a40a999 | 205 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 206 | * @brief Attaches a user callback to the error Handler. |
gregeric | 0:b9444a40a999 | 207 | * The call back will be then called each time the library |
gregeric | 0:b9444a40a999 | 208 | * detects an error |
gregeric | 0:b9444a40a999 | 209 | * @param[in] callback Name of the callback to attach |
gregeric | 0:b9444a40a999 | 210 | * to the error Hanlder |
gregeric | 0:b9444a40a999 | 211 | * @retval None |
gregeric | 0:b9444a40a999 | 212 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 213 | void L6474_AttachErrorHandler(void (*callback)(uint16_t)) |
gregeric | 0:b9444a40a999 | 214 | { |
gregeric | 0:b9444a40a999 | 215 | errorHandlerCallback = (void (*)(uint16_t))callback; |
gregeric | 0:b9444a40a999 | 216 | } |
gregeric | 0:b9444a40a999 | 217 | |
gregeric | 0:b9444a40a999 | 218 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 219 | * @brief Attaches a user callback to the flag Interrupt |
gregeric | 0:b9444a40a999 | 220 | * The call back will be then called each time the status |
gregeric | 0:b9444a40a999 | 221 | * flag pin will be pulled down due to the occurrence of |
gregeric | 0:b9444a40a999 | 222 | * a programmed alarms ( OCD, thermal pre-warning or |
gregeric | 0:b9444a40a999 | 223 | * shutdown, UVLO, wrong command, non-performable command) |
gregeric | 0:b9444a40a999 | 224 | * @param[in] callback Name of the callback to attach |
gregeric | 0:b9444a40a999 | 225 | * to the Flag Interrupt |
gregeric | 0:b9444a40a999 | 226 | * @retval None |
gregeric | 0:b9444a40a999 | 227 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 228 | void L6474_AttachFlagInterrupt(void (*callback)(void)) |
gregeric | 0:b9444a40a999 | 229 | { |
gregeric | 0:b9444a40a999 | 230 | flagInterruptCallback = (void (*)())callback; |
gregeric | 0:b9444a40a999 | 231 | } |
gregeric | 0:b9444a40a999 | 232 | |
gregeric | 0:b9444a40a999 | 233 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 234 | * @brief Starts the L6474 library |
gregeric | 0:b9444a40a999 | 235 | * @param[in] nbDevices Number of L6474 devices to use (from 1 to 3) |
gregeric | 0:b9444a40a999 | 236 | * @retval None |
gregeric | 0:b9444a40a999 | 237 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 238 | void L6474_Init(uint8_t nbDevices) |
gregeric | 0:b9444a40a999 | 239 | { |
gregeric | 0:b9444a40a999 | 240 | uint32_t i; |
gregeric | 0:b9444a40a999 | 241 | numberOfDevices = nbDevices; |
gregeric | 0:b9444a40a999 | 242 | |
gregeric | 0:b9444a40a999 | 243 | l6474DriverInstance++; |
gregeric | 0:b9444a40a999 | 244 | |
gregeric | 0:b9444a40a999 | 245 | /* Initialise the GPIOs */ |
gregeric | 0:b9444a40a999 | 246 | BSP_MotorControlBoard_GpioInit(nbDevices); |
gregeric | 0:b9444a40a999 | 247 | |
gregeric | 0:b9444a40a999 | 248 | if(BSP_MotorControlBoard_SpiInit() != 0) |
gregeric | 0:b9444a40a999 | 249 | { |
gregeric | 0:b9444a40a999 | 250 | /* Initialization Error */ |
gregeric | 0:b9444a40a999 | 251 | L6474_ErrorHandler(L6474_ERROR_0); |
gregeric | 0:b9444a40a999 | 252 | } |
gregeric | 0:b9444a40a999 | 253 | |
gregeric | 0:b9444a40a999 | 254 | /* Initialise the PWMs used for the Step clocks ----------------------------*/ |
gregeric | 0:b9444a40a999 | 255 | switch (nbDevices) |
gregeric | 0:b9444a40a999 | 256 | { |
gregeric | 0:b9444a40a999 | 257 | case 3: |
gregeric | 0:b9444a40a999 | 258 | BSP_MotorControlBoard_PwmInit(2); |
gregeric | 0:b9444a40a999 | 259 | case 2: |
gregeric | 0:b9444a40a999 | 260 | BSP_MotorControlBoard_PwmInit(1); |
gregeric | 0:b9444a40a999 | 261 | case 1: |
gregeric | 0:b9444a40a999 | 262 | BSP_MotorControlBoard_PwmInit(0); |
gregeric | 0:b9444a40a999 | 263 | default: |
gregeric | 0:b9444a40a999 | 264 | ; |
gregeric | 0:b9444a40a999 | 265 | } |
gregeric | 0:b9444a40a999 | 266 | |
gregeric | 0:b9444a40a999 | 267 | /* Initialise the L6474s ------------------------------------------------*/ |
gregeric | 0:b9444a40a999 | 268 | |
gregeric | 0:b9444a40a999 | 269 | /* Standby-reset deactivation */ |
gregeric | 0:b9444a40a999 | 270 | BSP_MotorControlBoard_ReleaseReset(); |
gregeric | 0:b9444a40a999 | 271 | |
gregeric | 0:b9444a40a999 | 272 | /* Let a delay after reset */ |
gregeric | 0:b9444a40a999 | 273 | BSP_MotorControlBoard_Delay(1); |
gregeric | 0:b9444a40a999 | 274 | |
gregeric | 0:b9444a40a999 | 275 | /* Set all registers and context variables to the predefined values from l6474_target_config.h */ |
gregeric | 0:b9444a40a999 | 276 | L6474_SetDeviceParamsToPredefinedValues(); |
gregeric | 0:b9444a40a999 | 277 | |
gregeric | 0:b9444a40a999 | 278 | /* Disable L6474 powerstage */ |
gregeric | 0:b9444a40a999 | 279 | for (i = 0; i < nbDevices; i++) |
gregeric | 0:b9444a40a999 | 280 | { |
gregeric | 0:b9444a40a999 | 281 | L6474_CmdDisable(i); |
gregeric | 0:b9444a40a999 | 282 | /* Get Status to clear flags after start up */ |
gregeric | 0:b9444a40a999 | 283 | L6474_CmdGetStatus(i); |
gregeric | 0:b9444a40a999 | 284 | } |
gregeric | 0:b9444a40a999 | 285 | } |
gregeric | 0:b9444a40a999 | 286 | |
gregeric | 0:b9444a40a999 | 287 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 288 | * @brief Returns the acceleration of the specified device |
gregeric | 0:b9444a40a999 | 289 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 290 | * @retval Acceleration in pps^2 |
gregeric | 0:b9444a40a999 | 291 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 292 | uint16_t L6474_GetAcceleration(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 293 | { |
gregeric | 0:b9444a40a999 | 294 | return (devicePrm[deviceId].acceleration); |
gregeric | 0:b9444a40a999 | 295 | } |
gregeric | 0:b9444a40a999 | 296 | |
gregeric | 0:b9444a40a999 | 297 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 298 | * @brief Returns the current speed of the specified device |
gregeric | 0:b9444a40a999 | 299 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 300 | * @retval Speed in pps |
gregeric | 0:b9444a40a999 | 301 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 302 | uint16_t L6474_GetCurrentSpeed(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 303 | { |
gregeric | 0:b9444a40a999 | 304 | return devicePrm[deviceId].speed; |
gregeric | 0:b9444a40a999 | 305 | } |
gregeric | 0:b9444a40a999 | 306 | |
gregeric | 0:b9444a40a999 | 307 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 308 | * @brief Returns the deceleration of the specified device |
gregeric | 0:b9444a40a999 | 309 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 310 | * @retval Deceleration in pps^2 |
gregeric | 0:b9444a40a999 | 311 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 312 | uint16_t L6474_GetDeceleration(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 313 | { |
gregeric | 0:b9444a40a999 | 314 | return (devicePrm[deviceId].deceleration); |
gregeric | 0:b9444a40a999 | 315 | } |
gregeric | 0:b9444a40a999 | 316 | |
gregeric | 0:b9444a40a999 | 317 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 318 | * @brief Returns the device state |
gregeric | 0:b9444a40a999 | 319 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 320 | * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) |
gregeric | 0:b9444a40a999 | 321 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 322 | motorState_t L6474_GetDeviceState(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 323 | { |
gregeric | 0:b9444a40a999 | 324 | return devicePrm[deviceId].motionState; |
gregeric | 0:b9444a40a999 | 325 | } |
gregeric | 0:b9444a40a999 | 326 | |
gregeric | 0:b9444a40a999 | 327 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 328 | * @brief Returns the FW version of the library |
gregeric | 0:b9444a40a999 | 329 | * @param None |
gregeric | 0:b9444a40a999 | 330 | * @retval L6474_FW_VERSION |
gregeric | 0:b9444a40a999 | 331 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 332 | uint8_t L6474_GetFwVersion(void) |
gregeric | 0:b9444a40a999 | 333 | { |
gregeric | 0:b9444a40a999 | 334 | return (L6474_FW_VERSION); |
gregeric | 0:b9444a40a999 | 335 | } |
gregeric | 0:b9444a40a999 | 336 | |
gregeric | 0:b9444a40a999 | 337 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 338 | * @brief Return motor handle (pointer to the L6474 motor driver structure) |
gregeric | 0:b9444a40a999 | 339 | * @param None |
gregeric | 0:b9444a40a999 | 340 | * @retval Pointer to the motorDrv_t structure |
gregeric | 0:b9444a40a999 | 341 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 342 | motorDrv_t* L6474_GetMotorHandle(void) |
gregeric | 0:b9444a40a999 | 343 | { |
gregeric | 0:b9444a40a999 | 344 | return (&l6474Drv); |
gregeric | 0:b9444a40a999 | 345 | } |
gregeric | 0:b9444a40a999 | 346 | |
gregeric | 0:b9444a40a999 | 347 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 348 | * @brief Returns the mark position of the specified device |
gregeric | 0:b9444a40a999 | 349 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 350 | * @retval Mark register value converted in a 32b signed integer |
gregeric | 0:b9444a40a999 | 351 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 352 | int32_t L6474_GetMark(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 353 | { |
gregeric | 0:b9444a40a999 | 354 | return L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_MARK)); |
gregeric | 0:b9444a40a999 | 355 | } |
gregeric | 0:b9444a40a999 | 356 | |
gregeric | 0:b9444a40a999 | 357 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 358 | * @brief Returns the max speed of the specified device |
gregeric | 0:b9444a40a999 | 359 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 360 | * @retval maxSpeed in pps |
gregeric | 0:b9444a40a999 | 361 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 362 | uint16_t L6474_GetMaxSpeed(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 363 | { |
gregeric | 0:b9444a40a999 | 364 | return (devicePrm[deviceId].maxSpeed); |
gregeric | 0:b9444a40a999 | 365 | } |
gregeric | 0:b9444a40a999 | 366 | |
gregeric | 0:b9444a40a999 | 367 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 368 | * @brief Returns the min speed of the specified device |
gregeric | 0:b9444a40a999 | 369 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 370 | * @retval minSpeed in pps |
gregeric | 0:b9444a40a999 | 371 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 372 | uint16_t L6474_GetMinSpeed(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 373 | { |
gregeric | 0:b9444a40a999 | 374 | return (devicePrm[deviceId].minSpeed); |
gregeric | 0:b9444a40a999 | 375 | } |
gregeric | 0:b9444a40a999 | 376 | |
gregeric | 0:b9444a40a999 | 377 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 378 | * @brief Returns the ABS_POSITION of the specified device |
gregeric | 0:b9444a40a999 | 379 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 380 | * @retval ABS_POSITION register value converted in a 32b signed integer |
gregeric | 0:b9444a40a999 | 381 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 382 | int32_t L6474_GetPosition(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 383 | { |
gregeric | 0:b9444a40a999 | 384 | return L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); |
gregeric | 0:b9444a40a999 | 385 | } |
gregeric | 0:b9444a40a999 | 386 | |
gregeric | 0:b9444a40a999 | 387 | |
gregeric | 0:b9444a40a999 | 388 | |
gregeric | 0:b9444a40a999 | 389 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 390 | * @brief Requests the motor to move to the home position (ABS_POSITION = 0) |
gregeric | 0:b9444a40a999 | 391 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 392 | * @retval None |
gregeric | 0:b9444a40a999 | 393 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 394 | void L6474_GoHome(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 395 | { |
gregeric | 0:b9444a40a999 | 396 | L6474_GoTo(deviceId, 0); |
gregeric | 0:b9444a40a999 | 397 | } |
gregeric | 0:b9444a40a999 | 398 | |
gregeric | 0:b9444a40a999 | 399 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 400 | * @brief Requests the motor to move to the mark position |
gregeric | 0:b9444a40a999 | 401 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 402 | * @retval None |
gregeric | 0:b9444a40a999 | 403 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 404 | void L6474_GoMark(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 405 | { |
gregeric | 0:b9444a40a999 | 406 | uint32_t mark; |
gregeric | 0:b9444a40a999 | 407 | |
gregeric | 0:b9444a40a999 | 408 | mark = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_MARK)); |
gregeric | 0:b9444a40a999 | 409 | L6474_GoTo(deviceId,mark); |
gregeric | 0:b9444a40a999 | 410 | } |
gregeric | 0:b9444a40a999 | 411 | |
gregeric | 0:b9444a40a999 | 412 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 413 | * @brief Requests the motor to move to the specified position |
gregeric | 0:b9444a40a999 | 414 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 415 | * @param[in] targetPosition absolute position in steps |
gregeric | 0:b9444a40a999 | 416 | * @retval None |
gregeric | 0:b9444a40a999 | 417 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 418 | void L6474_GoTo(uint8_t deviceId, int32_t targetPosition) |
gregeric | 0:b9444a40a999 | 419 | { |
gregeric | 0:b9444a40a999 | 420 | motorDir_t direction; |
gregeric | 0:b9444a40a999 | 421 | int32_t steps; |
gregeric | 0:b9444a40a999 | 422 | |
gregeric | 0:b9444a40a999 | 423 | /* Eventually deactivate motor */ |
gregeric | 0:b9444a40a999 | 424 | if (devicePrm[deviceId].motionState != INACTIVE) |
gregeric | 0:b9444a40a999 | 425 | { |
gregeric | 0:b9444a40a999 | 426 | L6474_HardStop(deviceId); |
gregeric | 0:b9444a40a999 | 427 | } |
gregeric | 0:b9444a40a999 | 428 | |
gregeric | 0:b9444a40a999 | 429 | /* Get current position */ |
gregeric | 0:b9444a40a999 | 430 | devicePrm[deviceId].currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); |
gregeric | 0:b9444a40a999 | 431 | |
gregeric | 0:b9444a40a999 | 432 | /* Compute the number of steps to perform */ |
gregeric | 0:b9444a40a999 | 433 | steps = targetPosition - devicePrm[deviceId].currentPosition; |
gregeric | 0:b9444a40a999 | 434 | |
gregeric | 0:b9444a40a999 | 435 | if (steps >= 0) |
gregeric | 0:b9444a40a999 | 436 | { |
gregeric | 0:b9444a40a999 | 437 | devicePrm[deviceId].stepsToTake = steps; |
gregeric | 0:b9444a40a999 | 438 | direction = FORWARD; |
gregeric | 0:b9444a40a999 | 439 | |
gregeric | 0:b9444a40a999 | 440 | } |
gregeric | 0:b9444a40a999 | 441 | else |
gregeric | 0:b9444a40a999 | 442 | { |
gregeric | 0:b9444a40a999 | 443 | devicePrm[deviceId].stepsToTake = -steps; |
gregeric | 0:b9444a40a999 | 444 | direction = BACKWARD; |
gregeric | 0:b9444a40a999 | 445 | } |
gregeric | 0:b9444a40a999 | 446 | |
gregeric | 0:b9444a40a999 | 447 | if (steps != 0) |
gregeric | 0:b9444a40a999 | 448 | { |
gregeric | 0:b9444a40a999 | 449 | |
gregeric | 0:b9444a40a999 | 450 | devicePrm[deviceId].commandExecuted = MOVE_CMD; |
gregeric | 0:b9444a40a999 | 451 | |
gregeric | 0:b9444a40a999 | 452 | /* Direction setup */ |
gregeric | 0:b9444a40a999 | 453 | L6474_SetDirection(deviceId,direction); |
gregeric | 0:b9444a40a999 | 454 | |
gregeric | 0:b9444a40a999 | 455 | L6474_ComputeSpeedProfile(deviceId, devicePrm[deviceId].stepsToTake); |
gregeric | 0:b9444a40a999 | 456 | |
gregeric | 0:b9444a40a999 | 457 | /* Motor activation */ |
gregeric | 0:b9444a40a999 | 458 | L6474_StartMovement(deviceId); |
gregeric | 0:b9444a40a999 | 459 | } |
gregeric | 0:b9444a40a999 | 460 | } |
gregeric | 0:b9444a40a999 | 461 | |
gregeric | 0:b9444a40a999 | 462 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 463 | * @brief Immediatly stops the motor and disable the power bridge |
gregeric | 0:b9444a40a999 | 464 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 465 | * @retval None |
gregeric | 0:b9444a40a999 | 466 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 467 | void L6474_HardStop(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 468 | { |
gregeric | 0:b9444a40a999 | 469 | /* Disable corresponding PWM */ |
gregeric | 0:b9444a40a999 | 470 | BSP_MotorControlBoard_PwmStop(deviceId); |
gregeric | 0:b9444a40a999 | 471 | |
gregeric | 0:b9444a40a999 | 472 | /* Disable power stage */ |
gregeric | 0:b9444a40a999 | 473 | #ifndef L6474_CONF_BRAKE_WHEN_HALTED |
gregeric | 0:b9444a40a999 | 474 | L6474_CmdDisable(deviceId); |
gregeric | 0:b9444a40a999 | 475 | #else |
gregeric | 0:b9444a40a999 | 476 | /* Set powerstage to reduced current, retaining position */ |
gregeric | 0:b9444a40a999 | 477 | switch (deviceId) |
gregeric | 0:b9444a40a999 | 478 | { |
gregeric | 0:b9444a40a999 | 479 | case 0: |
gregeric | 0:b9444a40a999 | 480 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 481 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 482 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0/L6474_CONF_BRAKE_CURRENT_FACTOR)); |
gregeric | 0:b9444a40a999 | 483 | break; |
gregeric | 0:b9444a40a999 | 484 | case 1: |
gregeric | 0:b9444a40a999 | 485 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 486 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 487 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1/L6474_CONF_BRAKE_CURRENT_FACTOR)); |
gregeric | 0:b9444a40a999 | 488 | break; |
gregeric | 0:b9444a40a999 | 489 | case 2: |
gregeric | 0:b9444a40a999 | 490 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 491 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 492 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2/L6474_CONF_BRAKE_CURRENT_FACTOR)); |
gregeric | 0:b9444a40a999 | 493 | break; |
gregeric | 0:b9444a40a999 | 494 | default: ; |
gregeric | 0:b9444a40a999 | 495 | } |
gregeric | 0:b9444a40a999 | 496 | #endif |
gregeric | 0:b9444a40a999 | 497 | /* Set inactive state */ |
gregeric | 0:b9444a40a999 | 498 | devicePrm[deviceId].motionState = INACTIVE; |
gregeric | 0:b9444a40a999 | 499 | devicePrm[deviceId].commandExecuted = NO_CMD; |
gregeric | 0:b9444a40a999 | 500 | devicePrm[deviceId].stepsToTake = MAX_STEPS; |
gregeric | 0:b9444a40a999 | 501 | } |
gregeric | 0:b9444a40a999 | 502 | |
gregeric | 0:b9444a40a999 | 503 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 504 | * @brief Moves the motor of the specified number of steps |
gregeric | 0:b9444a40a999 | 505 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 506 | * @param[in] direction FORWARD or BACKWARD |
gregeric | 0:b9444a40a999 | 507 | * @param[in] stepCount Number of steps to perform |
gregeric | 0:b9444a40a999 | 508 | * @retval None |
gregeric | 0:b9444a40a999 | 509 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 510 | void L6474_Move(uint8_t deviceId, motorDir_t direction, uint32_t stepCount) |
gregeric | 0:b9444a40a999 | 511 | { |
gregeric | 0:b9444a40a999 | 512 | /* Eventually deactivate motor */ |
gregeric | 0:b9444a40a999 | 513 | if (devicePrm[deviceId].motionState != INACTIVE) |
gregeric | 0:b9444a40a999 | 514 | { |
gregeric | 0:b9444a40a999 | 515 | L6474_HardStop(deviceId); |
gregeric | 0:b9444a40a999 | 516 | } |
gregeric | 0:b9444a40a999 | 517 | |
gregeric | 0:b9444a40a999 | 518 | if (stepCount != 0) |
gregeric | 0:b9444a40a999 | 519 | { |
gregeric | 0:b9444a40a999 | 520 | devicePrm[deviceId].stepsToTake = stepCount; |
gregeric | 0:b9444a40a999 | 521 | |
gregeric | 0:b9444a40a999 | 522 | devicePrm[deviceId].commandExecuted = MOVE_CMD; |
gregeric | 0:b9444a40a999 | 523 | |
gregeric | 0:b9444a40a999 | 524 | devicePrm[deviceId].currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); |
gregeric | 0:b9444a40a999 | 525 | |
gregeric | 0:b9444a40a999 | 526 | /* Direction setup */ |
gregeric | 0:b9444a40a999 | 527 | L6474_SetDirection(deviceId,direction); |
gregeric | 0:b9444a40a999 | 528 | |
gregeric | 0:b9444a40a999 | 529 | L6474_ComputeSpeedProfile(deviceId, stepCount); |
gregeric | 0:b9444a40a999 | 530 | |
gregeric | 0:b9444a40a999 | 531 | /* Motor activation */ |
gregeric | 0:b9444a40a999 | 532 | L6474_StartMovement(deviceId); |
gregeric | 0:b9444a40a999 | 533 | } |
gregeric | 0:b9444a40a999 | 534 | } |
gregeric | 0:b9444a40a999 | 535 | |
gregeric | 0:b9444a40a999 | 536 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 537 | * @brief Read id |
gregeric | 0:b9444a40a999 | 538 | * @param None |
gregeric | 0:b9444a40a999 | 539 | * @retval Id of the l6474 Driver Instance |
gregeric | 0:b9444a40a999 | 540 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 541 | uint16_t L6474_ReadId(void) |
gregeric | 0:b9444a40a999 | 542 | { |
gregeric | 0:b9444a40a999 | 543 | return(l6474DriverInstance); |
gregeric | 0:b9444a40a999 | 544 | } |
gregeric | 0:b9444a40a999 | 545 | |
gregeric | 0:b9444a40a999 | 546 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 547 | * @brief Resets all L6474 devices |
gregeric | 0:b9444a40a999 | 548 | * @param None |
gregeric | 0:b9444a40a999 | 549 | * @retval None |
gregeric | 0:b9444a40a999 | 550 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 551 | void L6474_ResetAllDevices(void) |
gregeric | 0:b9444a40a999 | 552 | { |
gregeric | 0:b9444a40a999 | 553 | uint8_t loop; |
gregeric | 0:b9444a40a999 | 554 | |
gregeric | 0:b9444a40a999 | 555 | for (loop = 0; loop < numberOfDevices; loop++) |
gregeric | 0:b9444a40a999 | 556 | { |
gregeric | 0:b9444a40a999 | 557 | /* Stop movement and disable power stage*/ |
gregeric | 0:b9444a40a999 | 558 | L6474_HardStop(loop); |
gregeric | 6:19c1b4a04c24 | 559 | #ifdef L6474_CONF_BRAKE_WHEN_HALTED |
gregeric | 6:19c1b4a04c24 | 560 | /* Disable corresponding PWM */ |
gregeric | 6:19c1b4a04c24 | 561 | BSP_MotorControlBoard_PwmStop(loop); |
gregeric | 6:19c1b4a04c24 | 562 | |
gregeric | 6:19c1b4a04c24 | 563 | /* Disable power stage */ |
gregeric | 6:19c1b4a04c24 | 564 | L6474_CmdDisable(loop); |
gregeric | 6:19c1b4a04c24 | 565 | #endif |
gregeric | 0:b9444a40a999 | 566 | } |
gregeric | 0:b9444a40a999 | 567 | L6474_Reset(); |
gregeric | 0:b9444a40a999 | 568 | BSP_MotorControlBoard_Delay(1); // Reset pin must be forced low for at least 10us |
gregeric | 0:b9444a40a999 | 569 | BSP_MotorControlBoard_ReleaseReset(); |
gregeric | 0:b9444a40a999 | 570 | BSP_MotorControlBoard_Delay(1); |
gregeric | 0:b9444a40a999 | 571 | } |
gregeric | 0:b9444a40a999 | 572 | |
gregeric | 0:b9444a40a999 | 573 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 574 | * @brief Runs the motor. It will accelerate from the min |
gregeric | 0:b9444a40a999 | 575 | * speed up to the max speed by using the device acceleration. |
gregeric | 0:b9444a40a999 | 576 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 577 | * @param[in] direction FORWARD or BACKWARD |
gregeric | 0:b9444a40a999 | 578 | * @retval None |
gregeric | 0:b9444a40a999 | 579 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 580 | void L6474_Run(uint8_t deviceId, motorDir_t direction) |
gregeric | 0:b9444a40a999 | 581 | { |
gregeric | 0:b9444a40a999 | 582 | /* Eventually deactivate motor */ |
gregeric | 0:b9444a40a999 | 583 | if (devicePrm[deviceId].motionState != INACTIVE) |
gregeric | 0:b9444a40a999 | 584 | { |
gregeric | 0:b9444a40a999 | 585 | L6474_HardStop(deviceId); |
gregeric | 0:b9444a40a999 | 586 | } |
gregeric | 0:b9444a40a999 | 587 | |
gregeric | 0:b9444a40a999 | 588 | /* Direction setup */ |
gregeric | 0:b9444a40a999 | 589 | L6474_SetDirection(deviceId,direction); |
gregeric | 0:b9444a40a999 | 590 | |
gregeric | 0:b9444a40a999 | 591 | devicePrm[deviceId].commandExecuted = RUN_CMD; |
gregeric | 0:b9444a40a999 | 592 | |
gregeric | 0:b9444a40a999 | 593 | /* Motor activation */ |
gregeric | 0:b9444a40a999 | 594 | L6474_StartMovement(deviceId); |
gregeric | 0:b9444a40a999 | 595 | } |
gregeric | 0:b9444a40a999 | 596 | |
gregeric | 0:b9444a40a999 | 597 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 598 | * @brief Changes the acceleration of the specified device |
gregeric | 0:b9444a40a999 | 599 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 600 | * @param[in] newAcc New acceleration to apply in pps^2 |
gregeric | 0:b9444a40a999 | 601 | * @retval true if the command is successfully executed, else false |
gregeric | 0:b9444a40a999 | 602 | * @note The command is not performed is the device is executing |
gregeric | 0:b9444a40a999 | 603 | * a MOVE or GOTO command (but it can be used during a RUN command) |
gregeric | 0:b9444a40a999 | 604 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 605 | bool L6474_SetAcceleration(uint8_t deviceId,uint16_t newAcc) |
gregeric | 0:b9444a40a999 | 606 | { |
gregeric | 0:b9444a40a999 | 607 | bool cmdExecuted = FALSE; |
gregeric | 0:b9444a40a999 | 608 | if ((newAcc != 0)&& |
gregeric | 0:b9444a40a999 | 609 | ((devicePrm[deviceId].motionState == INACTIVE)|| |
gregeric | 0:b9444a40a999 | 610 | (devicePrm[deviceId].commandExecuted == RUN_CMD))) |
gregeric | 0:b9444a40a999 | 611 | { |
gregeric | 0:b9444a40a999 | 612 | devicePrm[deviceId].acceleration = newAcc; |
gregeric | 0:b9444a40a999 | 613 | cmdExecuted = TRUE; |
gregeric | 0:b9444a40a999 | 614 | } |
gregeric | 0:b9444a40a999 | 615 | return cmdExecuted; |
gregeric | 0:b9444a40a999 | 616 | } |
gregeric | 0:b9444a40a999 | 617 | |
gregeric | 0:b9444a40a999 | 618 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 619 | * @brief Changes the deceleration of the specified device |
gregeric | 0:b9444a40a999 | 620 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 621 | * @param[in] newDec New deceleration to apply in pps^2 |
gregeric | 0:b9444a40a999 | 622 | * @retval true if the command is successfully executed, else false |
gregeric | 0:b9444a40a999 | 623 | * @note The command is not performed is the device is executing |
gregeric | 0:b9444a40a999 | 624 | * a MOVE or GOTO command (but it can be used during a RUN command) |
gregeric | 0:b9444a40a999 | 625 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 626 | bool L6474_SetDeceleration(uint8_t deviceId, uint16_t newDec) |
gregeric | 0:b9444a40a999 | 627 | { |
gregeric | 0:b9444a40a999 | 628 | bool cmdExecuted = FALSE; |
gregeric | 0:b9444a40a999 | 629 | if ((newDec != 0)&& |
gregeric | 0:b9444a40a999 | 630 | ((devicePrm[deviceId].motionState == INACTIVE)|| |
gregeric | 0:b9444a40a999 | 631 | (devicePrm[deviceId].commandExecuted == RUN_CMD))) |
gregeric | 0:b9444a40a999 | 632 | { |
gregeric | 0:b9444a40a999 | 633 | devicePrm[deviceId].deceleration = newDec; |
gregeric | 0:b9444a40a999 | 634 | cmdExecuted = TRUE; |
gregeric | 0:b9444a40a999 | 635 | } |
gregeric | 0:b9444a40a999 | 636 | return cmdExecuted; |
gregeric | 0:b9444a40a999 | 637 | } |
gregeric | 0:b9444a40a999 | 638 | |
gregeric | 0:b9444a40a999 | 639 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 640 | * @brief Set current position to be the Home position (ABS pos set to 0) |
gregeric | 0:b9444a40a999 | 641 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 642 | * @retval None |
gregeric | 0:b9444a40a999 | 643 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 644 | void L6474_SetHome(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 645 | { |
gregeric | 0:b9444a40a999 | 646 | L6474_CmdSetParam(deviceId, L6474_ABS_POS, 0); |
gregeric | 0:b9444a40a999 | 647 | } |
gregeric | 0:b9444a40a999 | 648 | |
gregeric | 0:b9444a40a999 | 649 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 650 | * @brief Sets current position to be the Mark position |
gregeric | 0:b9444a40a999 | 651 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 652 | * @retval None |
gregeric | 0:b9444a40a999 | 653 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 654 | void L6474_SetMark(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 655 | { |
gregeric | 0:b9444a40a999 | 656 | uint32_t mark = L6474_CmdGetParam(deviceId,L6474_ABS_POS); |
gregeric | 0:b9444a40a999 | 657 | L6474_CmdSetParam(deviceId,L6474_MARK, mark); |
gregeric | 0:b9444a40a999 | 658 | } |
gregeric | 0:b9444a40a999 | 659 | |
gregeric | 0:b9444a40a999 | 660 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 661 | * @brief Changes the max speed of the specified device |
gregeric | 0:b9444a40a999 | 662 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 663 | * @param[in] newMaxSpeed New max speed to apply in pps |
gregeric | 0:b9444a40a999 | 664 | * @retval true if the command is successfully executed, else false |
gregeric | 0:b9444a40a999 | 665 | * @note The command is not performed is the device is executing |
gregeric | 0:b9444a40a999 | 666 | * a MOVE or GOTO command (but it can be used during a RUN command). |
gregeric | 0:b9444a40a999 | 667 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 668 | bool L6474_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed) |
gregeric | 0:b9444a40a999 | 669 | { |
gregeric | 0:b9444a40a999 | 670 | bool cmdExecuted = FALSE; |
gregeric | 0:b9444a40a999 | 671 | if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&& |
gregeric | 0:b9444a40a999 | 672 | (newMaxSpeed <= L6474_MAX_PWM_FREQ) && |
gregeric | 0:b9444a40a999 | 673 | (devicePrm[deviceId].minSpeed <= newMaxSpeed) && |
gregeric | 0:b9444a40a999 | 674 | ((devicePrm[deviceId].motionState == INACTIVE)|| |
gregeric | 0:b9444a40a999 | 675 | (devicePrm[deviceId].commandExecuted == RUN_CMD))) |
gregeric | 0:b9444a40a999 | 676 | { |
gregeric | 0:b9444a40a999 | 677 | devicePrm[deviceId].maxSpeed = newMaxSpeed; |
gregeric | 0:b9444a40a999 | 678 | cmdExecuted = TRUE; |
gregeric | 0:b9444a40a999 | 679 | } |
gregeric | 0:b9444a40a999 | 680 | return cmdExecuted; |
gregeric | 0:b9444a40a999 | 681 | } |
gregeric | 0:b9444a40a999 | 682 | |
gregeric | 0:b9444a40a999 | 683 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 684 | * @brief Changes the min speed of the specified device |
gregeric | 0:b9444a40a999 | 685 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 686 | * @param[in] newMinSpeed New min speed to apply in pps |
gregeric | 0:b9444a40a999 | 687 | * @retval true if the command is successfully executed, else false |
gregeric | 0:b9444a40a999 | 688 | * @note The command is not performed is the device is executing |
gregeric | 0:b9444a40a999 | 689 | * a MOVE or GOTO command (but it can be used during a RUN command). |
gregeric | 0:b9444a40a999 | 690 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 691 | bool L6474_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed) |
gregeric | 0:b9444a40a999 | 692 | { |
gregeric | 0:b9444a40a999 | 693 | bool cmdExecuted = FALSE; |
gregeric | 0:b9444a40a999 | 694 | if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&& |
gregeric | 0:b9444a40a999 | 695 | (newMinSpeed <= L6474_MAX_PWM_FREQ) && |
gregeric | 0:b9444a40a999 | 696 | (newMinSpeed <= devicePrm[deviceId].maxSpeed) && |
gregeric | 0:b9444a40a999 | 697 | ((devicePrm[deviceId].motionState == INACTIVE)|| |
gregeric | 0:b9444a40a999 | 698 | (devicePrm[deviceId].commandExecuted == RUN_CMD))) |
gregeric | 0:b9444a40a999 | 699 | { |
gregeric | 0:b9444a40a999 | 700 | devicePrm[deviceId].minSpeed = newMinSpeed; |
gregeric | 0:b9444a40a999 | 701 | cmdExecuted = TRUE; |
gregeric | 0:b9444a40a999 | 702 | } |
gregeric | 0:b9444a40a999 | 703 | return cmdExecuted; |
gregeric | 0:b9444a40a999 | 704 | } |
gregeric | 0:b9444a40a999 | 705 | |
gregeric | 0:b9444a40a999 | 706 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 707 | * @brief Stops the motor by using the device deceleration |
gregeric | 0:b9444a40a999 | 708 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 709 | * @retval true if the command is successfully executed, else false |
gregeric | 0:b9444a40a999 | 710 | * @note The command is not performed is the device is in INACTIVE state. |
gregeric | 0:b9444a40a999 | 711 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 712 | bool L6474_SoftStop(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 713 | { |
gregeric | 0:b9444a40a999 | 714 | bool cmdExecuted = FALSE; |
gregeric | 0:b9444a40a999 | 715 | if (devicePrm[deviceId].motionState != INACTIVE) |
gregeric | 0:b9444a40a999 | 716 | { |
gregeric | 0:b9444a40a999 | 717 | devicePrm[deviceId].commandExecuted = SOFT_STOP_CMD; |
gregeric | 0:b9444a40a999 | 718 | cmdExecuted = TRUE; |
gregeric | 0:b9444a40a999 | 719 | } |
gregeric | 0:b9444a40a999 | 720 | return (cmdExecuted); |
gregeric | 0:b9444a40a999 | 721 | } |
gregeric | 0:b9444a40a999 | 722 | |
gregeric | 0:b9444a40a999 | 723 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 724 | * @brief Locks until the device state becomes Inactive |
gregeric | 0:b9444a40a999 | 725 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 726 | * @retval None |
gregeric | 0:b9444a40a999 | 727 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 728 | void L6474_WaitWhileActive(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 729 | { |
gregeric | 0:b9444a40a999 | 730 | /* Wait while motor is running */ |
gregeric | 0:b9444a40a999 | 731 | while (L6474_GetDeviceState(deviceId) != INACTIVE); |
gregeric | 0:b9444a40a999 | 732 | } |
gregeric | 0:b9444a40a999 | 733 | |
gregeric | 0:b9444a40a999 | 734 | /** |
gregeric | 0:b9444a40a999 | 735 | * @} |
gregeric | 0:b9444a40a999 | 736 | */ |
gregeric | 0:b9444a40a999 | 737 | |
gregeric | 0:b9444a40a999 | 738 | /** @defgroup L6474_Control_Functions |
gregeric | 0:b9444a40a999 | 739 | * @{ |
gregeric | 0:b9444a40a999 | 740 | */ |
gregeric | 0:b9444a40a999 | 741 | |
gregeric | 0:b9444a40a999 | 742 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 743 | * @brief Issue the Disable command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 744 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 745 | * @retval None |
gregeric | 0:b9444a40a999 | 746 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 747 | void L6474_CmdDisable(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 748 | { |
gregeric | 0:b9444a40a999 | 749 | L6474_SendCommand(deviceId, L6474_DISABLE); |
gregeric | 0:b9444a40a999 | 750 | } |
gregeric | 0:b9444a40a999 | 751 | |
gregeric | 0:b9444a40a999 | 752 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 753 | * @brief Issues the Enable command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 754 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 755 | * @retval None |
gregeric | 0:b9444a40a999 | 756 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 757 | void L6474_CmdEnable(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 758 | { |
gregeric | 0:b9444a40a999 | 759 | L6474_SendCommand(deviceId, L6474_ENABLE); |
gregeric | 0:b9444a40a999 | 760 | } |
gregeric | 0:b9444a40a999 | 761 | |
gregeric | 0:b9444a40a999 | 762 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 763 | * @brief Issues the GetParam command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 764 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 765 | * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) |
gregeric | 0:b9444a40a999 | 766 | * @retval Register value |
gregeric | 0:b9444a40a999 | 767 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 768 | uint32_t L6474_CmdGetParam(uint8_t deviceId, uint32_t param) |
gregeric | 0:b9444a40a999 | 769 | { |
gregeric | 0:b9444a40a999 | 770 | uint32_t i; |
gregeric | 0:b9444a40a999 | 771 | uint32_t spiRxData; |
gregeric | 0:b9444a40a999 | 772 | uint8_t maxArgumentNbBytes = 0; |
gregeric | 0:b9444a40a999 | 773 | uint8_t spiIndex = numberOfDevices - deviceId - 1; |
gregeric | 0:b9444a40a999 | 774 | bool itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 775 | |
gregeric | 0:b9444a40a999 | 776 | do |
gregeric | 0:b9444a40a999 | 777 | { |
gregeric | 0:b9444a40a999 | 778 | spiPreemtionByIsr = FALSE; |
gregeric | 0:b9444a40a999 | 779 | if (itDisable) |
gregeric | 0:b9444a40a999 | 780 | { |
gregeric | 0:b9444a40a999 | 781 | /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ |
gregeric | 0:b9444a40a999 | 782 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 783 | itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 784 | } |
gregeric | 0:b9444a40a999 | 785 | |
gregeric | 0:b9444a40a999 | 786 | for (i = 0; i < numberOfDevices; i++) |
gregeric | 0:b9444a40a999 | 787 | { |
gregeric | 0:b9444a40a999 | 788 | spiTxBursts[0][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 789 | spiTxBursts[1][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 790 | spiTxBursts[2][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 791 | spiTxBursts[3][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 792 | spiRxBursts[1][i] = 0; |
gregeric | 0:b9444a40a999 | 793 | spiRxBursts[2][i] = 0; |
gregeric | 0:b9444a40a999 | 794 | spiRxBursts[3][i] = 0; |
gregeric | 0:b9444a40a999 | 795 | } |
gregeric | 0:b9444a40a999 | 796 | switch (param) |
gregeric | 0:b9444a40a999 | 797 | { |
gregeric | 0:b9444a40a999 | 798 | case L6474_ABS_POS: ; |
gregeric | 0:b9444a40a999 | 799 | case L6474_MARK: |
gregeric | 0:b9444a40a999 | 800 | spiTxBursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); |
gregeric | 0:b9444a40a999 | 801 | maxArgumentNbBytes = 3; |
gregeric | 0:b9444a40a999 | 802 | break; |
gregeric | 0:b9444a40a999 | 803 | case L6474_EL_POS: ; |
gregeric | 0:b9444a40a999 | 804 | case L6474_CONFIG: ; |
gregeric | 0:b9444a40a999 | 805 | case L6474_STATUS: |
gregeric | 0:b9444a40a999 | 806 | spiTxBursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); |
gregeric | 0:b9444a40a999 | 807 | maxArgumentNbBytes = 2; |
gregeric | 0:b9444a40a999 | 808 | break; |
gregeric | 0:b9444a40a999 | 809 | default: |
gregeric | 0:b9444a40a999 | 810 | spiTxBursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); |
gregeric | 0:b9444a40a999 | 811 | maxArgumentNbBytes = 1; |
gregeric | 0:b9444a40a999 | 812 | } |
gregeric | 0:b9444a40a999 | 813 | |
gregeric | 0:b9444a40a999 | 814 | /* Disable interruption before checking */ |
gregeric | 0:b9444a40a999 | 815 | /* pre-emption by ISR and SPI transfers*/ |
gregeric | 0:b9444a40a999 | 816 | BSP_MotorControlBoard_DisableIrq(); |
gregeric | 0:b9444a40a999 | 817 | itDisable = TRUE; |
gregeric | 0:b9444a40a999 | 818 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
gregeric | 0:b9444a40a999 | 819 | |
gregeric | 0:b9444a40a999 | 820 | for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
gregeric | 0:b9444a40a999 | 821 | i < L6474_CMD_ARG_MAX_NB_BYTES; |
gregeric | 0:b9444a40a999 | 822 | i++) |
gregeric | 0:b9444a40a999 | 823 | { |
gregeric | 0:b9444a40a999 | 824 | L6474_WriteBytes(&spiTxBursts[i][0], |
gregeric | 0:b9444a40a999 | 825 | &spiRxBursts[i][0]); |
gregeric | 0:b9444a40a999 | 826 | } |
gregeric | 0:b9444a40a999 | 827 | |
gregeric | 0:b9444a40a999 | 828 | spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)| |
gregeric | 0:b9444a40a999 | 829 | (spiRxBursts[2][spiIndex] << 8) | |
gregeric | 0:b9444a40a999 | 830 | (spiRxBursts[3][spiIndex]); |
gregeric | 0:b9444a40a999 | 831 | |
gregeric | 0:b9444a40a999 | 832 | /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ |
gregeric | 0:b9444a40a999 | 833 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 834 | |
gregeric | 0:b9444a40a999 | 835 | return (spiRxData); |
gregeric | 0:b9444a40a999 | 836 | } |
gregeric | 0:b9444a40a999 | 837 | |
gregeric | 0:b9444a40a999 | 838 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 839 | * @brief Issues the GetStatus command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 840 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 841 | * @retval Status Register value |
gregeric | 0:b9444a40a999 | 842 | * @note Once the GetStatus command is performed, the flags of the status register |
gregeric | 0:b9444a40a999 | 843 | * are reset. This is not the case when the status register is read with the |
gregeric | 0:b9444a40a999 | 844 | * GetParam command (via the functions L6474ReadStatusRegister or L6474CmdGetParam). |
gregeric | 0:b9444a40a999 | 845 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 846 | uint16_t L6474_CmdGetStatus(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 847 | { |
gregeric | 0:b9444a40a999 | 848 | uint32_t i; |
gregeric | 0:b9444a40a999 | 849 | uint16_t status; |
gregeric | 0:b9444a40a999 | 850 | uint8_t spiIndex = numberOfDevices - deviceId - 1; |
gregeric | 0:b9444a40a999 | 851 | bool itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 852 | |
gregeric | 0:b9444a40a999 | 853 | do |
gregeric | 0:b9444a40a999 | 854 | { |
gregeric | 0:b9444a40a999 | 855 | spiPreemtionByIsr = FALSE; |
gregeric | 0:b9444a40a999 | 856 | if (itDisable) |
gregeric | 0:b9444a40a999 | 857 | { |
gregeric | 0:b9444a40a999 | 858 | /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ |
gregeric | 0:b9444a40a999 | 859 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 860 | itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 861 | } |
gregeric | 0:b9444a40a999 | 862 | |
gregeric | 0:b9444a40a999 | 863 | for (i = 0; i < numberOfDevices; i++) |
gregeric | 0:b9444a40a999 | 864 | { |
gregeric | 0:b9444a40a999 | 865 | spiTxBursts[0][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 866 | spiTxBursts[1][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 867 | spiTxBursts[2][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 868 | spiRxBursts[1][i] = 0; |
gregeric | 0:b9444a40a999 | 869 | spiRxBursts[2][i] = 0; |
gregeric | 0:b9444a40a999 | 870 | } |
gregeric | 0:b9444a40a999 | 871 | spiTxBursts[0][spiIndex] = L6474_GET_STATUS; |
gregeric | 0:b9444a40a999 | 872 | |
gregeric | 0:b9444a40a999 | 873 | /* Disable interruption before checking */ |
gregeric | 0:b9444a40a999 | 874 | /* pre-emption by ISR and SPI transfers*/ |
gregeric | 0:b9444a40a999 | 875 | BSP_MotorControlBoard_DisableIrq(); |
gregeric | 0:b9444a40a999 | 876 | itDisable = TRUE; |
gregeric | 0:b9444a40a999 | 877 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
gregeric | 0:b9444a40a999 | 878 | |
gregeric | 0:b9444a40a999 | 879 | for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++) |
gregeric | 0:b9444a40a999 | 880 | { |
gregeric | 0:b9444a40a999 | 881 | L6474_WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); |
gregeric | 0:b9444a40a999 | 882 | } |
gregeric | 0:b9444a40a999 | 883 | status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]); |
gregeric | 0:b9444a40a999 | 884 | |
gregeric | 0:b9444a40a999 | 885 | /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ |
gregeric | 0:b9444a40a999 | 886 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 887 | |
gregeric | 0:b9444a40a999 | 888 | return (status); |
gregeric | 0:b9444a40a999 | 889 | } |
gregeric | 0:b9444a40a999 | 890 | |
gregeric | 0:b9444a40a999 | 891 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 892 | * @brief Issues the Nop command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 893 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 894 | * @retval None |
gregeric | 0:b9444a40a999 | 895 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 896 | void L6474_CmdNop(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 897 | { |
gregeric | 0:b9444a40a999 | 898 | L6474_SendCommand(deviceId, L6474_NOP); |
gregeric | 0:b9444a40a999 | 899 | } |
gregeric | 0:b9444a40a999 | 900 | |
gregeric | 0:b9444a40a999 | 901 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 902 | * @brief Issues the SetParam command to the L6474 of the specified device |
gregeric | 0:b9444a40a999 | 903 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 904 | * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) |
gregeric | 0:b9444a40a999 | 905 | * @param[in] value Value to set in the register |
gregeric | 0:b9444a40a999 | 906 | * @retval None |
gregeric | 0:b9444a40a999 | 907 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 908 | void L6474_CmdSetParam(uint8_t deviceId, |
gregeric | 0:b9444a40a999 | 909 | uint32_t param, |
gregeric | 0:b9444a40a999 | 910 | uint32_t value) |
gregeric | 0:b9444a40a999 | 911 | { |
gregeric | 0:b9444a40a999 | 912 | uint32_t i; |
gregeric | 0:b9444a40a999 | 913 | uint8_t maxArgumentNbBytes = 0; |
gregeric | 0:b9444a40a999 | 914 | uint8_t spiIndex = numberOfDevices - deviceId - 1; |
gregeric | 0:b9444a40a999 | 915 | bool itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 916 | do |
gregeric | 0:b9444a40a999 | 917 | { |
gregeric | 0:b9444a40a999 | 918 | spiPreemtionByIsr = FALSE; |
gregeric | 0:b9444a40a999 | 919 | if (itDisable) |
gregeric | 0:b9444a40a999 | 920 | { |
gregeric | 0:b9444a40a999 | 921 | /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ |
gregeric | 0:b9444a40a999 | 922 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 923 | itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 924 | } |
gregeric | 0:b9444a40a999 | 925 | for (i = 0; i < numberOfDevices; i++) |
gregeric | 0:b9444a40a999 | 926 | { |
gregeric | 0:b9444a40a999 | 927 | spiTxBursts[0][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 928 | spiTxBursts[1][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 929 | spiTxBursts[2][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 930 | spiTxBursts[3][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 931 | } |
gregeric | 0:b9444a40a999 | 932 | switch (param) |
gregeric | 0:b9444a40a999 | 933 | { |
gregeric | 0:b9444a40a999 | 934 | case L6474_ABS_POS: ; |
gregeric | 0:b9444a40a999 | 935 | case L6474_MARK: |
gregeric | 0:b9444a40a999 | 936 | spiTxBursts[0][spiIndex] = param; |
gregeric | 0:b9444a40a999 | 937 | spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16); |
gregeric | 0:b9444a40a999 | 938 | spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); |
gregeric | 0:b9444a40a999 | 939 | maxArgumentNbBytes = 3; |
gregeric | 0:b9444a40a999 | 940 | break; |
gregeric | 0:b9444a40a999 | 941 | case L6474_EL_POS: ; |
gregeric | 0:b9444a40a999 | 942 | case L6474_CONFIG: |
gregeric | 0:b9444a40a999 | 943 | spiTxBursts[1][spiIndex] = param; |
gregeric | 0:b9444a40a999 | 944 | spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); |
gregeric | 0:b9444a40a999 | 945 | maxArgumentNbBytes = 2; |
gregeric | 0:b9444a40a999 | 946 | break; |
gregeric | 0:b9444a40a999 | 947 | default: |
gregeric | 0:b9444a40a999 | 948 | spiTxBursts[2][spiIndex] = param; |
gregeric | 0:b9444a40a999 | 949 | maxArgumentNbBytes = 1; |
gregeric | 0:b9444a40a999 | 950 | } |
gregeric | 0:b9444a40a999 | 951 | spiTxBursts[3][spiIndex] = (uint8_t)(value); |
gregeric | 0:b9444a40a999 | 952 | |
gregeric | 0:b9444a40a999 | 953 | /* Disable interruption before checking */ |
gregeric | 0:b9444a40a999 | 954 | /* pre-emption by ISR and SPI transfers*/ |
gregeric | 0:b9444a40a999 | 955 | BSP_MotorControlBoard_DisableIrq(); |
gregeric | 0:b9444a40a999 | 956 | itDisable = TRUE; |
gregeric | 0:b9444a40a999 | 957 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
gregeric | 0:b9444a40a999 | 958 | |
gregeric | 0:b9444a40a999 | 959 | /* SPI transfer */ |
gregeric | 0:b9444a40a999 | 960 | for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
gregeric | 0:b9444a40a999 | 961 | i < L6474_CMD_ARG_MAX_NB_BYTES; |
gregeric | 0:b9444a40a999 | 962 | i++) |
gregeric | 0:b9444a40a999 | 963 | { |
gregeric | 0:b9444a40a999 | 964 | L6474_WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]); |
gregeric | 0:b9444a40a999 | 965 | } |
gregeric | 0:b9444a40a999 | 966 | /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ |
gregeric | 0:b9444a40a999 | 967 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 968 | } |
gregeric | 0:b9444a40a999 | 969 | |
gregeric | 0:b9444a40a999 | 970 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 971 | * @brief Reads the Status Register value |
gregeric | 0:b9444a40a999 | 972 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 973 | * @retval Status register valued |
gregeric | 0:b9444a40a999 | 974 | * @note The status register flags are not cleared |
gregeric | 0:b9444a40a999 | 975 | * at the difference with L6474CmdGetStatus() |
gregeric | 0:b9444a40a999 | 976 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 977 | uint16_t L6474_ReadStatusRegister(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 978 | { |
gregeric | 0:b9444a40a999 | 979 | return (L6474_CmdGetParam(deviceId,L6474_STATUS)); |
gregeric | 0:b9444a40a999 | 980 | } |
gregeric | 0:b9444a40a999 | 981 | |
gregeric | 0:b9444a40a999 | 982 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 983 | * @brief Releases the L6474 reset (pin set to High) of all devices |
gregeric | 0:b9444a40a999 | 984 | * @param None |
gregeric | 0:b9444a40a999 | 985 | * @retval None |
gregeric | 0:b9444a40a999 | 986 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 987 | void L6474_ReleaseReset(void) |
gregeric | 0:b9444a40a999 | 988 | { |
gregeric | 0:b9444a40a999 | 989 | BSP_MotorControlBoard_ReleaseReset(); |
gregeric | 0:b9444a40a999 | 990 | } |
gregeric | 0:b9444a40a999 | 991 | |
gregeric | 0:b9444a40a999 | 992 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 993 | * @brief Resets the L6474 (reset pin set to low) of all devices |
gregeric | 0:b9444a40a999 | 994 | * @param None |
gregeric | 0:b9444a40a999 | 995 | * @retval None |
gregeric | 0:b9444a40a999 | 996 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 997 | void L6474_Reset(void) |
gregeric | 0:b9444a40a999 | 998 | { |
gregeric | 0:b9444a40a999 | 999 | BSP_MotorControlBoard_Reset(); |
gregeric | 0:b9444a40a999 | 1000 | } |
gregeric | 0:b9444a40a999 | 1001 | |
gregeric | 0:b9444a40a999 | 1002 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1003 | * @brief Set the stepping mode |
gregeric | 0:b9444a40a999 | 1004 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1005 | * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t |
gregeric | 0:b9444a40a999 | 1006 | * @retval None |
gregeric | 0:b9444a40a999 | 1007 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1008 | void L6474_SelectStepMode(uint8_t deviceId, motorStepMode_t stepMod) |
gregeric | 0:b9444a40a999 | 1009 | { |
gregeric | 0:b9444a40a999 | 1010 | uint8_t stepModeRegister; |
gregeric | 0:b9444a40a999 | 1011 | L6474_STEP_SEL_t l6474StepMod; |
gregeric | 0:b9444a40a999 | 1012 | |
gregeric | 0:b9444a40a999 | 1013 | switch (stepMod) |
gregeric | 0:b9444a40a999 | 1014 | { |
gregeric | 0:b9444a40a999 | 1015 | case STEP_MODE_FULL: |
gregeric | 0:b9444a40a999 | 1016 | l6474StepMod = L6474_STEP_SEL_1; |
gregeric | 0:b9444a40a999 | 1017 | break; |
gregeric | 0:b9444a40a999 | 1018 | case STEP_MODE_HALF: |
gregeric | 0:b9444a40a999 | 1019 | l6474StepMod = L6474_STEP_SEL_1_2; |
gregeric | 0:b9444a40a999 | 1020 | break; |
gregeric | 0:b9444a40a999 | 1021 | case STEP_MODE_1_4: |
gregeric | 0:b9444a40a999 | 1022 | l6474StepMod = L6474_STEP_SEL_1_4; |
gregeric | 0:b9444a40a999 | 1023 | break; |
gregeric | 0:b9444a40a999 | 1024 | case STEP_MODE_1_8: |
gregeric | 0:b9444a40a999 | 1025 | l6474StepMod = L6474_STEP_SEL_1_8; |
gregeric | 0:b9444a40a999 | 1026 | break; |
gregeric | 0:b9444a40a999 | 1027 | case STEP_MODE_1_16: |
gregeric | 0:b9444a40a999 | 1028 | default: |
gregeric | 0:b9444a40a999 | 1029 | l6474StepMod = L6474_STEP_SEL_1_16; |
gregeric | 0:b9444a40a999 | 1030 | break; |
gregeric | 0:b9444a40a999 | 1031 | } |
gregeric | 0:b9444a40a999 | 1032 | |
gregeric | 0:b9444a40a999 | 1033 | /* Eventually deactivate motor */ |
gregeric | 0:b9444a40a999 | 1034 | if (devicePrm[deviceId].motionState != INACTIVE) |
gregeric | 0:b9444a40a999 | 1035 | { |
gregeric | 0:b9444a40a999 | 1036 | L6474_HardStop(deviceId); |
gregeric | 0:b9444a40a999 | 1037 | } |
gregeric | 0:b9444a40a999 | 1038 | |
gregeric | 0:b9444a40a999 | 1039 | /* Read Step mode register and clear STEP_SEL field */ |
gregeric | 0:b9444a40a999 | 1040 | stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(deviceId,L6474_STEP_MODE)) ; |
gregeric | 0:b9444a40a999 | 1041 | |
gregeric | 0:b9444a40a999 | 1042 | /* Apply new step mode */ |
gregeric | 0:b9444a40a999 | 1043 | L6474_CmdSetParam(deviceId, L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod); |
gregeric | 0:b9444a40a999 | 1044 | |
gregeric | 0:b9444a40a999 | 1045 | /* Reset abs pos register */ |
gregeric | 0:b9444a40a999 | 1046 | L6474_SetHome(deviceId); |
gregeric | 0:b9444a40a999 | 1047 | } |
gregeric | 0:b9444a40a999 | 1048 | |
gregeric | 0:b9444a40a999 | 1049 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1050 | * @brief Specifies the direction |
gregeric | 0:b9444a40a999 | 1051 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1052 | * @param[in] dir FORWARD or BACKWARD |
gregeric | 0:b9444a40a999 | 1053 | * @note The direction change is only applied if the device |
gregeric | 0:b9444a40a999 | 1054 | * is in INACTIVE state |
gregeric | 0:b9444a40a999 | 1055 | * @retval None |
gregeric | 0:b9444a40a999 | 1056 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1057 | void L6474_SetDirection(uint8_t deviceId, motorDir_t dir) |
gregeric | 0:b9444a40a999 | 1058 | { |
gregeric | 0:b9444a40a999 | 1059 | if (devicePrm[deviceId].motionState == INACTIVE) |
gregeric | 0:b9444a40a999 | 1060 | { |
gregeric | 0:b9444a40a999 | 1061 | devicePrm[deviceId].direction = dir; |
gregeric | 0:b9444a40a999 | 1062 | BSP_MotorControlBoard_SetDirectionGpio(deviceId, dir); |
gregeric | 0:b9444a40a999 | 1063 | } |
gregeric | 0:b9444a40a999 | 1064 | } |
gregeric | 0:b9444a40a999 | 1065 | |
gregeric | 0:b9444a40a999 | 1066 | /** |
gregeric | 0:b9444a40a999 | 1067 | * @} |
gregeric | 0:b9444a40a999 | 1068 | */ |
gregeric | 0:b9444a40a999 | 1069 | |
gregeric | 0:b9444a40a999 | 1070 | /** @addtogroup L6474_Private_functions |
gregeric | 0:b9444a40a999 | 1071 | * @{ |
gregeric | 0:b9444a40a999 | 1072 | */ |
gregeric | 0:b9444a40a999 | 1073 | |
gregeric | 0:b9444a40a999 | 1074 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1075 | * @brief Updates the current speed of the device |
gregeric | 0:b9444a40a999 | 1076 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1077 | * @param[in] newSpeed in pps |
gregeric | 0:b9444a40a999 | 1078 | * @retval None |
gregeric | 0:b9444a40a999 | 1079 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1080 | void L6474_ApplySpeed(uint8_t deviceId, uint16_t newSpeed) |
gregeric | 0:b9444a40a999 | 1081 | { |
gregeric | 0:b9444a40a999 | 1082 | if (newSpeed < L6474_MIN_PWM_FREQ) |
gregeric | 0:b9444a40a999 | 1083 | { |
gregeric | 0:b9444a40a999 | 1084 | newSpeed = L6474_MIN_PWM_FREQ; |
gregeric | 0:b9444a40a999 | 1085 | } |
gregeric | 0:b9444a40a999 | 1086 | if (newSpeed > L6474_MAX_PWM_FREQ) |
gregeric | 0:b9444a40a999 | 1087 | { |
gregeric | 0:b9444a40a999 | 1088 | newSpeed = L6474_MAX_PWM_FREQ; |
gregeric | 0:b9444a40a999 | 1089 | } |
gregeric | 0:b9444a40a999 | 1090 | |
gregeric | 0:b9444a40a999 | 1091 | devicePrm[deviceId].speed = newSpeed; |
gregeric | 0:b9444a40a999 | 1092 | |
gregeric | 0:b9444a40a999 | 1093 | switch (deviceId) |
gregeric | 0:b9444a40a999 | 1094 | { |
gregeric | 0:b9444a40a999 | 1095 | case 0: |
gregeric | 0:b9444a40a999 | 1096 | BSP_MotorControlBoard_Pwm1SetFreq(newSpeed); |
gregeric | 0:b9444a40a999 | 1097 | break; |
gregeric | 0:b9444a40a999 | 1098 | case 1: |
gregeric | 0:b9444a40a999 | 1099 | BSP_MotorControlBoard_Pwm2SetFreq(newSpeed); |
gregeric | 0:b9444a40a999 | 1100 | break; |
gregeric | 0:b9444a40a999 | 1101 | case 2: |
gregeric | 0:b9444a40a999 | 1102 | BSP_MotorControlBoard_Pwm3SetFreq(newSpeed); |
gregeric | 0:b9444a40a999 | 1103 | break; |
gregeric | 0:b9444a40a999 | 1104 | default: |
gregeric | 0:b9444a40a999 | 1105 | break; //ignore error |
gregeric | 0:b9444a40a999 | 1106 | } |
gregeric | 0:b9444a40a999 | 1107 | } |
gregeric | 0:b9444a40a999 | 1108 | |
gregeric | 0:b9444a40a999 | 1109 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1110 | * @brief Computes the speed profile according to the number of steps to move |
gregeric | 0:b9444a40a999 | 1111 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1112 | * @param[in] nbSteps number of steps to perform |
gregeric | 0:b9444a40a999 | 1113 | * @retval None |
gregeric | 0:b9444a40a999 | 1114 | * @note Using the acceleration and deceleration of the device, |
gregeric | 0:b9444a40a999 | 1115 | * this function determines the duration in steps of the acceleration, |
gregeric | 0:b9444a40a999 | 1116 | * steady and deceleration phases. |
gregeric | 0:b9444a40a999 | 1117 | * If the total number of steps to perform is big enough, a trapezoidal move |
gregeric | 0:b9444a40a999 | 1118 | * is performed (i.e. there is a steady phase where the motor runs at the maximum |
gregeric | 0:b9444a40a999 | 1119 | * speed. |
gregeric | 0:b9444a40a999 | 1120 | * Else, a triangular move is performed (no steady phase: the maximum speed is never |
gregeric | 0:b9444a40a999 | 1121 | * reached. |
gregeric | 0:b9444a40a999 | 1122 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1123 | void L6474_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps) |
gregeric | 0:b9444a40a999 | 1124 | { |
gregeric | 0:b9444a40a999 | 1125 | uint32_t reqAccSteps; |
gregeric | 0:b9444a40a999 | 1126 | uint32_t reqDecSteps; |
gregeric | 0:b9444a40a999 | 1127 | |
gregeric | 0:b9444a40a999 | 1128 | /* compute the number of steps to get the targeted speed */ |
gregeric | 0:b9444a40a999 | 1129 | uint16_t minSpeed = devicePrm[deviceId].minSpeed; |
gregeric | 0:b9444a40a999 | 1130 | reqAccSteps = (devicePrm[deviceId].maxSpeed - minSpeed); |
gregeric | 0:b9444a40a999 | 1131 | reqAccSteps *= (devicePrm[deviceId].maxSpeed + minSpeed); |
gregeric | 0:b9444a40a999 | 1132 | reqDecSteps = reqAccSteps; |
gregeric | 0:b9444a40a999 | 1133 | reqAccSteps /= (uint32_t)devicePrm[deviceId].acceleration; |
gregeric | 0:b9444a40a999 | 1134 | reqAccSteps /= 2; |
gregeric | 0:b9444a40a999 | 1135 | |
gregeric | 0:b9444a40a999 | 1136 | /* compute the number of steps to stop */ |
gregeric | 0:b9444a40a999 | 1137 | reqDecSteps /= (uint32_t)devicePrm[deviceId].deceleration; |
gregeric | 0:b9444a40a999 | 1138 | reqDecSteps /= 2; |
gregeric | 0:b9444a40a999 | 1139 | |
gregeric | 0:b9444a40a999 | 1140 | if(( reqAccSteps + reqDecSteps ) > nbSteps) |
gregeric | 0:b9444a40a999 | 1141 | { |
gregeric | 0:b9444a40a999 | 1142 | /* Triangular move */ |
gregeric | 0:b9444a40a999 | 1143 | /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ |
gregeric | 0:b9444a40a999 | 1144 | uint32_t dec = devicePrm[deviceId].deceleration; |
gregeric | 0:b9444a40a999 | 1145 | uint32_t acc = devicePrm[deviceId].acceleration; |
gregeric | 0:b9444a40a999 | 1146 | |
gregeric | 0:b9444a40a999 | 1147 | reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); |
gregeric | 0:b9444a40a999 | 1148 | if (reqDecSteps > 1) |
gregeric | 0:b9444a40a999 | 1149 | { |
gregeric | 0:b9444a40a999 | 1150 | reqAccSteps = reqDecSteps - 1; |
gregeric | 0:b9444a40a999 | 1151 | if(reqAccSteps == 0) |
gregeric | 0:b9444a40a999 | 1152 | { |
gregeric | 0:b9444a40a999 | 1153 | reqAccSteps = 1; |
gregeric | 0:b9444a40a999 | 1154 | } |
gregeric | 0:b9444a40a999 | 1155 | } |
gregeric | 0:b9444a40a999 | 1156 | else |
gregeric | 0:b9444a40a999 | 1157 | { |
gregeric | 0:b9444a40a999 | 1158 | reqAccSteps = 0; |
gregeric | 0:b9444a40a999 | 1159 | } |
gregeric | 0:b9444a40a999 | 1160 | devicePrm[deviceId].endAccPos = reqAccSteps; |
gregeric | 0:b9444a40a999 | 1161 | devicePrm[deviceId].startDecPos = reqDecSteps; |
gregeric | 0:b9444a40a999 | 1162 | } |
gregeric | 0:b9444a40a999 | 1163 | else |
gregeric | 0:b9444a40a999 | 1164 | { |
gregeric | 0:b9444a40a999 | 1165 | /* Trapezoidal move */ |
gregeric | 0:b9444a40a999 | 1166 | /* accelerating phase to endAccPos */ |
gregeric | 0:b9444a40a999 | 1167 | /* steady phase from endAccPos to startDecPos */ |
gregeric | 0:b9444a40a999 | 1168 | /* decelerating from startDecPos to stepsToTake*/ |
gregeric | 0:b9444a40a999 | 1169 | devicePrm[deviceId].endAccPos = reqAccSteps; |
gregeric | 0:b9444a40a999 | 1170 | devicePrm[deviceId].startDecPos = nbSteps - reqDecSteps - 1; |
gregeric | 0:b9444a40a999 | 1171 | } |
gregeric | 0:b9444a40a999 | 1172 | } |
gregeric | 0:b9444a40a999 | 1173 | |
gregeric | 0:b9444a40a999 | 1174 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1175 | * @brief Converts the ABS_POSITION register value to a 32b signed integer |
gregeric | 0:b9444a40a999 | 1176 | * @param[in] abs_position_reg value of the ABS_POSITION register |
gregeric | 0:b9444a40a999 | 1177 | * @retval operation_result 32b signed integer corresponding to the absolute position |
gregeric | 0:b9444a40a999 | 1178 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1179 | int32_t L6474_ConvertPosition(uint32_t abs_position_reg) |
gregeric | 0:b9444a40a999 | 1180 | { |
gregeric | 0:b9444a40a999 | 1181 | int32_t operation_result; |
gregeric | 0:b9444a40a999 | 1182 | |
gregeric | 0:b9444a40a999 | 1183 | if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) |
gregeric | 0:b9444a40a999 | 1184 | { |
gregeric | 0:b9444a40a999 | 1185 | /* Negative register value */ |
gregeric | 0:b9444a40a999 | 1186 | abs_position_reg = ~abs_position_reg; |
gregeric | 0:b9444a40a999 | 1187 | abs_position_reg += 1; |
gregeric | 0:b9444a40a999 | 1188 | |
gregeric | 0:b9444a40a999 | 1189 | operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK); |
gregeric | 0:b9444a40a999 | 1190 | operation_result = -operation_result; |
gregeric | 0:b9444a40a999 | 1191 | } |
gregeric | 0:b9444a40a999 | 1192 | else |
gregeric | 0:b9444a40a999 | 1193 | { |
gregeric | 0:b9444a40a999 | 1194 | operation_result = (int32_t) abs_position_reg; |
gregeric | 0:b9444a40a999 | 1195 | } |
gregeric | 0:b9444a40a999 | 1196 | return operation_result; |
gregeric | 0:b9444a40a999 | 1197 | } |
gregeric | 0:b9444a40a999 | 1198 | |
gregeric | 0:b9444a40a999 | 1199 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1200 | * @brief Error handler which calls the user callback (if defined) |
gregeric | 0:b9444a40a999 | 1201 | * @param[in] error Number of the error |
gregeric | 0:b9444a40a999 | 1202 | * @retval None |
gregeric | 0:b9444a40a999 | 1203 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1204 | void L6474_ErrorHandler(uint16_t error) |
gregeric | 0:b9444a40a999 | 1205 | { |
gregeric | 0:b9444a40a999 | 1206 | if (errorHandlerCallback != 0) |
gregeric | 0:b9444a40a999 | 1207 | { |
gregeric | 0:b9444a40a999 | 1208 | (void) errorHandlerCallback(error); |
gregeric | 0:b9444a40a999 | 1209 | } |
gregeric | 0:b9444a40a999 | 1210 | else |
gregeric | 0:b9444a40a999 | 1211 | { |
gregeric | 0:b9444a40a999 | 1212 | while(1) |
gregeric | 0:b9444a40a999 | 1213 | { |
gregeric | 0:b9444a40a999 | 1214 | /* Infinite loop */ |
gregeric | 0:b9444a40a999 | 1215 | } |
gregeric | 0:b9444a40a999 | 1216 | } |
gregeric | 0:b9444a40a999 | 1217 | } |
gregeric | 0:b9444a40a999 | 1218 | |
gregeric | 0:b9444a40a999 | 1219 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1220 | * @brief Handlers of the flag interrupt which calls the user callback (if defined) |
gregeric | 0:b9444a40a999 | 1221 | * @param None |
gregeric | 0:b9444a40a999 | 1222 | * @retval None |
gregeric | 0:b9444a40a999 | 1223 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1224 | void L6474_FlagInterruptHandler(void) |
gregeric | 0:b9444a40a999 | 1225 | { |
gregeric | 0:b9444a40a999 | 1226 | if (flagInterruptCallback != 0) |
gregeric | 0:b9444a40a999 | 1227 | { |
gregeric | 0:b9444a40a999 | 1228 | /* Set isr flag */ |
gregeric | 0:b9444a40a999 | 1229 | isrFlag = TRUE; |
gregeric | 0:b9444a40a999 | 1230 | |
gregeric | 0:b9444a40a999 | 1231 | flagInterruptCallback(); |
gregeric | 0:b9444a40a999 | 1232 | |
gregeric | 0:b9444a40a999 | 1233 | /* Reset isr flag */ |
gregeric | 0:b9444a40a999 | 1234 | isrFlag = FALSE; |
gregeric | 0:b9444a40a999 | 1235 | } |
gregeric | 0:b9444a40a999 | 1236 | } |
gregeric | 0:b9444a40a999 | 1237 | |
gregeric | 0:b9444a40a999 | 1238 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1239 | * @brief Sends a command without arguments to the L6474 via the SPI |
gregeric | 0:b9444a40a999 | 1240 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1241 | * @param[in] param Command to send |
gregeric | 0:b9444a40a999 | 1242 | * @retval None |
gregeric | 0:b9444a40a999 | 1243 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1244 | void L6474_SendCommand(uint8_t deviceId, uint8_t param) |
gregeric | 0:b9444a40a999 | 1245 | { |
gregeric | 0:b9444a40a999 | 1246 | uint32_t i; |
gregeric | 0:b9444a40a999 | 1247 | uint8_t spiIndex = numberOfDevices - deviceId - 1; |
gregeric | 0:b9444a40a999 | 1248 | bool itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 1249 | |
gregeric | 0:b9444a40a999 | 1250 | do |
gregeric | 0:b9444a40a999 | 1251 | { |
gregeric | 0:b9444a40a999 | 1252 | spiPreemtionByIsr = FALSE; |
gregeric | 0:b9444a40a999 | 1253 | if (itDisable) |
gregeric | 0:b9444a40a999 | 1254 | { |
gregeric | 0:b9444a40a999 | 1255 | /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ |
gregeric | 0:b9444a40a999 | 1256 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 1257 | itDisable = FALSE; |
gregeric | 0:b9444a40a999 | 1258 | } |
gregeric | 0:b9444a40a999 | 1259 | |
gregeric | 0:b9444a40a999 | 1260 | for (i = 0; i < numberOfDevices; i++) |
gregeric | 0:b9444a40a999 | 1261 | { |
gregeric | 0:b9444a40a999 | 1262 | spiTxBursts[3][i] = L6474_NOP; |
gregeric | 0:b9444a40a999 | 1263 | } |
gregeric | 0:b9444a40a999 | 1264 | spiTxBursts[3][spiIndex] = param; |
gregeric | 0:b9444a40a999 | 1265 | |
gregeric | 0:b9444a40a999 | 1266 | /* Disable interruption before checking */ |
gregeric | 0:b9444a40a999 | 1267 | /* pre-emption by ISR and SPI transfers*/ |
gregeric | 0:b9444a40a999 | 1268 | BSP_MotorControlBoard_DisableIrq(); |
gregeric | 0:b9444a40a999 | 1269 | itDisable = TRUE; |
gregeric | 0:b9444a40a999 | 1270 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
gregeric | 0:b9444a40a999 | 1271 | |
gregeric | 0:b9444a40a999 | 1272 | L6474_WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); |
gregeric | 0:b9444a40a999 | 1273 | |
gregeric | 0:b9444a40a999 | 1274 | /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ |
gregeric | 0:b9444a40a999 | 1275 | BSP_MotorControlBoard_EnableIrq(); |
gregeric | 0:b9444a40a999 | 1276 | } |
gregeric | 0:b9444a40a999 | 1277 | |
gregeric | 0:b9444a40a999 | 1278 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1279 | * @brief Sets the registers of the L6474 to their predefined values |
gregeric | 0:b9444a40a999 | 1280 | * from l6474_target_config.h |
gregeric | 0:b9444a40a999 | 1281 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1282 | * @retval None |
gregeric | 0:b9444a40a999 | 1283 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1284 | void L6474_SetRegisterToPredefinedValues(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 1285 | { |
gregeric | 0:b9444a40a999 | 1286 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1287 | L6474_ABS_POS, |
gregeric | 0:b9444a40a999 | 1288 | 0); |
gregeric | 0:b9444a40a999 | 1289 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1290 | L6474_EL_POS, |
gregeric | 0:b9444a40a999 | 1291 | 0); |
gregeric | 0:b9444a40a999 | 1292 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1293 | L6474_MARK, |
gregeric | 0:b9444a40a999 | 1294 | 0); |
gregeric | 0:b9444a40a999 | 1295 | switch (deviceId) |
gregeric | 0:b9444a40a999 | 1296 | { |
gregeric | 0:b9444a40a999 | 1297 | case 0: |
gregeric | 0:b9444a40a999 | 1298 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1299 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1300 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); |
gregeric | 0:b9444a40a999 | 1301 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1302 | L6474_T_FAST, |
gregeric | 0:b9444a40a999 | 1303 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1304 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0); |
gregeric | 0:b9444a40a999 | 1305 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1306 | L6474_TON_MIN, |
gregeric | 0:b9444a40a999 | 1307 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0) |
gregeric | 0:b9444a40a999 | 1308 | ); |
gregeric | 0:b9444a40a999 | 1309 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1310 | L6474_TOFF_MIN, |
gregeric | 0:b9444a40a999 | 1311 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0)); |
gregeric | 0:b9444a40a999 | 1312 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1313 | L6474_OCD_TH, |
gregeric | 0:b9444a40a999 | 1314 | L6474_CONF_PARAM_OCD_TH_DEVICE_0); |
gregeric | 0:b9444a40a999 | 1315 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1316 | L6474_STEP_MODE, |
gregeric | 0:b9444a40a999 | 1317 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1318 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0); |
gregeric | 0:b9444a40a999 | 1319 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1320 | L6474_ALARM_EN, |
gregeric | 0:b9444a40a999 | 1321 | L6474_CONF_PARAM_ALARM_EN_DEVICE_0); |
gregeric | 0:b9444a40a999 | 1322 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1323 | L6474_CONFIG, |
gregeric | 0:b9444a40a999 | 1324 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1325 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1326 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1327 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 | |
gregeric | 0:b9444a40a999 | 1328 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0); |
gregeric | 0:b9444a40a999 | 1329 | break; |
gregeric | 0:b9444a40a999 | 1330 | case 1: |
gregeric | 0:b9444a40a999 | 1331 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1332 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1333 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); |
gregeric | 0:b9444a40a999 | 1334 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1335 | L6474_T_FAST, |
gregeric | 0:b9444a40a999 | 1336 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1337 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1); |
gregeric | 0:b9444a40a999 | 1338 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1339 | L6474_TON_MIN, |
gregeric | 0:b9444a40a999 | 1340 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1)); |
gregeric | 0:b9444a40a999 | 1341 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1342 | L6474_TOFF_MIN, |
gregeric | 0:b9444a40a999 | 1343 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1)); |
gregeric | 0:b9444a40a999 | 1344 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1345 | L6474_OCD_TH, |
gregeric | 0:b9444a40a999 | 1346 | L6474_CONF_PARAM_OCD_TH_DEVICE_1); |
gregeric | 0:b9444a40a999 | 1347 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1348 | L6474_STEP_MODE, |
gregeric | 0:b9444a40a999 | 1349 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1350 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1); |
gregeric | 0:b9444a40a999 | 1351 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1352 | L6474_ALARM_EN, |
gregeric | 0:b9444a40a999 | 1353 | L6474_CONF_PARAM_ALARM_EN_DEVICE_1); |
gregeric | 0:b9444a40a999 | 1354 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1355 | L6474_CONFIG, |
gregeric | 0:b9444a40a999 | 1356 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1357 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1358 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1359 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 | |
gregeric | 0:b9444a40a999 | 1360 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1); |
gregeric | 0:b9444a40a999 | 1361 | break; |
gregeric | 0:b9444a40a999 | 1362 | case 2: |
gregeric | 0:b9444a40a999 | 1363 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1364 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1365 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); |
gregeric | 0:b9444a40a999 | 1366 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1367 | L6474_T_FAST, |
gregeric | 0:b9444a40a999 | 1368 | (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1369 | (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2); |
gregeric | 0:b9444a40a999 | 1370 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1371 | L6474_TON_MIN, |
gregeric | 0:b9444a40a999 | 1372 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2)); |
gregeric | 0:b9444a40a999 | 1373 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1374 | L6474_TOFF_MIN, |
gregeric | 0:b9444a40a999 | 1375 | L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2)); |
gregeric | 0:b9444a40a999 | 1376 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1377 | L6474_OCD_TH, |
gregeric | 0:b9444a40a999 | 1378 | L6474_CONF_PARAM_OCD_TH_DEVICE_2); |
gregeric | 0:b9444a40a999 | 1379 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1380 | L6474_STEP_MODE, |
gregeric | 0:b9444a40a999 | 1381 | (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1382 | (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2); |
gregeric | 0:b9444a40a999 | 1383 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1384 | L6474_ALARM_EN, |
gregeric | 0:b9444a40a999 | 1385 | L6474_CONF_PARAM_ALARM_EN_DEVICE_2); |
gregeric | 0:b9444a40a999 | 1386 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1387 | L6474_CONFIG, |
gregeric | 0:b9444a40a999 | 1388 | (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1389 | (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1390 | (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1391 | (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 | |
gregeric | 0:b9444a40a999 | 1392 | (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2); |
gregeric | 0:b9444a40a999 | 1393 | break; |
gregeric | 0:b9444a40a999 | 1394 | default: ; |
gregeric | 0:b9444a40a999 | 1395 | } |
gregeric | 0:b9444a40a999 | 1396 | } |
gregeric | 0:b9444a40a999 | 1397 | |
gregeric | 0:b9444a40a999 | 1398 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1399 | * @brief Sets the parameters of the device to predefined values |
gregeric | 0:b9444a40a999 | 1400 | * from l6474_target_config.h |
gregeric | 0:b9444a40a999 | 1401 | * @param None |
gregeric | 0:b9444a40a999 | 1402 | * @retval None |
gregeric | 0:b9444a40a999 | 1403 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1404 | void L6474_SetDeviceParamsToPredefinedValues(void) |
gregeric | 0:b9444a40a999 | 1405 | { |
gregeric | 0:b9444a40a999 | 1406 | uint32_t i; |
gregeric | 0:b9444a40a999 | 1407 | |
gregeric | 0:b9444a40a999 | 1408 | devicePrm[0].acceleration = L6474_CONF_PARAM_ACC_DEVICE_0; |
gregeric | 0:b9444a40a999 | 1409 | devicePrm[0].deceleration = L6474_CONF_PARAM_DEC_DEVICE_0; |
gregeric | 0:b9444a40a999 | 1410 | devicePrm[0].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0; |
gregeric | 0:b9444a40a999 | 1411 | devicePrm[0].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0; |
gregeric | 0:b9444a40a999 | 1412 | |
gregeric | 0:b9444a40a999 | 1413 | devicePrm[1].acceleration = L6474_CONF_PARAM_ACC_DEVICE_1; |
gregeric | 0:b9444a40a999 | 1414 | devicePrm[1].deceleration = L6474_CONF_PARAM_DEC_DEVICE_1; |
gregeric | 0:b9444a40a999 | 1415 | devicePrm[1].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1; |
gregeric | 0:b9444a40a999 | 1416 | devicePrm[1].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1; |
gregeric | 0:b9444a40a999 | 1417 | |
gregeric | 0:b9444a40a999 | 1418 | devicePrm[2].acceleration = L6474_CONF_PARAM_ACC_DEVICE_2; |
gregeric | 0:b9444a40a999 | 1419 | devicePrm[2].deceleration = L6474_CONF_PARAM_DEC_DEVICE_2; |
gregeric | 0:b9444a40a999 | 1420 | devicePrm[2].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2; |
gregeric | 0:b9444a40a999 | 1421 | devicePrm[2].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2; |
gregeric | 0:b9444a40a999 | 1422 | |
gregeric | 0:b9444a40a999 | 1423 | for (i = 0; i < MAX_NUMBER_OF_DEVICES; i++) |
gregeric | 0:b9444a40a999 | 1424 | { |
gregeric | 0:b9444a40a999 | 1425 | devicePrm[i].accu = 0; |
gregeric | 0:b9444a40a999 | 1426 | devicePrm[i].currentPosition = 0; |
gregeric | 0:b9444a40a999 | 1427 | devicePrm[i].endAccPos = 0; |
gregeric | 0:b9444a40a999 | 1428 | devicePrm[i].relativePos = 0; |
gregeric | 0:b9444a40a999 | 1429 | devicePrm[i].startDecPos = 0; |
gregeric | 0:b9444a40a999 | 1430 | devicePrm[i].stepsToTake = 0; |
gregeric | 0:b9444a40a999 | 1431 | devicePrm[i].speed = 0; |
gregeric | 0:b9444a40a999 | 1432 | devicePrm[i].commandExecuted = NO_CMD; |
gregeric | 0:b9444a40a999 | 1433 | devicePrm[i].direction = FORWARD; |
gregeric | 0:b9444a40a999 | 1434 | devicePrm[i].motionState = INACTIVE; |
gregeric | 0:b9444a40a999 | 1435 | } |
gregeric | 0:b9444a40a999 | 1436 | |
gregeric | 0:b9444a40a999 | 1437 | for (i = 0; i < numberOfDevices; i++) |
gregeric | 0:b9444a40a999 | 1438 | { |
gregeric | 0:b9444a40a999 | 1439 | L6474_SetRegisterToPredefinedValues(i); |
gregeric | 0:b9444a40a999 | 1440 | } |
gregeric | 0:b9444a40a999 | 1441 | } |
gregeric | 0:b9444a40a999 | 1442 | |
gregeric | 0:b9444a40a999 | 1443 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1444 | * @brief Initialises the bridge parameters to start the movement |
gregeric | 0:b9444a40a999 | 1445 | * and enable the power bridge |
gregeric | 0:b9444a40a999 | 1446 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1447 | * @retval None |
gregeric | 0:b9444a40a999 | 1448 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1449 | void L6474_StartMovement(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 1450 | { |
gregeric | 0:b9444a40a999 | 1451 | #ifdef L6474_CONF_BRAKE_WHEN_HALTED |
gregeric | 0:b9444a40a999 | 1452 | /* Restore powerstage to full current capability */ |
gregeric | 0:b9444a40a999 | 1453 | switch (deviceId) |
gregeric | 0:b9444a40a999 | 1454 | { |
gregeric | 0:b9444a40a999 | 1455 | case 0: |
gregeric | 0:b9444a40a999 | 1456 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1457 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1458 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); |
gregeric | 0:b9444a40a999 | 1459 | break; |
gregeric | 0:b9444a40a999 | 1460 | case 1: |
gregeric | 0:b9444a40a999 | 1461 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1462 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1463 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); |
gregeric | 0:b9444a40a999 | 1464 | break; |
gregeric | 0:b9444a40a999 | 1465 | case 2: |
gregeric | 0:b9444a40a999 | 1466 | L6474_CmdSetParam(deviceId, |
gregeric | 0:b9444a40a999 | 1467 | L6474_TVAL, |
gregeric | 0:b9444a40a999 | 1468 | L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); |
gregeric | 0:b9444a40a999 | 1469 | break; |
gregeric | 0:b9444a40a999 | 1470 | default: ; |
gregeric | 0:b9444a40a999 | 1471 | } |
gregeric | 0:b9444a40a999 | 1472 | #endif |
gregeric | 0:b9444a40a999 | 1473 | |
gregeric | 0:b9444a40a999 | 1474 | /* Enable L6474 powerstage */ |
gregeric | 0:b9444a40a999 | 1475 | L6474_CmdEnable(deviceId); |
gregeric | 0:b9444a40a999 | 1476 | |
gregeric | 0:b9444a40a999 | 1477 | if (devicePrm[deviceId].endAccPos != 0) |
gregeric | 0:b9444a40a999 | 1478 | { |
gregeric | 0:b9444a40a999 | 1479 | devicePrm[deviceId].motionState = ACCELERATING; |
gregeric | 0:b9444a40a999 | 1480 | } |
gregeric | 0:b9444a40a999 | 1481 | else |
gregeric | 0:b9444a40a999 | 1482 | { |
gregeric | 0:b9444a40a999 | 1483 | devicePrm[deviceId].motionState = DECELERATING; |
gregeric | 0:b9444a40a999 | 1484 | } |
gregeric | 0:b9444a40a999 | 1485 | devicePrm[deviceId].accu = 0; |
gregeric | 0:b9444a40a999 | 1486 | devicePrm[deviceId].relativePos = 0; |
gregeric | 0:b9444a40a999 | 1487 | L6474_ApplySpeed(deviceId, devicePrm[deviceId].minSpeed); |
gregeric | 0:b9444a40a999 | 1488 | } |
gregeric | 0:b9444a40a999 | 1489 | |
gregeric | 0:b9444a40a999 | 1490 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1491 | * @brief Handles the device state machine at each ste |
gregeric | 0:b9444a40a999 | 1492 | * @param[in] deviceId (from 0 to 2) |
gregeric | 0:b9444a40a999 | 1493 | * @retval None |
gregeric | 0:b9444a40a999 | 1494 | * @note Must only be called by the timer ISR |
gregeric | 0:b9444a40a999 | 1495 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1496 | void L6474_StepClockHandler(uint8_t deviceId) |
gregeric | 0:b9444a40a999 | 1497 | { |
gregeric | 0:b9444a40a999 | 1498 | /* Set isr flag */ |
gregeric | 0:b9444a40a999 | 1499 | isrFlag = TRUE; |
gregeric | 0:b9444a40a999 | 1500 | |
gregeric | 0:b9444a40a999 | 1501 | /* Incrementation of the relative position */ |
gregeric | 0:b9444a40a999 | 1502 | devicePrm[deviceId].relativePos++; |
gregeric | 0:b9444a40a999 | 1503 | |
gregeric | 0:b9444a40a999 | 1504 | switch (devicePrm[deviceId].motionState) |
gregeric | 0:b9444a40a999 | 1505 | { |
gregeric | 0:b9444a40a999 | 1506 | case ACCELERATING: |
gregeric | 0:b9444a40a999 | 1507 | { |
gregeric | 0:b9444a40a999 | 1508 | uint32_t relPos = devicePrm[deviceId].relativePos; |
gregeric | 0:b9444a40a999 | 1509 | uint32_t endAccPos = devicePrm[deviceId].endAccPos; |
gregeric | 0:b9444a40a999 | 1510 | uint16_t speed = devicePrm[deviceId].speed; |
gregeric | 0:b9444a40a999 | 1511 | uint32_t acc = ((uint32_t)devicePrm[deviceId].acceleration << 16); |
gregeric | 0:b9444a40a999 | 1512 | |
gregeric | 0:b9444a40a999 | 1513 | if ((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)|| |
gregeric | 0:b9444a40a999 | 1514 | ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1515 | (relPos == devicePrm[deviceId].startDecPos))) |
gregeric | 0:b9444a40a999 | 1516 | { |
gregeric | 0:b9444a40a999 | 1517 | devicePrm[deviceId].motionState = DECELERATING; |
gregeric | 0:b9444a40a999 | 1518 | devicePrm[deviceId].accu = 0; |
gregeric | 0:b9444a40a999 | 1519 | } |
gregeric | 0:b9444a40a999 | 1520 | else if ((speed >= devicePrm[deviceId].maxSpeed)|| |
gregeric | 0:b9444a40a999 | 1521 | ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1522 | (relPos == endAccPos))) |
gregeric | 0:b9444a40a999 | 1523 | { |
gregeric | 0:b9444a40a999 | 1524 | devicePrm[deviceId].motionState = STEADY; |
gregeric | 0:b9444a40a999 | 1525 | } |
gregeric | 0:b9444a40a999 | 1526 | else |
gregeric | 0:b9444a40a999 | 1527 | { |
gregeric | 0:b9444a40a999 | 1528 | bool speedUpdated = FALSE; |
gregeric | 0:b9444a40a999 | 1529 | /* Go on accelerating */ |
gregeric | 0:b9444a40a999 | 1530 | if (speed == 0) speed =1; |
gregeric | 0:b9444a40a999 | 1531 | devicePrm[deviceId].accu += acc / speed; |
gregeric | 0:b9444a40a999 | 1532 | while (devicePrm[deviceId].accu >= (0X10000L)) |
gregeric | 0:b9444a40a999 | 1533 | { |
gregeric | 0:b9444a40a999 | 1534 | devicePrm[deviceId].accu -= (0X10000L); |
gregeric | 0:b9444a40a999 | 1535 | speed +=1; |
gregeric | 0:b9444a40a999 | 1536 | speedUpdated = TRUE; |
gregeric | 0:b9444a40a999 | 1537 | } |
gregeric | 0:b9444a40a999 | 1538 | |
gregeric | 0:b9444a40a999 | 1539 | if (speedUpdated) |
gregeric | 0:b9444a40a999 | 1540 | { |
gregeric | 0:b9444a40a999 | 1541 | if (speed > devicePrm[deviceId].maxSpeed) |
gregeric | 0:b9444a40a999 | 1542 | { |
gregeric | 0:b9444a40a999 | 1543 | speed = devicePrm[deviceId].maxSpeed; |
gregeric | 0:b9444a40a999 | 1544 | } |
gregeric | 0:b9444a40a999 | 1545 | devicePrm[deviceId].speed = speed; |
gregeric | 0:b9444a40a999 | 1546 | L6474_ApplySpeed(deviceId, devicePrm[deviceId].speed); |
gregeric | 0:b9444a40a999 | 1547 | } |
gregeric | 0:b9444a40a999 | 1548 | } |
gregeric | 0:b9444a40a999 | 1549 | break; |
gregeric | 0:b9444a40a999 | 1550 | } |
gregeric | 0:b9444a40a999 | 1551 | case STEADY: |
gregeric | 0:b9444a40a999 | 1552 | { |
gregeric | 0:b9444a40a999 | 1553 | uint16_t maxSpeed = devicePrm[deviceId].maxSpeed; |
gregeric | 0:b9444a40a999 | 1554 | uint32_t relativePos = devicePrm[deviceId].relativePos; |
gregeric | 0:b9444a40a999 | 1555 | if ((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)|| |
gregeric | 0:b9444a40a999 | 1556 | ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1557 | (relativePos >= (devicePrm[deviceId].startDecPos))) || |
gregeric | 0:b9444a40a999 | 1558 | ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1559 | (devicePrm[deviceId].speed > maxSpeed))) |
gregeric | 0:b9444a40a999 | 1560 | { |
gregeric | 0:b9444a40a999 | 1561 | devicePrm[deviceId].motionState = DECELERATING; |
gregeric | 0:b9444a40a999 | 1562 | devicePrm[deviceId].accu = 0; |
gregeric | 0:b9444a40a999 | 1563 | } |
gregeric | 0:b9444a40a999 | 1564 | else if ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1565 | (devicePrm[deviceId].speed < maxSpeed)) |
gregeric | 0:b9444a40a999 | 1566 | { |
gregeric | 0:b9444a40a999 | 1567 | devicePrm[deviceId].motionState = ACCELERATING; |
gregeric | 0:b9444a40a999 | 1568 | devicePrm[deviceId].accu = 0; |
gregeric | 0:b9444a40a999 | 1569 | } |
gregeric | 0:b9444a40a999 | 1570 | break; |
gregeric | 0:b9444a40a999 | 1571 | } |
gregeric | 0:b9444a40a999 | 1572 | case DECELERATING: |
gregeric | 0:b9444a40a999 | 1573 | { |
gregeric | 0:b9444a40a999 | 1574 | uint32_t relativePos = devicePrm[deviceId].relativePos; |
gregeric | 0:b9444a40a999 | 1575 | uint16_t speed = devicePrm[deviceId].speed; |
gregeric | 0:b9444a40a999 | 1576 | uint32_t deceleration = ((uint32_t)devicePrm[deviceId].deceleration << 16); |
gregeric | 0:b9444a40a999 | 1577 | if (((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)&&(speed <= devicePrm[deviceId].minSpeed))|| |
gregeric | 0:b9444a40a999 | 1578 | ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1579 | (relativePos >= devicePrm[deviceId].stepsToTake))) |
gregeric | 0:b9444a40a999 | 1580 | { |
gregeric | 0:b9444a40a999 | 1581 | /* Motion process complete */ |
gregeric | 0:b9444a40a999 | 1582 | L6474_HardStop(deviceId); |
gregeric | 0:b9444a40a999 | 1583 | } |
gregeric | 0:b9444a40a999 | 1584 | else if ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& |
gregeric | 0:b9444a40a999 | 1585 | (speed <= devicePrm[deviceId].maxSpeed)) |
gregeric | 0:b9444a40a999 | 1586 | { |
gregeric | 0:b9444a40a999 | 1587 | devicePrm[deviceId].motionState = STEADY; |
gregeric | 0:b9444a40a999 | 1588 | } |
gregeric | 0:b9444a40a999 | 1589 | else |
gregeric | 0:b9444a40a999 | 1590 | { |
gregeric | 0:b9444a40a999 | 1591 | /* Go on decelerating */ |
gregeric | 0:b9444a40a999 | 1592 | if (speed > devicePrm[deviceId].minSpeed) |
gregeric | 0:b9444a40a999 | 1593 | { |
gregeric | 0:b9444a40a999 | 1594 | bool speedUpdated = FALSE; |
gregeric | 0:b9444a40a999 | 1595 | if (speed == 0) speed =1; |
gregeric | 0:b9444a40a999 | 1596 | devicePrm[deviceId].accu += deceleration / speed; |
gregeric | 0:b9444a40a999 | 1597 | while (devicePrm[deviceId].accu >= (0X10000L)) |
gregeric | 0:b9444a40a999 | 1598 | { |
gregeric | 0:b9444a40a999 | 1599 | devicePrm[deviceId].accu -= (0X10000L); |
gregeric | 0:b9444a40a999 | 1600 | if (speed > 1) |
gregeric | 0:b9444a40a999 | 1601 | { |
gregeric | 0:b9444a40a999 | 1602 | speed -=1; |
gregeric | 0:b9444a40a999 | 1603 | } |
gregeric | 0:b9444a40a999 | 1604 | speedUpdated = TRUE; |
gregeric | 0:b9444a40a999 | 1605 | } |
gregeric | 0:b9444a40a999 | 1606 | |
gregeric | 0:b9444a40a999 | 1607 | if (speedUpdated) |
gregeric | 0:b9444a40a999 | 1608 | { |
gregeric | 0:b9444a40a999 | 1609 | if (speed < devicePrm[deviceId].minSpeed) |
gregeric | 0:b9444a40a999 | 1610 | { |
gregeric | 0:b9444a40a999 | 1611 | speed = devicePrm[deviceId].minSpeed; |
gregeric | 0:b9444a40a999 | 1612 | } |
gregeric | 0:b9444a40a999 | 1613 | devicePrm[deviceId].speed = speed; |
gregeric | 0:b9444a40a999 | 1614 | L6474_ApplySpeed(deviceId, devicePrm[deviceId].speed); |
gregeric | 0:b9444a40a999 | 1615 | } |
gregeric | 0:b9444a40a999 | 1616 | } |
gregeric | 0:b9444a40a999 | 1617 | } |
gregeric | 0:b9444a40a999 | 1618 | break; |
gregeric | 0:b9444a40a999 | 1619 | } |
gregeric | 0:b9444a40a999 | 1620 | default: |
gregeric | 0:b9444a40a999 | 1621 | { |
gregeric | 0:b9444a40a999 | 1622 | break; |
gregeric | 0:b9444a40a999 | 1623 | } |
gregeric | 0:b9444a40a999 | 1624 | } |
gregeric | 0:b9444a40a999 | 1625 | /* Set isr flag */ |
gregeric | 0:b9444a40a999 | 1626 | isrFlag = FALSE; |
gregeric | 0:b9444a40a999 | 1627 | } |
gregeric | 0:b9444a40a999 | 1628 | |
gregeric | 0:b9444a40a999 | 1629 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1630 | * @brief Converts mA in compatible values for TVAL register |
gregeric | 0:b9444a40a999 | 1631 | * @param[in] Tval |
gregeric | 0:b9444a40a999 | 1632 | * @retval TVAL values |
gregeric | 0:b9444a40a999 | 1633 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1634 | inline uint8_t L6474_Tval_Current_to_Par(double Tval) |
gregeric | 0:b9444a40a999 | 1635 | { |
gregeric | 0:b9444a40a999 | 1636 | return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); |
gregeric | 0:b9444a40a999 | 1637 | } |
gregeric | 0:b9444a40a999 | 1638 | |
gregeric | 0:b9444a40a999 | 1639 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1640 | * @brief Convert time in us in compatible values |
gregeric | 0:b9444a40a999 | 1641 | * for TON_MIN register |
gregeric | 0:b9444a40a999 | 1642 | * @param[in] Tmin |
gregeric | 0:b9444a40a999 | 1643 | * @retval TON_MIN values |
gregeric | 0:b9444a40a999 | 1644 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1645 | inline uint8_t L6474_Tmin_Time_to_Par(double Tmin) |
gregeric | 0:b9444a40a999 | 1646 | { |
gregeric | 0:b9444a40a999 | 1647 | return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); |
gregeric | 0:b9444a40a999 | 1648 | } |
gregeric | 0:b9444a40a999 | 1649 | |
gregeric | 0:b9444a40a999 | 1650 | /******************************************************//** |
gregeric | 0:b9444a40a999 | 1651 | * @brief Write and receive a byte via SPI |
gregeric | 0:b9444a40a999 | 1652 | * @param[in] pByteToTransmit pointer to the byte to transmit |
gregeric | 0:b9444a40a999 | 1653 | * @param[in] pReceivedByte pointer to the received byte |
gregeric | 0:b9444a40a999 | 1654 | * @retval None |
gregeric | 0:b9444a40a999 | 1655 | **********************************************************/ |
gregeric | 0:b9444a40a999 | 1656 | void L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) |
gregeric | 0:b9444a40a999 | 1657 | { |
gregeric | 0:b9444a40a999 | 1658 | if (BSP_MotorControlBoard_SpiWriteBytes(pByteToTransmit, pReceivedByte, numberOfDevices) != 0) |
gregeric | 0:b9444a40a999 | 1659 | { |
gregeric | 0:b9444a40a999 | 1660 | L6474_ErrorHandler(L6474_ERROR_1); |
gregeric | 0:b9444a40a999 | 1661 | } |
gregeric | 0:b9444a40a999 | 1662 | |
gregeric | 0:b9444a40a999 | 1663 | if (isrFlag) |
gregeric | 0:b9444a40a999 | 1664 | { |
gregeric | 0:b9444a40a999 | 1665 | spiPreemtionByIsr = TRUE; |
gregeric | 0:b9444a40a999 | 1666 | } |
gregeric | 0:b9444a40a999 | 1667 | } |
gregeric | 0:b9444a40a999 | 1668 | |
gregeric | 0:b9444a40a999 | 1669 | /** |
gregeric | 0:b9444a40a999 | 1670 | * @} |
gregeric | 0:b9444a40a999 | 1671 | */ |
gregeric | 0:b9444a40a999 | 1672 | |
gregeric | 0:b9444a40a999 | 1673 | /** |
gregeric | 0:b9444a40a999 | 1674 | * @} |
gregeric | 0:b9444a40a999 | 1675 | */ |
gregeric | 0:b9444a40a999 | 1676 | |
gregeric | 0:b9444a40a999 | 1677 | /** |
gregeric | 0:b9444a40a999 | 1678 | * @} |
gregeric | 0:b9444a40a999 | 1679 | */ |
gregeric | 0:b9444a40a999 | 1680 | |
gregeric | 0:b9444a40a999 | 1681 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |