Personal fork of the library
Fork of X_NUCLEO_IHM01A1 by
Diff: Components/l6474/l6474_class.cpp
- Revision:
- 0:2887415a46cd
- Child:
- 1:b38ebb8ea286
diff -r 000000000000 -r 2887415a46cd Components/l6474/l6474_class.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Components/l6474/l6474_class.cpp Wed Oct 14 15:12:18 2015 +0000 @@ -0,0 +1,1348 @@ +/** + ****************************************************************************** + * @file l6474_class.cpp + * @author IPC Rennes + * @version V1.5.0 + * @date November 12, 2014 + * @brief L6474 driver (fully integrated microstepping motor driver) + * @note (C) COPYRIGHT 2014 STMicroelectronics + ****************************************************************************** + * @attention + * + * <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2> + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + +/* Generated with Stm32CubeTOO -----------------------------------------------*/ + + +/* Revision ------------------------------------------------------------------*/ +/* + Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev + Branch/Trunk/Tag: trunk + Based on: X-CUBE-SPN1/trunk/Drivers/BSP/Components/l6474/l6474.c + Revision: 0 +*/ + + +/* Includes ------------------------------------------------------------------*/ + +#include "l6474_class.h" +#include "l6474.h" + + +/* Private constants ---------------------------------------------------------*/ + +/// Error while initialising the SPI +#define L6474_ERROR_0 (0x8000) +/// Error: Bad SPI transaction +#define L6474_ERROR_1 (0x8001) + +/// Maximum number of steps +#define MAX_STEPS (0x7FFFFFFF) + +/// Maximum frequency of the PWMs in Hz +#define L6474_MAX_PWM_FREQ (10000) + +/// Minimum frequency of the PWMs in Hz +#define L6474_MIN_PWM_FREQ (2) + + +/* Private variables --------------------------------------------------------*/ + +/* Number of devices. */ +uint8_t L6474::numberOfDevices = 0; + + +/* Private function prototypes -----------------------------------------------*/ + +/******************************************************//** + * @brief Attaches a user callback to the error Handler. + * The call back will be then called each time the library + * detects an error + * @param[in] callback Name of the callback to attach + * to the error Hanlder + * @retval None + **********************************************************/ +void L6474::L6474_AttachErrorHandler(void (*callback)(uint16_t error)) +{ + errorHandlerCallback = (void (*)(uint16_t error)) callback; +} + +/******************************************************//** + * @brief Attaches a user callback to the flag Interrupt + * The call back will be then called each time the status + * flag pin will be pulled down due to the occurrence of + * a programmed alarms ( OCD, thermal pre-warning or + * shutdown, UVLO, wrong command, non-performable command) + * @param[in] callback Name of the callback to attach + * to the Flag Interrupt + * @retval None + **********************************************************/ +void L6474::L6474_AttachFlagInterrupt(void (*callback)(void)) +{ + flagInterruptCallback = (void (*)(void))callback; +} + +/******************************************************//** + * @brief Starts the L6474 library + * @retval COMPONENT_OK in case of success + **********************************************************/ +DrvStatusTypeDef L6474::L6474_Init(MOTOR_InitTypeDef *L6474_Init) +{ + /* Initialise the PWMs used for the Step clocks ----------------------------*/ + L6474_PwmInit(); + + /* Initialise the L6474s ------------------------------------------------*/ + + /* Standby-reset deactivation */ + L6474_ReleaseReset(); + + /* Let a delay after reset */ + L6474_Delay(1); + + /* Set all registers and context variables to the predefined values from l6474_target_config.h */ + L6474_SetDeviceParamsToPredefinedValues(); + + /* Disable L6474 powerstage */ + L6474_CmdDisable(); + + /* Get Status to clear flags after start up */ + L6474_CmdGetStatus(); + + return COMPONENT_OK; +} + +/******************************************************//** + * @brief Read id + * @param id pointer to the identifier to be read. + * @retval COMPONENT_OK in case of success + **********************************************************/ +DrvStatusTypeDef L6474::L6474_ReadID(uint8_t *id) +{ + *id = type; + + return COMPONENT_OK; +} + +/******************************************************//** + * @brief Returns the acceleration of the specified device + * @retval Acceleration in pps^2 + **********************************************************/ +uint16_t L6474::L6474_GetAcceleration(void) +{ + return (devicePrm.acceleration); +} + +/******************************************************//** + * @brief Returns the current speed of the specified device + * @retval Speed in pps + **********************************************************/ +uint16_t L6474::L6474_GetCurrentSpeed(void) +{ + return devicePrm.speed; +} + +/******************************************************//** + * @brief Returns the deceleration of the specified device + * @retval Deceleration in pps^2 + **********************************************************/ +uint16_t L6474::L6474_GetDeceleration(void) +{ + return (devicePrm.deceleration); +} + +/******************************************************//** + * @brief Returns the device state + * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) + **********************************************************/ +motorState_t L6474::L6474_GetDeviceState(void) +{ + return devicePrm.motionState; +} + +/******************************************************//** + * @brief Returns the FW version of the library + * @param None + * @retval L6474_FW_VERSION + **********************************************************/ +uint8_t L6474::L6474_GetFwVersion(void) +{ + return (L6474_FW_VERSION); +} + +/******************************************************//** + * @brief Returns the mark position of the specified device + * @retval Mark register value converted in a 32b signed integer + **********************************************************/ +int32_t L6474::L6474_GetMark(void) +{ + return L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); +} + +/******************************************************//** + * @brief Returns the max speed of the specified device + * @retval maxSpeed in pps + **********************************************************/ +uint16_t L6474::L6474_GetMaxSpeed(void) +{ + return (devicePrm.maxSpeed); +} + +/******************************************************//** + * @brief Returns the min speed of the specified device + * @retval minSpeed in pps + **********************************************************/ +uint16_t L6474::L6474_GetMinSpeed(void) +{ + return (devicePrm.minSpeed); +} + +/******************************************************//** + * @brief Returns the ABS_POSITION of the specified device + * @retval ABS_POSITION register value converted in a 32b signed integer + **********************************************************/ +int32_t L6474::L6474_GetPosition(void) +{ + return L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); +} + +/******************************************************//** + * @brief Requests the motor to move to the home position (ABS_POSITION = 0) + * @retval None + **********************************************************/ +void L6474::L6474_GoHome(void) +{ + L6474_GoTo(0); +} + +/******************************************************//** + * @brief Requests the motor to move to the mark position + * @retval None + **********************************************************/ +void L6474::L6474_GoMark(void) +{ + uint32_t mark; + + mark = L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); + L6474_GoTo(mark); +} + +/******************************************************//** + * @brief Requests the motor to move to the specified position + * @param[in] targetPosition absolute position in steps + * @retval None + **********************************************************/ +void L6474::L6474_GoTo(int32_t targetPosition) +{ + motorDir_t direction; + int32_t steps; + + /* Eventually deactivate motor */ + if (devicePrm.motionState != INACTIVE) + { + L6474_HardStop(); + } + + /* Get current position */ + devicePrm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); + + /* Compute the number of steps to perform */ + steps = targetPosition - devicePrm.currentPosition; + + if (steps >= 0) + { + devicePrm.stepsToTake = steps; + direction = FORWARD; + } + else + { + devicePrm.stepsToTake = -steps; + direction = BACKWARD; + } + + if (steps != 0) + { + devicePrm.commandExecuted = MOVE_CMD; + + /* Direction setup */ + L6474_SetDirection(direction); + + L6474_ComputeSpeedProfile(devicePrm.stepsToTake); + + /* Motor activation */ + L6474_StartMovement(); + } +} + +/******************************************************//** + * @brief Immediatly stops the motor and disable the power bridge + * @retval None + **********************************************************/ +void L6474::L6474_HardStop(void) +{ + /* Disable corresponding PWM */ + L6474_PwmStop(); + + /* Disable power stage */ + L6474_CmdDisable(); + + /* Set inactive state */ + devicePrm.motionState = INACTIVE; + devicePrm.commandExecuted = NO_CMD; + devicePrm.stepsToTake = MAX_STEPS; +} + +/******************************************************//** + * @brief Moves the motor of the specified number of steps + * @param[in] direction FORWARD or BACKWARD + * @param[in] stepCount Number of steps to perform + * @retval None + **********************************************************/ +void L6474::L6474_Move(motorDir_t direction, uint32_t stepCount) +{ + /* Eventually deactivate motor */ + if (devicePrm.motionState != INACTIVE) + { + L6474_HardStop(); + } + + if (stepCount != 0) + { + devicePrm.stepsToTake = stepCount; + + devicePrm.commandExecuted = MOVE_CMD; + + devicePrm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); + + /* Direction setup */ + L6474_SetDirection(direction); + + L6474_ComputeSpeedProfile(stepCount); + + /* Motor activation */ + L6474_StartMovement(); + } +} + +#if 0 +/******************************************************//** + * @brief Resets all L6474 devices + * @param None + * @retval None + **********************************************************/ +void L6474::L6474_ResetAllDevices(void) +{ + uint8_t loop; + + for (loop = 0; loop < numberOfDevices; loop++) + { + /* Stop movement and disable power stage*/ + L6474_HardStop(loop); + } + L6474_Reset(); + L6474_Delay(1); // Reset pin must be forced low for at least 10us + L6474_ReleaseReset(); + L6474_Delay(1); +} +#endif + +/******************************************************//** + * @brief Runs the motor. It will accelerate from the min + * speed up to the max speed by using the device acceleration. + * @param[in] direction FORWARD or BACKWARD + * @retval None + **********************************************************/ +void L6474::L6474_Run(motorDir_t direction) +{ + /* Eventually deactivate motor */ + if (devicePrm.motionState != INACTIVE) + { + L6474_HardStop(); + } + + /* Direction setup */ + L6474_SetDirection(direction); + + devicePrm.commandExecuted = RUN_CMD; + + /* Motor activation */ + L6474_StartMovement(); +} + +/******************************************************//** + * @brief Changes the acceleration of the specified device + * @param[in] newAcc New acceleration to apply in pps^2 + * @retval true if the command is successfully executed, else false + * @note The command is not performed is the device is executing + * a MOVE or GOTO command (but it can be used during a RUN command) + **********************************************************/ +bool L6474::L6474_SetAcceleration(uint16_t newAcc) +{ + bool cmdExecuted = FALSE; + if ((newAcc != 0)&& + ((devicePrm.motionState == INACTIVE)|| + (devicePrm.commandExecuted == RUN_CMD))) + { + devicePrm.acceleration = newAcc; + cmdExecuted = TRUE; + } + return cmdExecuted; +} + +/******************************************************//** + * @brief Changes the deceleration of the specified device + * @param[in] newDec New deceleration to apply in pps^2 + * @retval true if the command is successfully executed, else false + * @note The command is not performed is the device is executing + * a MOVE or GOTO command (but it can be used during a RUN command) + **********************************************************/ +bool L6474::L6474_SetDeceleration(uint16_t newDec) +{ + bool cmdExecuted = FALSE; + if ((newDec != 0)&& + ((devicePrm.motionState == INACTIVE)|| + (devicePrm.commandExecuted == RUN_CMD))) + { + devicePrm.deceleration = newDec; + cmdExecuted = TRUE; + } + return cmdExecuted; +} + +/******************************************************//** + * @brief Set current position to be the Home position (ABS pos set to 0) + * @retval None + **********************************************************/ +void L6474::L6474_SetHome(void) +{ + L6474_CmdSetParam(L6474_ABS_POS, 0); +} + +/******************************************************//** + * @brief Sets current position to be the Mark position + * @retval None + **********************************************************/ +void L6474::L6474_SetMark(void) +{ + uint32_t mark = L6474_CmdGetParam(L6474_ABS_POS); + L6474_CmdSetParam(L6474_MARK, mark); +} + +/******************************************************//** + * @brief Changes the max speed of the specified device + * @param[in] newMaxSpeed New max speed to apply in pps + * @retval true if the command is successfully executed, else false + * @note The command is not performed is the device is executing + * a MOVE or GOTO command (but it can be used during a RUN command). + **********************************************************/ +bool L6474::L6474_SetMaxSpeed(uint16_t newMaxSpeed) +{ + bool cmdExecuted = FALSE; + if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&& + (newMaxSpeed <= L6474_MAX_PWM_FREQ) && + (devicePrm.minSpeed <= newMaxSpeed) && + ((devicePrm.motionState == INACTIVE)|| + (devicePrm.commandExecuted == RUN_CMD))) + { + devicePrm.maxSpeed = newMaxSpeed; + cmdExecuted = TRUE; + } + return cmdExecuted; +} + +/******************************************************//** + * @brief Changes the min speed of the specified device + * @param[in] newMinSpeed New min speed to apply in pps + * @retval true if the command is successfully executed, else false + * @note The command is not performed is the device is executing + * a MOVE or GOTO command (but it can be used during a RUN command). + **********************************************************/ +bool L6474::L6474_SetMinSpeed(uint16_t newMinSpeed) +{ + bool cmdExecuted = FALSE; + if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&& + (newMinSpeed <= L6474_MAX_PWM_FREQ) && + (newMinSpeed <= devicePrm.maxSpeed) && + ((devicePrm.motionState == INACTIVE)|| + (devicePrm.commandExecuted == RUN_CMD))) + { + devicePrm.minSpeed = newMinSpeed; + cmdExecuted = TRUE; + } + return cmdExecuted; +} + +/******************************************************//** + * @brief Stops the motor by using the device deceleration + * @retval true if the command is successfully executed, else false + * @note The command is not performed is the device is in INACTIVE state. + **********************************************************/ +bool L6474::L6474_SoftStop(void) +{ + bool cmdExecuted = FALSE; + if (devicePrm.motionState != INACTIVE) + { + devicePrm.commandExecuted = SOFT_STOP_CMD; + cmdExecuted = TRUE; + } + return (cmdExecuted); +} + +/******************************************************//** + * @brief Issue the Disable command to the L6474 of the specified device + * @retval None + **********************************************************/ +void L6474::L6474_CmdDisable(void) +{ + L6474_SendCommand(L6474_DISABLE); +} + +/******************************************************//** + * @brief Issues the Enable command to the L6474 of the specified device + * @retval None + **********************************************************/ +void L6474::L6474_CmdEnable(void) +{ + L6474_SendCommand(L6474_ENABLE); +} + +/******************************************************//** + * @brief Issues the GetParam command to the L6474 of the specified device + * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) + * @retval Register value + **********************************************************/ +uint32_t L6474::L6474_CmdGetParam(uint32_t param) +{ + uint32_t i; + uint32_t spiRxData; + uint8_t maxArgumentNbBytes = 0; + bool itDisable = FALSE; + + do + { + spiPreemtionByIsr = FALSE; + if (itDisable) + { + /* re-enable L6474_EnableIrq if disable in previous iteration */ + L6474_EnableIrq(); + itDisable = FALSE; + } + + spiTxBursts[0] = L6474_NOP; + spiTxBursts[1] = L6474_NOP; + spiTxBursts[2] = L6474_NOP; + spiTxBursts[3] = L6474_NOP; + spiRxBursts[1] = 0; + spiRxBursts[2] = 0; + spiRxBursts[3] = 0; + + switch (param) + { + case L6474_ABS_POS: ; + case L6474_MARK: + spiTxBursts[0] = ((uint8_t)L6474_GET_PARAM )| (param); + maxArgumentNbBytes = 3; + break; + case L6474_EL_POS: ; + case L6474_CONFIG: ; + case L6474_STATUS: + spiTxBursts[1] = ((uint8_t)L6474_GET_PARAM )| (param); + maxArgumentNbBytes = 2; + break; + default: + spiTxBursts[2] = ((uint8_t)L6474_GET_PARAM )| (param); + maxArgumentNbBytes = 1; + } + + /* Disable interruption before checking */ + /* pre-emption by ISR and SPI transfers*/ + L6474_DisableIrq(); + itDisable = TRUE; + } while (spiPreemtionByIsr); // check pre-emption by ISR + + for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; + i < L6474_CMD_ARG_MAX_NB_BYTES; + i++) + { + L6474_WriteBytes(&spiTxBursts[i], &spiRxBursts[i]); + } + + spiRxData = ((uint32_t)spiRxBursts[1] << 16) | + (spiRxBursts[2] << 8) | + (spiRxBursts[3]); + + /* re-enable L6474_EnableIrq after SPI transfers*/ + L6474_EnableIrq(); + + return (spiRxData); +} + +/******************************************************//** + * @brief Issues the GetStatus command to the L6474 of the specified device + * @retval Status Register value + * @note Once the GetStatus command is performed, the flags of the status register + * are reset. This is not the case when the status register is read with the + * GetParam command (via the functions L6474ReadStatusRegister or L6474CmdGetParam). + **********************************************************/ +uint16_t L6474::L6474_CmdGetStatus(void) +{ + uint32_t i; + uint16_t status; + bool itDisable = FALSE; + + do + { + spiPreemtionByIsr = FALSE; + if (itDisable) + { + /* re-enable L6474_EnableIrq if disable in previous iteration */ + L6474_EnableIrq(); + itDisable = FALSE; + } + + spiTxBursts[0] = L6474_NOP; + spiTxBursts[1] = L6474_NOP; + spiTxBursts[2] = L6474_NOP; + spiRxBursts[1] = 0; + spiRxBursts[2] = 0; + spiTxBursts[0] = L6474_GET_STATUS; + + /* Disable interruption before checking */ + /* pre-emption by ISR and SPI transfers*/ + L6474_DisableIrq(); + itDisable = TRUE; + } while (spiPreemtionByIsr); // check pre-emption by ISR + + for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++) + { + L6474_WriteBytes(&spiTxBursts[i], &spiRxBursts[i]); + } + status = (spiRxBursts[1] << 8) | (spiRxBursts[2]); + + /* re-enable L6474_EnableIrq after SPI transfers*/ + L6474_EnableIrq(); + + return (status); +} + +/******************************************************//** + * @brief Issues the Nop command to the L6474 of the specified device + * @retval None + **********************************************************/ +void L6474::L6474_CmdNop(void) +{ + L6474_SendCommand(L6474_NOP); +} + +/******************************************************//** + * @brief Issues the SetParam command to the L6474 of the specified device + * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) + * @param[in] value Value to set in the register + * @retval None + **********************************************************/ +void L6474::L6474_CmdSetParam(uint32_t param, uint32_t value) +{ + uint32_t i; + uint8_t maxArgumentNbBytes = 0; + bool itDisable = FALSE; + do + { + spiPreemtionByIsr = FALSE; + if (itDisable) + { + /* re-enable L6474_EnableIrq if disable in previous iteration */ + L6474_EnableIrq(); + itDisable = FALSE; + } + + spiTxBursts[0] = L6474_NOP; + spiTxBursts[1] = L6474_NOP; + spiTxBursts[2] = L6474_NOP; + spiTxBursts[3] = L6474_NOP; + + switch (param) + { + case L6474_ABS_POS: ; + case L6474_MARK: + spiTxBursts[0] = param; + spiTxBursts[1] = (uint8_t)(value >> 16); + spiTxBursts[2] = (uint8_t)(value >> 8); + maxArgumentNbBytes = 3; + break; + case L6474_EL_POS: ; + case L6474_CONFIG: + spiTxBursts[1] = param; + spiTxBursts[2] = (uint8_t)(value >> 8); + maxArgumentNbBytes = 2; + break; + default: + spiTxBursts[2] = param; + maxArgumentNbBytes = 1; + break; + } + spiTxBursts[3] = (uint8_t)(value); + + /* Disable interruption before checking */ + /* pre-emption by ISR and SPI transfers*/ + L6474_DisableIrq(); + itDisable = TRUE; + } while (spiPreemtionByIsr); // check pre-emption by ISR + + /* SPI transfer */ + for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; + i < L6474_CMD_ARG_MAX_NB_BYTES; + i++) + { + L6474_WriteBytes(&spiTxBursts[i],&spiRxBursts[i]); + } + /* re-enable L6474_EnableIrq after SPI transfers*/ + L6474_EnableIrq(); +} + +/******************************************************//** + * @brief Reads the Status Register value + * @retval Status register valued + * @note The status register flags are not cleared + * at the difference with L6474CmdGetStatus() + **********************************************************/ +uint16_t L6474::L6474_ReadStatusRegister(void) +{ + return (L6474_CmdGetParam(L6474_STATUS)); +} + +/******************************************************//** + * @brief Set the stepping mode + * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t + * @retval None + **********************************************************/ +void L6474::L6474_SelectStepMode(motorStepMode_t stepMod) +{ + uint8_t stepModeRegister; + L6474_STEP_SEL_t l6474StepMod; + + switch (stepMod) + { + case STEP_MODE_FULL: + l6474StepMod = L6474_STEP_SEL_1; + break; + case STEP_MODE_HALF: + l6474StepMod = L6474_STEP_SEL_1_2; + break; + case STEP_MODE_1_4: + l6474StepMod = L6474_STEP_SEL_1_4; + break; + case STEP_MODE_1_8: + l6474StepMod = L6474_STEP_SEL_1_8; + break; + case STEP_MODE_1_16: + default: + l6474StepMod = L6474_STEP_SEL_1_16; + break; + } + + /* Eventually deactivate motor */ + if (devicePrm.motionState != INACTIVE) + { + L6474_HardStop(); + } + + /* Read Step mode register and clear STEP_SEL field */ + stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(L6474_STEP_MODE)) ; + + /* Apply new step mode */ + L6474_CmdSetParam(L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod); + + /* Reset abs pos register */ + L6474_SetHome(); +} + +/******************************************************//** + * @brief Specifies the direction + * @param[in] dir FORWARD or BACKWARD + * @note The direction change is only applied if the device + * is in INACTIVE state + * @retval None + **********************************************************/ +void L6474::L6474_SetDirection(motorDir_t dir) +{ + if (devicePrm.motionState == INACTIVE) + { + devicePrm.direction = dir; + L6474_SetDirectionGpio(dir); + } +} + +/******************************************************//** + * @brief Updates the current speed of the device + * @param[in] newSpeed in pps + * @retval None + **********************************************************/ +void L6474::L6474_ApplySpeed(uint16_t newSpeed) +{ + if (newSpeed < L6474_MIN_PWM_FREQ) + { + newSpeed = L6474_MIN_PWM_FREQ; + } + if (newSpeed > L6474_MAX_PWM_FREQ) + { + newSpeed = L6474_MAX_PWM_FREQ; + } + + devicePrm.speed = newSpeed; + + L6474_PwmSetFreq(newSpeed); +} + +/******************************************************//** + * @brief Computes the speed profile according to the number of steps to move + * @param[in] nbSteps number of steps to perform + * @retval None + * @note Using the acceleration and deceleration of the device, + * this function determines the duration in steps of the acceleration, + * steady and deceleration phases. + * If the total number of steps to perform is big enough, a trapezoidal move + * is performed (i.e. there is a steady phase where the motor runs at the maximum + * speed. + * Else, a triangular move is performed (no steady phase: the maximum speed is never + * reached. + **********************************************************/ +void L6474::L6474_ComputeSpeedProfile(uint32_t nbSteps) +{ + uint32_t reqAccSteps; + uint32_t reqDecSteps; + + /* compute the number of steps to get the targeted speed */ + uint16_t minSpeed = devicePrm.minSpeed; + reqAccSteps = (devicePrm.maxSpeed - minSpeed); + reqAccSteps *= (devicePrm.maxSpeed + minSpeed); + reqDecSteps = reqAccSteps; + reqAccSteps /= (uint32_t)devicePrm.acceleration; + reqAccSteps /= 2; + + /* compute the number of steps to stop */ + reqDecSteps /= (uint32_t)devicePrm.deceleration; + reqDecSteps /= 2; + + if(( reqAccSteps + reqDecSteps ) > nbSteps) + { + /* Triangular move */ + /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ + uint32_t dec = devicePrm.deceleration; + uint32_t acc = devicePrm.acceleration; + + reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); + if (reqDecSteps > 1) + { + reqAccSteps = reqDecSteps - 1; + if(reqAccSteps == 0) + { + reqAccSteps = 1; + } + } + else + { + reqAccSteps = 0; + } + devicePrm.endAccPos = reqAccSteps; + devicePrm.startDecPos = reqDecSteps; + } + else + { + /* Trapezoidal move */ + /* accelerating phase to endAccPos */ + /* steady phase from endAccPos to startDecPos */ + /* decelerating from startDecPos to stepsToTake*/ + devicePrm.endAccPos = reqAccSteps; + devicePrm.startDecPos = nbSteps - reqDecSteps - 1; + } +} + +/******************************************************//** + * @brief Converts the ABS_POSITION register value to a 32b signed integer + * @param[in] abs_position_reg value of the ABS_POSITION register + * @retval operation_result 32b signed integer corresponding to the absolute position + **********************************************************/ +int32_t L6474::L6474_ConvertPosition(uint32_t abs_position_reg) +{ + int32_t operation_result; + + if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) + { + /* Negative register value */ + abs_position_reg = ~abs_position_reg; + abs_position_reg += 1; + + operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK); + operation_result = -operation_result; + } + else + { + operation_result = (int32_t) abs_position_reg; + } + return operation_result; +} + +/******************************************************//** + * @brief Error handler which calls the user callback (if defined) + * @param[in] error Number of the error + * @retval None + **********************************************************/ +void L6474::L6474_ErrorHandler(uint16_t error) +{ + if (errorHandlerCallback != 0) + { + (void) errorHandlerCallback(error); + } + else + { + while(1) + { + /* Infinite loop */ + } + } +} + +/******************************************************//** + * @brief Handlers of the flag interrupt which calls the user callback (if defined) + * @param None + * @retval None + **********************************************************/ +void L6474::L6474_FlagInterruptHandler(void) +{ + if (flagInterruptCallback != 0) + { + /* Set isr flag */ + isrFlag = TRUE; + + flagInterruptCallback(); + + /* Reset isr flag */ + isrFlag = FALSE; + } +} + +/******************************************************//** + * @brief Sends a command without arguments to the L6474 via the SPI + * @param[in] param Command to send + * @retval None + **********************************************************/ +void L6474::L6474_SendCommand(uint8_t param) +{ + bool itDisable = FALSE; + + do + { + spiPreemtionByIsr = FALSE; + if (itDisable) + { + /* re-enable L6474_EnableIrq if disable in previous iteration */ + L6474_EnableIrq(); + itDisable = FALSE; + } + + spiTxBursts[3] = L6474_NOP; + spiTxBursts[3] = param; + + /* Disable interruption before checking */ + /* pre-emption by ISR and SPI transfers*/ + L6474_DisableIrq(); + itDisable = TRUE; + } while (spiPreemtionByIsr); // check pre-emption by ISR + + L6474_WriteBytes(&spiTxBursts[3], &spiRxBursts[3]); + + /* re-enable L6474_EnableIrq after SPI transfers*/ + L6474_EnableIrq(); +} + +/******************************************************//** + * @brief Sets the registers of the L6474 to their predefined values + * from l6474_target_config.h + * @retval None + **********************************************************/ +void L6474::L6474_SetRegisterToPredefinedValues(void) +{ + L6474_CmdSetParam( + L6474_ABS_POS, + 0); + L6474_CmdSetParam( + L6474_EL_POS, + 0); + L6474_CmdSetParam( + L6474_MARK, + 0); + switch (deviceInstance) + { + case 0: + L6474_CmdSetParam( + L6474_TVAL, + L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); + L6474_CmdSetParam( + L6474_T_FAST, + (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 | + (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0); + L6474_CmdSetParam( + L6474_TON_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0) + ); + L6474_CmdSetParam( + L6474_TOFF_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0)); + L6474_CmdSetParam( + L6474_OCD_TH, + L6474_CONF_PARAM_OCD_TH_DEVICE_0); + L6474_CmdSetParam( + L6474_STEP_MODE, + (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 | + (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0); + L6474_CmdSetParam( + L6474_ALARM_EN, + L6474_CONF_PARAM_ALARM_EN_DEVICE_0); + L6474_CmdSetParam( + L6474_CONFIG, + (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | + (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 | + (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 | + (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 | + (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0); + break; + case 1: + L6474_CmdSetParam( + L6474_TVAL, + L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); + L6474_CmdSetParam( + L6474_T_FAST, + (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 | + (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1); + L6474_CmdSetParam( + L6474_TON_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1)); + L6474_CmdSetParam( + L6474_TOFF_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1)); + L6474_CmdSetParam( + L6474_OCD_TH, + L6474_CONF_PARAM_OCD_TH_DEVICE_1); + L6474_CmdSetParam( + L6474_STEP_MODE, + (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 | + (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1); + L6474_CmdSetParam( + L6474_ALARM_EN, + L6474_CONF_PARAM_ALARM_EN_DEVICE_1); + L6474_CmdSetParam( + L6474_CONFIG, + (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | + (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 | + (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 | + (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 | + (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1); + break; + case 2: + L6474_CmdSetParam( + L6474_TVAL, + L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); + L6474_CmdSetParam( + L6474_T_FAST, + (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 | + (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2); + L6474_CmdSetParam( + L6474_TON_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2)); + L6474_CmdSetParam( + L6474_TOFF_MIN, + L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2)); + L6474_CmdSetParam( + L6474_OCD_TH, + L6474_CONF_PARAM_OCD_TH_DEVICE_2); + L6474_CmdSetParam( + L6474_STEP_MODE, + (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 | + (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2); + L6474_CmdSetParam( + L6474_ALARM_EN, + L6474_CONF_PARAM_ALARM_EN_DEVICE_2); + L6474_CmdSetParam( + L6474_CONFIG, + (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | + (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 | + (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 | + (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 | + (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2); + break; + default: ; + } +} + +/******************************************************//** + * @brief Sets the parameters of the device to predefined values + * from l6474_target_config.h + * @param None + * @retval None + **********************************************************/ +void L6474::L6474_SetDeviceParamsToPredefinedValues(void) +{ + switch (deviceInstance) + { + case 0: + devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_0; + devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_0; + devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0; + devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0; + break; + + case 1: + devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_1; + devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_1; + devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1; + devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1; + break; + + case 2: + devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_2; + devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_2; + devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2; + devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2; + break; + } + + devicePrm.accu = 0; + devicePrm.currentPosition = 0; + devicePrm.endAccPos = 0; + devicePrm.relativePos = 0; + devicePrm.startDecPos = 0; + devicePrm.stepsToTake = 0; + devicePrm.speed = 0; + devicePrm.commandExecuted = NO_CMD; + devicePrm.direction = FORWARD; + devicePrm.motionState = INACTIVE; + + L6474_SetRegisterToPredefinedValues(); +} + +/******************************************************//** + * @brief Initialises the bridge parameters to start the movement + * and enable the power bridge + * @retval None + **********************************************************/ +void L6474::L6474_StartMovement(void) +{ + /* Enable L6474 powerstage */ + L6474_CmdEnable(); + if (devicePrm.endAccPos != 0) + { + devicePrm.motionState = ACCELERATING; + } + else + { + devicePrm.motionState = DECELERATING; + } + devicePrm.accu = 0; + devicePrm.relativePos = 0; + L6474_ApplySpeed(devicePrm.minSpeed); +} + +/******************************************************//** + * @brief Handles the device state machine at each ste + * @retval None + * @note Must only be called by the timer ISR + **********************************************************/ +void L6474::L6474_StepClockHandler(void) +{ + /* Set isr flag */ + isrFlag = TRUE; + + /* Incrementation of the relative position */ + devicePrm.relativePos++; + + switch (devicePrm.motionState) + { + case ACCELERATING: + { + uint32_t relPos = devicePrm.relativePos; + uint32_t endAccPos = devicePrm.endAccPos; + uint16_t speed = devicePrm.speed; + uint32_t acc = ((uint32_t)devicePrm.acceleration << 16); + + if ((devicePrm.commandExecuted == SOFT_STOP_CMD)|| + ((devicePrm.commandExecuted != RUN_CMD)&& + (relPos == devicePrm.startDecPos))) + { + devicePrm.motionState = DECELERATING; + devicePrm.accu = 0; + } + else if ((speed >= devicePrm.maxSpeed)|| + ((devicePrm.commandExecuted != RUN_CMD)&& + (relPos == endAccPos))) + { + devicePrm.motionState = STEADY; + } + else + { + bool speedUpdated = FALSE; + /* Go on accelerating */ + if (speed == 0) speed =1; + devicePrm.accu += acc / speed; + while (devicePrm.accu >= (0X10000L)) + { + devicePrm.accu -= (0X10000L); + speed +=1; + speedUpdated = TRUE; + } + + if (speedUpdated) + { + if (speed > devicePrm.maxSpeed) + { + speed = devicePrm.maxSpeed; + } + devicePrm.speed = speed; + L6474_ApplySpeed(devicePrm.speed); + } + } + break; + } + case STEADY: + { + uint16_t maxSpeed = devicePrm.maxSpeed; + uint32_t relativePos = devicePrm.relativePos; + if ((devicePrm.commandExecuted == SOFT_STOP_CMD)|| + ((devicePrm.commandExecuted != RUN_CMD)&& + (relativePos >= (devicePrm.startDecPos))) || + ((devicePrm.commandExecuted == RUN_CMD)&& + (devicePrm.speed > maxSpeed))) + { + devicePrm.motionState = DECELERATING; + devicePrm.accu = 0; + } + else if ((devicePrm.commandExecuted == RUN_CMD)&& + (devicePrm.speed < maxSpeed)) + { + devicePrm.motionState = ACCELERATING; + devicePrm.accu = 0; + } + break; + } + case DECELERATING: + { + uint32_t relativePos = devicePrm.relativePos; + uint16_t speed = devicePrm.speed; + uint32_t deceleration = ((uint32_t)devicePrm.deceleration << 16); + if (((devicePrm.commandExecuted == SOFT_STOP_CMD)&&(speed <= devicePrm.minSpeed))|| + ((devicePrm.commandExecuted != RUN_CMD)&& + (relativePos >= devicePrm.stepsToTake))) + { + /* Motion process complete */ + L6474_HardStop(); + } + else if ((devicePrm.commandExecuted == RUN_CMD)&& + (speed <= devicePrm.maxSpeed)) + { + devicePrm.motionState = STEADY; + } + else + { + /* Go on decelerating */ + if (speed > devicePrm.minSpeed) + { + bool speedUpdated = FALSE; + if (speed == 0) speed =1; + devicePrm.accu += deceleration / speed; + while (devicePrm.accu >= (0X10000L)) + { + devicePrm.accu -= (0X10000L); + if (speed > 1) + { + speed -=1; + } + speedUpdated = TRUE; + } + + if (speedUpdated) + { + if (speed < devicePrm.minSpeed) + { + speed = devicePrm.minSpeed; + } + devicePrm.speed = speed; + L6474_ApplySpeed(devicePrm.speed); + } + } + } + break; + } + default: + { + break; + } + } + /* Set isr flag */ + isrFlag = FALSE; +} + +/******************************************************//** + * @brief Converts mA in compatible values for TVAL register + * @param[in] Tval + * @retval TVAL values + **********************************************************/ +uint8_t L6474::L6474_Tval_Current_to_Par(double Tval) +{ + return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); +} + +/******************************************************//** + * @brief Convert time in us in compatible values + * for TON_MIN register + * @param[in] Tmin + * @retval TON_MIN values + **********************************************************/ +uint8_t L6474::L6474_Tmin_Time_to_Par(double Tmin) +{ + return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); +} + +/******************************************************//** + * @brief Write and receive a byte via SPI + * @param[in] pByteToTransmit pointer to the byte to transmit + * @param[in] pReceivedByte pointer to the received byte + * @retval None + **********************************************************/ +void L6474::L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) +{ + if (L6474_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0) + { + L6474_ErrorHandler(L6474_ERROR_1); + } + + if (isrFlag) + { + spiPreemtionByIsr = TRUE; + } +} + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/