Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.
Dependencies: ST_INTERFACES
Dependents: HelloWorld_IHM06A1
Fork of X-NUCLEO-IHM06A1 by
Motor Control Library
Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.
It features the:
- Read and write of the device parameters; GPIO, PWM and IRQ configuration; microstepping, direction position, speed, acceleration, deceleration and torque controls
- Automatic full-step switch management; high impedance or hold stop mode selection; enable and standby management
- Fault interrupts handling (over current, short-circuit and over temperature)
- Command locking until the device completes movement
The API allows to easily:
- perform various positioning, moves and stops
- get/set or monitor the motor positions
- set the home position and mark another position
- get/set the minimum and maximum speed
- get the current speed
- get/set the acceleration and deceleration
- get/set the stop mode (hold, hiz or standby)
- get/set the torque
- get/set the torque boost
- get/set the step mode (up to 1/256)
Platform compatibility
Compatible platforms have been tested with the configurations provided by the HelloWorld_IHM06A1 example.
Components/stspin220/STSpin220.cpp
- Committer:
- davide.aliprandi@st.com
- Date:
- 2017-03-24
- Revision:
- 3:a132aa6d66e4
File content as of revision 3:a132aa6d66e4:
/** ****************************************************************************** * @file STSpin220.cpp * @author IPC Rennes * @version V1.0.0 * @date July 27th, 2016 * @brief STSpin220 product related routines * @note (C) COPYRIGHT 2016 STMicroelectronics ****************************************************************************** * @attention * * <h2><center>© COPYRIGHT(c) 2016 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. * ****************************************************************************** */ /* Includes ------------------------------------------------------------------*/ #include "STSpin220.h" /* Definitions ---------------------------------------------------------------*/ /* Variables ----------------------------------------------------------------*/ /* Number of devices. */ uint8_t STSpin220::numberOfDevices = 0; /* Methods -------------------------------------------------------------------*/ /******************************************************//** * @brief Start the STSpin220 library * @param[in] pInit pointer to the initialization data * @retval COMPONENT_OK in case of success. **********************************************************/ status_t STSpin220::Stspin220_Init(void* pInit) { Stspin220_Disable(); if (pInit == NULL) { /* Set context variables to the predefined values from Stspin220_target_config.h */ /* Set GPIO according to these values */ Stspin220_SetDeviceParamsToPredefinedValues(); } else { Stspin220_SetDeviceParamsToGivenValues((Stspin220_init_t*) pInit); } Stspin220_Board_TimStckInit(false); return COMPONENT_OK; } /********************************************************** * @brief Read id * @param id pointer to the identifier to be read. * @retval COMPONENT_OK in case of success. **********************************************************/ status_t STSpin220::Stspin220_ReadID(uint8_t *id) { *id = deviceInstance; return COMPONENT_OK; } /********************************************************** * @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 STSpin220::Stspin220_AttachErrorHandler(void (*callback)(uint16_t error)) { errorHandlerCallback = (void (*)(uint16_t error)) callback; } /******************************************************//** * @brief Apply the set torque * @param[in] torqueMode torque mode * @retval None * @note **********************************************************/ void STSpin220::Stspin220_ApplyTorque(motorTorqueMode_t torqueMode) { uint8_t torqueValue = 0; devicePrm.updateTorque = false; switch(torqueMode) { case ACC_TORQUE: devicePrm.currentTorque = devicePrm.accelTorque; break; case DEC_TORQUE: devicePrm.currentTorque = devicePrm.decelTorque; break; case RUN_TORQUE: devicePrm.currentTorque = devicePrm.runTorque; break; case HOLD_TORQUE: devicePrm.currentTorque = devicePrm.holdTorque; break; case CURRENT_TORQUE: break; default: return; //ignore error } torqueValue = devicePrm.currentTorque; Stspin220_Board_PwmRefSetDutyCycle(torqueValue); } /******************************************************//** * @brief Disable the power bridges (leave the output bridges HiZ) * @retval None **********************************************************/ void STSpin220::Stspin220_disable(void) { Stspin220_Board_Disable(); } /******************************************************//** * @brief Enable the power bridges * @retval None **********************************************************/ void STSpin220::Stspin220_enable(void) { Stspin220_Board_Enable(); } /******************************************************//** * @brief Error handler which calls the user callback (if defined) * @param[in] error Number of the error * @retval None **********************************************************/ void STSpin220::Stspin220_ErrorHandler(uint16_t error) { if (errorHandlerCallback != 0) { errorHandlerCallback(error); } else { while(1) { /* Infinite loop */ } } } /******************************************************//** * @brief Exit STSpin220 device from standby (low power consumption) * @retval None **********************************************************/ void STSpin220::Stspin220_ExitDeviceFromStandby(void) { uint32_t sequencerPosition = devicePrm.sequencerPosition; /* Exit standby and set step mode */ Stspin220_SetStepModeWithoutReset(devicePrm.stepMode); if (devicePrm.sequencerPosition != 0) { /* Set direction to FORWARD to ensure the HW sequencer is increased at */ /* each step clock rising edge */ Stspin220_SetDirection(FORWARD); /* Going out of standby */ devicePrm.motionState = STANDBYTOINACTIVE; /* Program the step clock */ Stspin220_Board_TimStckInit(true); Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); toggleOdd = 0; while (devicePrm.sequencerPosition != 0); while (toggleOdd!=0); devicePrm.sequencerPosition = sequencerPosition; } devicePrm.motionState = INACTIVE; } /******************************************************//** * @brief Return the acceleration * @retval Acceleration in pps^2 **********************************************************/ uint16_t STSpin220::Stspin220_GetAcceleration(void) { return (devicePrm.acceleration); } /******************************************************//** * @brief Return the current speed * @retval Speed in pps **********************************************************/ uint16_t STSpin220::Stspin220_GetCurrentSpeed(void) { return devicePrm.speed; } /******************************************************//** * @brief Return the deceleration * @retval Deceleration in pps^2 **********************************************************/ uint16_t STSpin220::Stspin220_GetDeceleration(void) { return (devicePrm.deceleration); } /******************************************************//** * @brief Return the device state * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) **********************************************************/ motorState_t STSpin220::Stspin220_get_device_state(void) { return devicePrm.motionState; } /******************************************************//** * @brief Get the motor current direction * @retval direction **********************************************************/ motorDir_t STSpin220::Stspin220_GetDirection(void) { return devicePrm.direction; } /******************************************************//** * @brief Return the FW version. * @retval FW version **********************************************************/ uint32_t STSpin220::Stspin220_GetFwVersion(void) { return (STSPIN220_FW_VERSION); } /******************************************************//** * @brief Get the mark position (32b signed) * @retval mark position **********************************************************/ int32_t STSpin220::Stspin220_GetMark(void) { return devicePrm.markPosition; } /******************************************************//** * @brief Return the max speed * @retval maxSpeed in pps **********************************************************/ uint16_t STSpin220::Stspin220_GetMaxSpeed(void) { return (devicePrm.maxSpeed); } /******************************************************//** * @brief Get the min speed * in step/s for full, half and wave modes * in microsteps/s for microstep modes * @retval return the min speed in step/s or microstep/s * @note **********************************************************/ uint16_t STSpin220::Stspin220_GetMinSpeed(void) { return (devicePrm.minSpeed); } /******************************************************//** * @brief Get the current position (32b signed) * @retval current position value **********************************************************/ int32_t STSpin220::Stspin220_GetPosition(void) { return devicePrm.currentPosition; } /******************************************************//** * @brief Get the motor step mode * @retval step mode **********************************************************/ motorStepMode_t STSpin220::Stspin220_get_step_mode(void) { return devicePrm.stepMode; } /******************************************************//** * @brief Get the selected stop mode * @retval the selected stop mode **********************************************************/ motorStopMode_t STSpin220::Stspin220_GetStopMode(void) { return devicePrm.stopMode; } /******************************************************//** * @brief Get the torque * @param[in] torqueMode torque mode * @retval the torqueValue in % (from 0 to 100) * @note **********************************************************/ uint8_t STSpin220::Stspin220_GetTorque(motorTorqueMode_t torqueMode) { uint8_t torqueValue = 0; switch(torqueMode) { case ACC_TORQUE: torqueValue = devicePrm.accelTorque; break; case DEC_TORQUE: torqueValue = devicePrm.decelTorque; break; case RUN_TORQUE: torqueValue = devicePrm.runTorque; break; case HOLD_TORQUE: torqueValue = devicePrm.holdTorque; break; case CURRENT_TORQUE: torqueValue = devicePrm.currentTorque; break; default: break; } return torqueValue; } /******************************************************//** * @brief Get the torque boost feature status * @retval TRUE if enabled, FALSE if disabled **********************************************************/ bool STSpin220::Stspin220_GetTorqueBoostenable(void) { return devicePrm.torqueBoostEnable; } /******************************************************//** * @brief Get the torque boost threshold * @retval The torque boost threshold above which the step mode is * changed to full step **********************************************************/ uint16_t STSpin220::Stspin220_GetTorqueBoostThreshold(void) { return devicePrm.torqueBoostSpeedThreshold; } /******************************************************//** * @brief Go to the home position * @retval None **********************************************************/ void STSpin220::Stspin220_GoHome(void) { Stspin220_GoTo(0); } /******************************************************//** * @brief Go to the Mark position * @retval None **********************************************************/ void STSpin220::Stspin220_GoMark(void) { Stspin220_GoTo(devicePrm.markPosition); } /******************************************************//** * @brief Request the motor to move to the specified position * @param[in] targetPosition absolute position in steps * @retval None * @note The position is at the resolution corresponding to the * selected step mode. * STEP_MODE_FULL : step * STEP_MODE_HALF : 1/2 step * STEP_MODE_1_4 : 1/4 step * STEP_MODE_1_8 : 1/8 step * STEP_MODE_1_16 : 1/16 step * STEP_MODE_1_32 : 1/32 step * STEP_MODE_1_64 : 1/64 step * STEP_MODE_1_128 : 1/128 step * STEP_MODE_1_256 : 1/256 step **********************************************************/ void STSpin220::Stspin220_GoTo(int32_t targetPosition) { motorDir_t direction; /* Exit from standby if needed */ if (devicePrm.motionState == STANDBY) { Stspin220_ExitDeviceFromStandby(); } /* Deactivate motor if needed */ else if (devicePrm.motionState != INACTIVE) { Stspin220_HardHiZ(); } if (targetPosition > devicePrm.currentPosition) { devicePrm.stepsToTake = targetPosition -\ devicePrm.currentPosition; if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) { direction = FORWARD; } else { direction = BACKWARD; devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ devicePrm.stepsToTake; } } else { devicePrm.stepsToTake = devicePrm.currentPosition -\ targetPosition; if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) { direction = BACKWARD; } else { direction = FORWARD; devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ devicePrm.stepsToTake; } } if (devicePrm.stepsToTake != 0) { devicePrm.commandExecuted = MOVE_CMD; /* Direction setup */ Stspin220_SetDirection(direction); Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake); /* Motor activation */ Stspin220_StartMovement(); } } /******************************************************//** * @brief move the motor to the absolute position * @param[in] direction FORWARD or BACKWARD * @param[in] targetPosition 32 bit signed value position * @retval None * @note The position is at the resolution corresponding to the * selected step mode. * STEP_MODE_FULL : step * STEP_MODE_HALF : 1/2 step * STEP_MODE_1_4 : 1/4 step * STEP_MODE_1_8 : 1/8 step * STEP_MODE_1_16 : 1/16 step * STEP_MODE_1_32 : 1/32 step * STEP_MODE_1_64 : 1/64 step * STEP_MODE_1_128 : 1/128 step * STEP_MODE_1_256 : 1/256 step **********************************************************/ void STSpin220::Stspin220_GoToDir(motorDir_t direction, int32_t targetPosition) { /* Exit from standby if needed */ if (devicePrm.motionState == STANDBY) { Stspin220_ExitDeviceFromStandby(); } /* Deactivate motor if needed */ else if (devicePrm.motionState != INACTIVE) { Stspin220_HardHiZ(); } if (direction != BACKWARD) { if (targetPosition > devicePrm.currentPosition) { devicePrm.stepsToTake = targetPosition -\ devicePrm.currentPosition; } else { devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ (targetPosition -\ devicePrm.currentPosition); } } else { if (targetPosition > devicePrm.currentPosition) { devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ (devicePrm.currentPosition -\ targetPosition); } else { devicePrm.stepsToTake = devicePrm.currentPosition -\ targetPosition; } } if (devicePrm.stepsToTake != 0) { devicePrm.commandExecuted = MOVE_CMD; /* Direction setup */ Stspin220_SetDirection(direction); Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake); /* Motor activation */ Stspin220_StartMovement(); } } /******************************************************//** * @brief Immediately stop the motor and disables the power bridges * @retval None **********************************************************/ void STSpin220::Stspin220_HardHiZ(void) { /* Set inactive state */ devicePrm.motionState = INACTIVE; /* Disable step clock */ if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) { Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); } /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */ /* after the last step clock rising edge triggering the last step */ Stspin220_Board_Delay(DISABLE_DELAY); /* Set reference voltage to 0 */ Stspin220_SetTorque(CURRENT_TORQUE, 0); /* Disable power bridges */ Stspin220_Board_Disable(); /* Comeback to nominal step mode */ if (devicePrm.stepModeLatched != devicePrm.stepMode) { Stspin220_Board_UnsetFullStep(); devicePrm.stepMode = devicePrm.stepModeLatched; } devicePrm.commandExecuted = NO_CMD; devicePrm.stepsToTake = 0; devicePrm.speed = 0; } /******************************************************//** * @brief Immediatly stop the motor * and either set holding torque when stop mode is HOLD_MODE, * or call Stspin220_HardHiz function when stop mode is HIZ_MODE, * or call Stspin220_PutDeviceInStandby function when stop mode is STANDBY_MODE * @retval None **********************************************************/ void STSpin220::Stspin220_HardStop(void) { if (devicePrm.stopMode == HOLD_MODE) { /* Set inactive state */ devicePrm.motionState = INACTIVE; /* Disable step clock */ if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) { Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); } /* Set holding torque */ Stspin220_ApplyTorque(HOLD_TORQUE); /* Comeback to nominal step mode */ if (devicePrm.stepModeLatched != devicePrm.stepMode) { Stspin220_Board_UnsetFullStep(); devicePrm.stepMode = devicePrm.stepModeLatched; } devicePrm.commandExecuted = NO_CMD; devicePrm.stepsToTake = 0; devicePrm.speed = 0; } else if (devicePrm.stopMode == HIZ_MODE) { Stspin220_HardHiZ(); } else if (devicePrm.stopMode == STANDBY_MODE) { Stspin220_PutDeviceInStandby(); } } /******************************************************//** * @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 STSpin220::Stspin220_Move(motorDir_t direction, uint32_t stepCount) { /* Exit from standby if needed */ if (devicePrm.motionState == STANDBY) { Stspin220_ExitDeviceFromStandby(); } /* Deactivate motor if needed */ else if (devicePrm.motionState != INACTIVE) { Stspin220_HardHiZ(); } if (stepCount != 0) { devicePrm.stepsToTake = stepCount; devicePrm.commandExecuted = MOVE_CMD; /* Direction setup */ Stspin220_SetDirection(direction); Stspin220_ComputeSpeedProfile(stepCount); /* Motor activation */ Stspin220_StartMovement(); } } /******************************************************//** * @brief Put STSpin220 device in standby (low power consumption) * @retval None **********************************************************/ void STSpin220::Stspin220_PutDeviceInStandby(void) { /* Stop movement */ Stspin220_HardHiZ(); /* Enter standby */ Stspin220_Board_Reset(); devicePrm.motionState = STANDBY; } /******************************************************//** * @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 STSpin220::Stspin220_Run(motorDir_t direction) { /* Exit from standby if needed */ if (devicePrm.motionState == STANDBY) { Stspin220_ExitDeviceFromStandby(); } /* Deactivate motor if needed */ else if (devicePrm.motionState != INACTIVE) { Stspin220_HardHiZ(); } /* Direction setup */ Stspin220_SetDirection(direction); devicePrm.commandExecuted = RUN_CMD; /* Motor activation */ Stspin220_StartMovement(); } /******************************************************//** * @brief Changes the acceleration * @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 if the device is executing * a MOVE or GOTO command (but it can be used during a RUN command) **********************************************************/ bool STSpin220::Stspin220_SetAcceleration(uint16_t newAcc) { bool cmdExecuted = false; if ((newAcc != 0)&& (((devicePrm.motionState & INACTIVE) == INACTIVE)|| (devicePrm.commandExecuted == RUN_CMD))) { devicePrm.acceleration = newAcc; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Changes the deceleration * @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 if the device is executing * a MOVE or GOTO command (but it can be used during a RUN command) **********************************************************/ bool STSpin220::Stspin220_SetDeceleration(uint16_t newDec) { bool cmdExecuted = false; if ((newDec != 0)&& (((devicePrm.motionState & INACTIVE) == INACTIVE)|| (devicePrm.commandExecuted == RUN_CMD))) { devicePrm.deceleration = newDec; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Specifies the direction * @param[in] dir FORWARD or BACKWARD * @note The direction change is applied if the device * is in INACTIVE or STANDBY state or if the device is * executing a run command * @retval None **********************************************************/ void STSpin220::Stspin220_SetDirection(motorDir_t dir) { if ((devicePrm.motionState == INACTIVE)||\ (devicePrm.motionState == STANDBY)) { devicePrm.direction = dir; Stspin220_Board_SetDirectionGpio(dir); } else if ((devicePrm.commandExecuted&RUN_CMD)!=0) { devicePrm.commandExecuted=(deviceCommand_t) (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted); } } /******************************************************//** * @brief Set current position to be the Home position * (current position set to 0) * @retval None **********************************************************/ void STSpin220::Stspin220_SetHome(void) { devicePrm.currentPosition = 0; } /******************************************************//** * @brief Set current position to be the Mark position * @retval None **********************************************************/ void STSpin220::Stspin220_SetMark(void) { devicePrm.markPosition = devicePrm.currentPosition; } /******************************************************//** * @brief Changes the max speed * @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 STSpin220::Stspin220_SetMaxSpeed(uint16_t newMaxSpeed) { bool cmdExecuted = false; if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\ ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\ ((devicePrm.torqueBoostEnable != false)&&\ ((newMaxSpeed>>Stspin220_get_step_mode())<= STSPIN220_MAX_STCK_FREQ)))&&\ (devicePrm.minSpeed <= newMaxSpeed) &&\ (((devicePrm.motionState & INACTIVE) == INACTIVE)||\ (devicePrm.commandExecuted == RUN_CMD))) { devicePrm.maxSpeed = newMaxSpeed; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Changes the min speed * @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 STSpin220::Stspin220_SetMinSpeed(uint16_t newMinSpeed) { bool cmdExecuted = false; if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&& (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) && (newMinSpeed <= devicePrm.maxSpeed) && (((devicePrm.motionState & INACTIVE) == INACTIVE)|| (devicePrm.commandExecuted == RUN_CMD))) { devicePrm.minSpeed = newMinSpeed; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Set the stepping mode * @param[in] stepMode from full step to 1/256 microstep * as specified in enum motorStepMode_t * @retval true if the command is successfully executed, else false **********************************************************/ bool STSpin220::Stspin220_SetStepMode(motorStepMode_t stepMode) { /* Eventually deactivate motor */ if ((devicePrm.motionState != INACTIVE)&&\ (devicePrm.motionState != STANDBY)) { Stspin220_HardHiZ(); } /* Enter standby */ Stspin220_Board_Reset(); /* Reset the microstepping sequencer position */ devicePrm.sequencerPosition = 0; /* Reset current and mark positions */ devicePrm.currentPosition = 0; devicePrm.markPosition = 0; /* Set the step mode */ return (Stspin220_SetStepModeWithoutReset(stepMode)); } /******************************************************//** * @brief Select the mode to stop the motor. * @param[in] stopMode HOLD_MODE to let power bridge enabled * @retval None **********************************************************/ void STSpin220::Stspin220_SetStopMode(motorStopMode_t stopMode) { devicePrm.stopMode = stopMode; } /******************************************************//** * @brief Set the torque * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t * @param[in] torqueValue in % (from 0 to 100) * @retval None * @note **********************************************************/ void STSpin220::Stspin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue) { devicePrm.updateTorque = true; if (torqueValue>100) torqueValue = 100; switch(torqueMode) { case ACC_TORQUE: devicePrm.accelTorque = torqueValue; break; case DEC_TORQUE: devicePrm.decelTorque = torqueValue; break; case RUN_TORQUE: devicePrm.runTorque = torqueValue; break; case HOLD_TORQUE: devicePrm.holdTorque = torqueValue; if (devicePrm.motionState != INACTIVE) { break; } case CURRENT_TORQUE: devicePrm.currentTorque = torqueValue; Stspin220_Board_PwmRefSetDutyCycle(torqueValue); default: devicePrm.updateTorque = false; break; //ignore error } } /******************************************************//** * @brief Enable or disable the torque boost feature * @param[in] enable true to enable torque boost, false to disable * @retval None **********************************************************/ void STSpin220::Stspin220_SetTorqueBoostEnable(bool enable) { devicePrm.torqueBoostEnable = enable; } /******************************************************//** * @brief Set the torque boost threshold * @param[in] speedThreshold speed threshold above which the step mode is * changed to full step * @retval None **********************************************************/ void STSpin220::Stspin220_SetTorqueBoostThreshold(uint16_t speedThreshold) { devicePrm.torqueBoostSpeedThreshold = speedThreshold; } /******************************************************//** * @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 if the device is in INACTIVE, * STANDBYTOINACTIVE or STANDBY state. **********************************************************/ bool STSpin220::Stspin220_SoftStop(void) { bool cmdExecuted = false; if ((devicePrm.motionState & INACTIVE) != INACTIVE) { devicePrm.commandExecuted=(deviceCommand_t) (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted); cmdExecuted = true; } return (cmdExecuted); } /******************************************************//** * @brief Get the frequency of REF PWM * @retval the frequency of REF PWM in Hz * @note **********************************************************/ uint32_t STSpin220::Stspin220_VrefPwmGetFreq(void) { return devicePrm.refPwmFreq; } /******************************************************//** * @brief Set the frequency of REF PWM * @param[in] newFreq in Hz * @retval None * @note **********************************************************/ void STSpin220::Stspin220_VrefPwmSetFreq(uint32_t newFreq) { devicePrm.refPwmFreq = newFreq; Stspin220_Board_PwmRefSetFreq(newFreq); } /******************************************************//** * @brief Locks until the device state becomes Inactive * @retval None **********************************************************/ void STSpin220::Stspin220_WaitWhileActive(void) { /* Wait while motor is running */ while (((Stspin220_get_device_state()&INACTIVE)!=INACTIVE)||\ (((Stspin220_get_device_state()&INACTIVE)==INACTIVE)&&(toggleOdd!=0))); } /* ------------------------------------------------------------------------- */ /* Internal functions ------------------------------------------------------ */ /* ------------------------------------------------------------------------- */ /******************************************************//** * @brief Updates the current speed of the device * @param[in] newSpeed in pps * @retval None **********************************************************/ void STSpin220::Stspin220_ApplySpeed(uint16_t newSpeed) { if (devicePrm.torqueBoostEnable != false) { if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256) { Stspin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED); } if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL) { if (((newSpeed>>devicePrm.stepModeLatched)>\ devicePrm.torqueBoostSpeedThreshold)&&\ (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\ ((devicePrm.stepsToTake-devicePrm.relativePos)>=\ (1<<devicePrm.stepModeLatched)))) { if ((devicePrm.sequencerPosition & 0xFF) == 0X80) { Stspin220_Board_SetFullStep(); devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL; devicePrm.accu >>= devicePrm.stepModeLatched; newSpeed >>= devicePrm.stepModeLatched; } } } else if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\ (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\ (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) == MOVE_CMD)&&\ ((devicePrm.stepsToTake-devicePrm.relativePos)<=\ (1<<devicePrm.stepModeLatched)))) { Stspin220_Board_UnsetFullStep(); devicePrm.stepMode = devicePrm.stepModeLatched; devicePrm.accu <<= devicePrm.stepModeLatched; newSpeed <<= devicePrm.stepModeLatched; } } else if (devicePrm.stepMode != devicePrm.stepModeLatched) { //torqueBoostEnable has just been disabled Stspin220_Board_UnsetFullStep(); devicePrm.stepMode = devicePrm.stepModeLatched; devicePrm.accu <<= devicePrm.stepModeLatched; newSpeed <<= devicePrm.stepModeLatched; } if (newSpeed < STSPIN220_MIN_STCK_FREQ) { newSpeed = STSPIN220_MIN_STCK_FREQ; } if (newSpeed > STSPIN220_MAX_STCK_FREQ) { newSpeed = STSPIN220_MAX_STCK_FREQ; } devicePrm.speed = newSpeed; Stspin220_Board_TimStckSetFreq(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 STSpin220::Stspin220_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 Set the parameters of the device whose values are not defined in * stspin220_target_config.h * @retval None **********************************************************/ void STSpin220::Stspin220_SetDeviceParamsOtherValues(void) { uint16_t tmp; devicePrm.accu = 0; devicePrm.currentPosition = 0; devicePrm.sequencerPosition = 0; devicePrm.endAccPos = 0; devicePrm.relativePos = 0; devicePrm.startDecPos = 0; devicePrm.stepsToTake = 0; devicePrm.updateTorque = false; devicePrm.speed = 0; devicePrm.commandExecuted = NO_CMD; devicePrm.direction = FORWARD; tmp = devicePrm.minSpeed; if (((devicePrm.torqueBoostEnable != false)&&\ (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\ (tmp>devicePrm.maxSpeed)) { Stspin220_ErrorHandler(STSPIN220_ERROR_INIT); } } /******************************************************//** * @brief Set the parameters of the device to values of the structure pointed * by pInitDevicePrm. Set GPIO according to these values. * @param pInitDevicePrm pointer onto the structure containing values to * initialize the device parameters. * @retval None **********************************************************/ void STSpin220::Stspin220_SetDeviceParamsToGivenValues(Stspin220_init_t* pInitDevicePrm) { devicePrm.motionState = STANDBY; if (Stspin220_SetAcceleration(pInitDevicePrm->acceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); if (Stspin220_SetDeceleration(pInitDevicePrm->deceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); if (Stspin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); if (Stspin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); Stspin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq); Stspin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque); Stspin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque); Stspin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque); Stspin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque); devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable; devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold; Stspin220_SetStopMode(pInitDevicePrm->stopMode); Stspin220_SetDeviceParamsOtherValues(); /* Set predefined step mode */ /* Standby-reset deactivation included to latch the MODEX inputs */ Stspin220_SetStepMode(pInitDevicePrm->stepMode); } /******************************************************//** * @brief Sets the parameters of the device to predefined values * from stspin220_target_config.h * @retval None **********************************************************/ void STSpin220::Stspin220_SetDeviceParamsToPredefinedValues(void) { devicePrm.motionState = STANDBY; if (Stspin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); if (Stspin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); if (Stspin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); if (Stspin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); Stspin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY); Stspin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE); Stspin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE); Stspin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE); Stspin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE); devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN; devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH; Stspin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP); Stspin220_SetDeviceParamsOtherValues(); /* Set predefined step mode */ /* Standby-reset deactivation included to latch the MODEX inputs */ Stspin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE); } /******************************************************//** * @brief Set the stepping mode without reset * @param[in] stepMode from full step to 1/256 microstep * as specified in enum motorStepMode_t * @retval true if the command is successfully executed, else false **********************************************************/ bool STSpin220::Stspin220_SetStepModeWithoutReset(motorStepMode_t stepMode) { /* Store step mode */ devicePrm.stepMode = stepMode; devicePrm.stepModeLatched = stepMode; /* Set the mode pins to the levels corresponding to the selected step mode */ switch (stepMode) { case STEP_MODE_FULL: Stspin220_Board_SetFullStep(); break; case STEP_MODE_HALF: Stspin220_Board_SetModePins(1, 0, 1, 0); break; case STEP_MODE_1_4: Stspin220_Board_SetModePins(0, 1, 0, 1); break; case STEP_MODE_1_8: Stspin220_Board_SetModePins(1, 1, 1, 0); break; case STEP_MODE_1_16: Stspin220_Board_SetModePins(1, 1, 1, 1); break; case STEP_MODE_1_32: Stspin220_Board_SetModePins(0, 0, 0, 1); break; case STEP_MODE_1_64: Stspin220_Board_SetModePins(1, 1, 0, 1); break; case STEP_MODE_1_128: Stspin220_Board_SetModePins(0, 0, 1, 0); break; case STEP_MODE_1_256: Stspin220_Board_SetModePins(1, 1, 0, 0); break; default: return false; } /* Wait */ Stspin220_Board_Delay(SELECT_STEP_MODE_DELAY); /* Exit standby, selected step mode is latched */ Stspin220_Board_ReleaseReset(); /* Let a delay after reset release and step mode latching*/ Stspin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */ /* after the device quit the standby condition */ if (stepMode!=(motorStepMode_t)STEP_MODE_FULL) { Stspin220_Board_UnsetFullStep(); } return true; } /******************************************************//** * @brief Initialises the bridge parameters to start the movement * and enable the power bridge * @retval None **********************************************************/ void STSpin220::Stspin220_StartMovement(void) { /* Enable STSpin220 powerstage */ Stspin220_Enable(); toggleOdd = 0; devicePrm.accu = 0; devicePrm.relativePos = 0; Stspin220_Board_TimStckInit(true); if ((devicePrm.endAccPos == 0)&&\ (devicePrm.commandExecuted != RUN_CMD)) { devicePrm.motionState = DECELERATING; Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE); } else { devicePrm.motionState = ACCELERATING; Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE); } /* Program the step clock */ Stspin220_ApplySpeed(devicePrm.minSpeed); } /******************************************************//** * @brief Handles the device state machine at each pulse * @retval None * @note Must only be called by the timer ISR **********************************************************/ void STSpin220::Stspin220_StepClockHandler(void) { uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode; uint16_t tmp; Stspin220_Board_Monitor_Set(); if (devicePrm.motionState == STANDBYTOINACTIVE) { if (toggleOdd != 0) { Stspin220_Board_StckMode3_Reset(); toggleOdd = 0; if (devicePrm.sequencerPosition == 0) { if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) { Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); } return; } } else { Stspin220_Board_StckMode3_Set(); toggleOdd = 1; tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); devicePrm.sequencerPosition -= tmp; } Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); return; } if (toggleOdd == 0) { Stspin220_Board_StckMode3_Set(); toggleOdd = 1; } else { Stspin220_Board_StckMode3_Reset(); toggleOdd = 0; /* Incrementation of the relative position */ devicePrm.relativePos += (1 << stepModeShift); /* Incrementation of the current position */ if (devicePrm.direction != BACKWARD) { devicePrm.currentPosition += (1 << stepModeShift); tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); devicePrm.sequencerPosition += tmp; if (devicePrm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1)) { devicePrm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1); } } else { devicePrm.currentPosition -= (1 << stepModeShift); tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); devicePrm.sequencerPosition -= tmp; if (devicePrm.sequencerPosition < 0) { devicePrm.sequencerPosition += (SEQUENCER_MAX_VALUE+1); } } 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)>>stepModeShift; if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos))) { devicePrm.motionState = DECELERATING; devicePrm.accu = 0; /* Apply decelerating torque */ Stspin220_ApplyTorque(DEC_TORQUE); } else if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\ ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos))) { devicePrm.motionState = STEADY; /* Apply running torque */ Stspin220_ApplyTorque(RUN_TORQUE); } 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>>stepModeShift)) { speed = devicePrm.maxSpeed>>stepModeShift; } devicePrm.speed = speed; } if (devicePrm.updateTorque!=false) { /* Apply accelerating torque */ Stspin220_ApplyTorque(ACC_TORQUE); } } break; } case STEADY: { uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift; uint32_t relativePos = devicePrm.relativePos; if (devicePrm.updateTorque!=false) { /* Apply accelerating torque */ Stspin220_ApplyTorque(RUN_TORQUE); } if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ ((devicePrm.commandExecuted==MOVE_CMD)&&\ (relativePos>=(devicePrm.startDecPos)))||\ ((devicePrm.commandExecuted==RUN_CMD)&&\ (devicePrm.speed>maxSpeed))) { devicePrm.motionState = DECELERATING; devicePrm.accu = 0; /* Apply decelerating torque */ Stspin220_ApplyTorque(DEC_TORQUE); } else if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed)) { devicePrm.motionState = ACCELERATING; devicePrm.accu = 0; /* Apply accelerating torque */ Stspin220_ApplyTorque(ACC_TORQUE); } break; } case DECELERATING: { uint32_t relativePos = devicePrm.relativePos; uint16_t speed = devicePrm.speed; uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift; if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\ (speed<=(devicePrm.minSpeed>>stepModeShift)))||\ ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake))) { /* Motion process complete */ if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0) { devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted); if (devicePrm.direction==BACKWARD) devicePrm.direction=FORWARD; else devicePrm.direction=BACKWARD; Stspin220_Board_SetDirectionGpio(devicePrm.direction); if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0) { devicePrm.motionState = ACCELERATING; devicePrm.accu = 0; /* Apply accelerating torque */ Stspin220_ApplyTorque(ACC_TORQUE); break; } } if (devicePrm.stopMode==HOLD_MODE) { Stspin220_HardStop(); } else if (devicePrm.stopMode==STANDBY_MODE) { Stspin220_PutDeviceInStandby(); } else { Stspin220_HardHiZ(); } } else if ((devicePrm.commandExecuted==RUN_CMD)&& (speed<=(devicePrm.maxSpeed>>stepModeShift))) { devicePrm.motionState = STEADY; /* Apply running torque */ Stspin220_ApplyTorque(RUN_TORQUE); } else { /* Go on decelerating */ if (speed>(devicePrm.minSpeed>>stepModeShift)) { bool speedUpdated = false; if (speed==0) speed =1; devicePrm.accu += dec / speed; while (devicePrm.accu>=(0X10000L)) { devicePrm.accu -= (0X10000L); if (speed>1) { speed -=1; } speedUpdated = true; } if (speedUpdated) { if (speed<(devicePrm.minSpeed>>stepModeShift)) { speed = devicePrm.minSpeed>>stepModeShift; } devicePrm.speed = speed; } if (devicePrm.updateTorque!=false) { /* Apply decelerating torque */ Stspin220_ApplyTorque(DEC_TORQUE); } } } break; } default: { break; } } } if ((devicePrm.motionState & INACTIVE) != INACTIVE) { Stspin220_ApplySpeed(devicePrm.speed); } else { if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) { Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); } } Stspin220_Board_Monitor_Reset(); } /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/