Davide Aliprandi
/
X_NUCLEO_IHM14A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
STSPIN820.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file STSPIN820.cpp 00004 * @author STM 00005 * @version V1.0.0 00006 * @date August 7th, 2017 00007 * @brief STSPIN820 driver (fully integrated microstepping motor driver) 00008 * @note (C) COPYRIGHT 2017 STMicroelectronics 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2017 STMicroelectronics</center></h2> 00013 * 00014 * Redistribution and use in source and binary forms, with or without modification, 00015 * are permitted provided that the following conditions are met: 00016 * 1. Redistributions of source code must retain the above copyright notice, 00017 * this list of conditions and the following disclaimer. 00018 * 2. Redistributions in binary form must reproduce the above copyright notice, 00019 * this list of conditions and the following disclaimer in the documentation 00020 * and/or other materials provided with the distribution. 00021 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00022 * may be used to endorse or promote products derived from this software 00023 * without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00026 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00027 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00028 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00029 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00030 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00031 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00033 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00034 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 ****************************************************************************** 00037 */ 00038 00039 00040 /* Generated with STM32CubeTOO -----------------------------------------------*/ 00041 00042 00043 /* Revision ------------------------------------------------------------------*/ 00044 /* 00045 Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev 00046 Branch/Trunk/Tag: trunk 00047 Based on: X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.c 00048 Revision: 0 00049 */ 00050 00051 00052 /* Includes ------------------------------------------------------------------*/ 00053 00054 #include "STSPIN820.h" 00055 00056 /* Private function prototypes -----------------------------------------------*/ 00057 00058 /** @defgroup STSPIN820_Private_Functions STSPIN820 Private Functions 00059 * @{ 00060 */ 00061 /* Methods -------------------------------------------------------------------*/ 00062 00063 /** @defgroup STSPIN820_Exported_Variables STSPIN820 Exported Variables 00064 * @{ 00065 */ 00066 00067 /** @defgroup STSPIN820_Library_Functions STSPIN820 Library Functions 00068 * @{ 00069 */ 00070 00071 /******************************************************//** 00072 * @brief Return motor handle (pointer to the STSPIN820 motor driver structure) 00073 * @retval Pointer to the MOTOR_vt_t structure 00074 **********************************************************/ 00075 MOTOR_vt_t* STSPIN820::STSPIN820_GetMotorHandle(void) 00076 { 00077 return (&STSPIN820_drv); 00078 } 00079 00080 /******************************************************//** 00081 * @brief Start the STSPIN820 library 00082 * @param[in] pInit pointer to the initialization data 00083 * @retval None 00084 **********************************************************/ 00085 void STSPIN820::STSPIN820_Init(void* pInit) 00086 { 00087 device_instance++; 00088 00089 /* Initialise the GPIOs */ 00090 STSPIN820_Board_GpioInit(); 00091 00092 /* Initialise the timer used for the step clock and ------------------------*/ 00093 /* the PWM for the reference voltage generation ----------------------------*/ 00094 STSPIN820_Board_TimStckInit(); 00095 STSPIN820_Board_PwmRefInit(); 00096 00097 if (pInit == 0) 00098 { 00099 /* Set all context variables to the predefined values */ 00100 /* from STSPIN820_config.h */ 00101 STSPIN820_SetDeviceParamsToPredefinedValues(); 00102 } 00103 else 00104 { 00105 STSPIN820_SetDeviceParamsToGivenValues((STSPIN820_init_t*) pInit); 00106 } 00107 } 00108 00109 /******************************************************//** 00110 * @brief Read id 00111 * @retval Id of the STSPIN820 Driver Instance 00112 **********************************************************/ 00113 uint16_t STSPIN820::STSPIN820_ReadId(void) 00114 { 00115 return (device_instance); 00116 } 00117 00118 /******************************************************//** 00119 * @brief Attach a user callback to the error Handler. 00120 * The call back will be then called each time the library 00121 * detects an error 00122 * @param[in] callback Name of the callback to attach 00123 * to the error Hanlder 00124 * @retval None 00125 **********************************************************/ 00126 void STSPIN820::STSPIN820_AttachErrorHandler(void (*callback)(uint16_t error)) 00127 { 00128 error_handler_callback = (void (*)(uint16_t error))callback; 00129 } 00130 00131 /******************************************************//** 00132 * @brief Attach a user callback to the flag Interrupt 00133 * The call back will be then called each time the status 00134 * flag pin will be pulled down due to the occurrence of 00135 * a programmed alarms ( OCD, thermal pre-warning or 00136 * shutdown, UVLO, wrong command, non-performable command) 00137 * @param[in] callback Name of the callback to attach 00138 * to the Flag Interrupt 00139 * @retval None 00140 **********************************************************/ 00141 void STSPIN820::STSPIN820_AttachFlagInterrupt(void (*callback)(void)) 00142 { 00143 flag_interrupt_callback = (void (*)(void))callback; 00144 } 00145 00146 /******************************************************//** 00147 * @brief Check if STSPIN820 has a fault by reading EN pin position. 00148 * @retval One if STSPIN820 has EN pin down, otherwise zero 00149 **********************************************************/ 00150 uint8_t STSPIN820::STSPIN820_CheckStatusHw(void) 00151 { 00152 if(!STSPIN820_Board_EN_AND_FAULT_PIN_GetState()) 00153 { 00154 return 0x01; 00155 } 00156 else 00157 { 00158 return 0x00; 00159 } 00160 } 00161 00162 /******************************************************//** 00163 * @brief Disable the power bridges (leave the output bridges HiZ) 00164 * @param[in] deviceId Unused parameter 00165 * @retval None 00166 **********************************************************/ 00167 void STSPIN820::STSPIN820_Disable(uint8_t deviceId) 00168 { 00169 STSPIN820_Board_Disable(); 00170 } 00171 00172 /******************************************************//** 00173 * @brief Enable the power bridges 00174 * @param[in] deviceId Unused parameter 00175 * @retval None 00176 **********************************************************/ 00177 void STSPIN820::STSPIN820_Enable(uint8_t deviceId) 00178 { 00179 STSPIN820_Board_Enable(); 00180 } 00181 00182 /******************************************************//** 00183 * @brief Error handler which calls the user callback (if defined) 00184 * @param[in] error Number of the error 00185 * @retval None 00186 **********************************************************/ 00187 void STSPIN820::STSPIN820_ErrorHandler(uint16_t error) 00188 { 00189 if (error_handler_callback != 0) 00190 { 00191 (void) error_handler_callback(error); 00192 } 00193 else 00194 { 00195 while(1) 00196 { 00197 /* Infinite loop */ 00198 } 00199 } 00200 } 00201 00202 /******************************************************//** 00203 * @brief Exit STSPIN820 device from standby (low power consumption) 00204 * @param[in] deviceId Unused parameter 00205 * @retval None 00206 **********************************************************/ 00207 void STSPIN820::STSPIN820_ExitDeviceFromStandby(uint8_t deviceId) 00208 { 00209 uint32_t sequencerPosition = device_prm.sequencerPosition; 00210 00211 /* Exit standby and set step mode */ 00212 /* Disable step clock */ 00213 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0) 00214 { 00215 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK); 00216 } 00217 00218 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */ 00219 /* after the last step clock rising edge triggering the last step */ 00220 STSPIN820_Board_Delay(DISABLE_DELAY); 00221 /* Set reference voltage to 0 */ 00222 STSPIN820_SetTorque(0, CURRENT_TORQUE, 0); 00223 00224 /* Disable power bridges */ 00225 STSPIN820_Board_Disable(); 00226 00227 device_prm.commandExecuted = NO_CMD; 00228 device_prm.stepsToTake = 0; 00229 device_prm.speed = 0; 00230 00231 /* Reset the microstepping sequencer position */ 00232 device_prm.sequencerPosition = 0; 00233 00234 /* Reset current and mark positions */ 00235 device_prm.currentPosition = 0; 00236 device_prm.markPosition = 0; 00237 00238 STSPIN820_SetStepMode(deviceId, device_prm.stepMode); 00239 STSPIN820_Board_ReleaseReset(); 00240 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); 00241 00242 STSPIN820_SetHome(deviceId); 00243 STSPIN820_SetMark(deviceId); 00244 00245 if (device_prm.sequencerPosition != 0) 00246 { 00247 /* Set direction to FORWARD to ensure the HW sequencer is increased at */ 00248 /* each step clock rising edge */ 00249 STSPIN820_SetDirection(0, FORWARD); 00250 /* Going out of standby */ 00251 device_prm.motionState = STANDBYTOINACTIVE; 00252 /* Initialize the step clock timer */ 00253 STSPIN820_Board_TimStckInit(); 00254 /* Program the step clock */ 00255 STSPIN820_Board_TimStckCompareInit(); 00256 STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ); 00257 toggle_odd = 0; 00258 STSPIN820_Board_TimStckStart(); 00259 while (device_prm.sequencerPosition != 0); 00260 while (toggle_odd!=0); 00261 device_prm.sequencerPosition = sequencerPosition; 00262 } 00263 00264 device_prm.motionState = INACTIVE; 00265 } 00266 00267 /******************************************************//** 00268 * @brief Return the acceleration of the specified device 00269 * @param[in] deviceId Unused parameter 00270 * @retval Acceleration in pps^2 00271 **********************************************************/ 00272 uint16_t STSPIN820::STSPIN820_GetAcceleration(int8_t deviceId) 00273 { 00274 return (device_prm.acceleration); 00275 } 00276 00277 /******************************************************//** 00278 * @brief Return the current speed of the specified device 00279 * @param[in] deviceId Unused parameter 00280 * @retval Speed in pps 00281 **********************************************************/ 00282 uint16_t STSPIN820::STSPIN820_GetCurrentSpeed(int8_t deviceId) 00283 { 00284 return device_prm.speed; 00285 } 00286 00287 /******************************************************//** 00288 * @brief Return the decay mode of the specified device 00289 * @param[in] deviceId Unused parameter 00290 * @retval Decay Mode State (SLOW or MIXED) 00291 **********************************************************/ 00292 motor_decay_mode_t STSPIN820::STSPIN820_GetDecayMode(uint8_t deviceId) 00293 { 00294 motor_decay_mode_t status; 00295 if (STSPIN820_Board_GetDecayGpio() != 0) 00296 { 00297 status = SLOW_DECAY; 00298 } 00299 else 00300 { 00301 status = MIXED_DECAY; 00302 } 00303 00304 return status; 00305 } 00306 00307 /******************************************************//** 00308 * @brief Return the deceleration of the specified device 00309 * @param[in] deviceId Unused parameter 00310 * @retval Deceleration in pps^2 00311 **********************************************************/ 00312 uint16_t STSPIN820::STSPIN820_GetDeceleration(uint8_t deviceId) 00313 { 00314 return (device_prm.deceleration); 00315 } 00316 00317 /******************************************************//** 00318 * @brief Return the device state 00319 * @param[in] deviceId Unused parameter 00320 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) 00321 **********************************************************/ 00322 motor_state_t STSPIN820::STSPIN820_GetDeviceState(uint8_t deviceId) 00323 { 00324 return device_prm.motionState; 00325 } 00326 00327 /******************************************************//** 00328 * @brief Get the motor current direction 00329 * @param[in] deviceId Unused parameter 00330 * @retval direction 00331 **********************************************************/ 00332 motor_direction_t STSPIN820::STSPIN820_GetDirection(uint8_t deviceId) 00333 { 00334 return device_prm.direction; 00335 } 00336 00337 /******************************************************//** 00338 * @brief Return the FW version of the library 00339 * @retval Stspin220_FW_VERSION 00340 **********************************************************/ 00341 uint32_t STSPIN820::STSPIN820_GetFwVersion(void) 00342 { 00343 return (STSPIN820_FW_VERSION); 00344 } 00345 00346 /******************************************************//** 00347 * @brief Return the mark position of the specified device 00348 * @param[in] deviceId Unused parameter 00349 * @retval mark position value 00350 **********************************************************/ 00351 int32_t STSPIN820::STSPIN820_GetMark(uint8_t deviceId) 00352 { 00353 return device_prm.markPosition; 00354 } 00355 00356 /******************************************************//** 00357 * @brief Return the max speed of the specified device 00358 * @param[in] deviceId Unused parameter 00359 * @retval maxSpeed in pps 00360 **********************************************************/ 00361 uint16_t STSPIN820::STSPIN820_GetMaxSpeed(uint8_t deviceId) 00362 { 00363 return (device_prm.maxSpeed); 00364 } 00365 00366 /******************************************************//** 00367 * @brief Return the min speed of the specified device 00368 * @param[in] deviceId Unused parameter 00369 * @retval minSpeed in pps 00370 **********************************************************/ 00371 uint16_t STSPIN820::STSPIN820_GetMinSpeed(uint8_t deviceId) 00372 { 00373 return (device_prm.minSpeed); 00374 } 00375 00376 /******************************************************//** 00377 * @brief Returns the number of devices 00378 * @retval number of devices 00379 **********************************************************/ 00380 uint8_t STSPIN820::STSPIN820_GetNbDevices(void) 00381 { 00382 return (number_of_devices); 00383 } 00384 00385 /******************************************************//** 00386 * @brief Return the current position value of the specified device 00387 * @param[in] deviceId Unused parameter 00388 * @retval current position value 00389 **********************************************************/ 00390 int32_t STSPIN820::STSPIN820_GetPosition(uint8_t deviceId) 00391 { 00392 return device_prm.currentPosition; 00393 } 00394 00395 /******************************************************//** 00396 * @brief Get the motor step mode 00397 * @param[in] deviceId Unused parameter 00398 * @retval step mode 00399 **********************************************************/ 00400 motor_step_mode_t STSPIN820::STSPIN820_GetStepMode(uint8_t deviceId) 00401 { 00402 return device_prm.stepMode; 00403 } 00404 00405 /******************************************************//** 00406 * @brief Get the selected stop mode 00407 * @param[in] deviceId Unused parameter 00408 * @retval the selected stop mode 00409 **********************************************************/ 00410 motor_stop_mode_t STSPIN820::STSPIN820_GetStopMode(uint8_t deviceId) 00411 { 00412 return device_prm.stopMode; 00413 } 00414 00415 /******************************************************//** 00416 * @brief Get the torque of the specified device 00417 * @param[in] deviceId Unused parameter 00418 * @param[in] torqueMode torque mode 00419 * @retval the torqueValue in % (from 0 to 100) 00420 * @note 00421 **********************************************************/ 00422 uint8_t STSPIN820::STSPIN820_GetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode) 00423 { 00424 uint8_t torqueValue = 0; 00425 switch(torqueMode) 00426 { 00427 case ACC_TORQUE: 00428 torqueValue = device_prm.accelTorque; 00429 break; 00430 case DEC_TORQUE: 00431 torqueValue = device_prm.decelTorque; 00432 break; 00433 case RUN_TORQUE: 00434 torqueValue = device_prm.runTorque; 00435 break; 00436 case HOLD_TORQUE: 00437 torqueValue = device_prm.holdTorque; 00438 break; 00439 case CURRENT_TORQUE: 00440 torqueValue = device_prm.currentTorque; 00441 break; 00442 default: 00443 break; 00444 } 00445 return torqueValue; 00446 } 00447 00448 /******************************************************//** 00449 * @brief Get the torque boost feature status 00450 * @param[in] deviceId Unused parameter 00451 * @retval TRUE if enabled, FALSE if disabled 00452 **********************************************************/ 00453 bool STSPIN820::STSPIN820_GetTorqueBoostEnable(uint8_t deviceId) 00454 { 00455 return device_prm.torqueBoostEnable; 00456 } 00457 00458 /******************************************************//** 00459 * @brief Get the torque boost threshold 00460 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1) 00461 * @retval the torque boost threshold above which the step mode is 00462 * changed to full step 00463 **********************************************************/ 00464 uint16_t STSPIN820::STSPIN820_GetTorqueBoostThreshold(uint8_t deviceId) 00465 { 00466 return device_prm.torqueBoostSpeedThreshold; 00467 } 00468 00469 /******************************************************//** 00470 * @brief Request the motor to move to the home position (ABS_POSITION = 0) 00471 * @param[in] deviceId Unused parameter 00472 * @retval None 00473 **********************************************************/ 00474 void STSPIN820::STSPIN820_GoHome(uint8_t deviceId) 00475 { 00476 deviceId = 0; 00477 00478 STSPIN820_GoTo(deviceId, 0); 00479 } 00480 00481 /******************************************************//** 00482 * @brief Request the motor to move to the mark position 00483 * @param[in] deviceId Unused parameter 00484 * @retval None 00485 **********************************************************/ 00486 void STSPIN820::STSPIN820_GoMark(uint8_t deviceId) 00487 { 00488 deviceId = 0; 00489 00490 STSPIN820_GoTo(deviceId, device_prm.markPosition); 00491 } 00492 00493 /******************************************************//** 00494 * @brief Request the motor to move to the specified position 00495 * @param[in] deviceId Unused parameter 00496 * @param[in] targetPosition absolute position in steps 00497 * @retval None 00498 * @note The position is at the resolution corresponding to the 00499 * selected step mode. 00500 * STEP_MODE_FULL : Full step 00501 * STEP_MODE_HALF : 1/2 step 00502 * STEP_MODE_1_4 : 1/4 step 00503 * STEP_MODE_1_8 : 1/8 step 00504 * STEP_MODE_1_16 : 1/16 step 00505 * STEP_MODE_1_32 : 1/32 step 00506 * STEP_MODE_1_128 : 1/128 step 00507 * STEP_MODE_1_256 : 1/256 step 00508 * @note The 1/64 step mode is not allowed 00509 **********************************************************/ 00510 void STSPIN820::STSPIN820_GoTo(uint8_t deviceId, int32_t targetPosition) 00511 { 00512 motor_direction_t direction; 00513 deviceId = 0; 00514 00515 /* Exit from standby if needed */ 00516 if (device_prm.motionState == STANDBY) 00517 { 00518 STSPIN820_ExitDeviceFromStandby(deviceId); 00519 } 00520 /* Deactivate motor if needed */ 00521 else if (device_prm.motionState != INACTIVE) 00522 { 00523 STSPIN820_HardHiZ(deviceId); 00524 } 00525 00526 if (targetPosition > device_prm.currentPosition) 00527 { 00528 device_prm.stepsToTake = targetPosition -\ 00529 device_prm.currentPosition; 00530 if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1)) 00531 { 00532 direction = FORWARD; 00533 } 00534 else 00535 { 00536 direction = BACKWARD; 00537 device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\ 00538 device_prm.stepsToTake; 00539 } 00540 } 00541 else 00542 { 00543 device_prm.stepsToTake = device_prm.currentPosition -\ 00544 targetPosition; 00545 if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1)) 00546 { 00547 direction = BACKWARD; 00548 } 00549 else 00550 { 00551 direction = FORWARD; 00552 device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\ 00553 device_prm.stepsToTake; 00554 } 00555 } 00556 00557 if (device_prm.stepsToTake != 0) 00558 { 00559 device_prm.commandExecuted = MOVE_CMD; 00560 00561 /* Direction setup */ 00562 STSPIN820_SetDirection(deviceId, direction); 00563 00564 STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake); 00565 00566 /* Motor activation */ 00567 STSPIN820_StartMovement(deviceId); 00568 } 00569 } 00570 00571 /******************************************************//** 00572 * @brief Move the motor to the absolute position 00573 * @param[in] deviceId Unused parameter 00574 * @param[in] direction FORWARD or BACKWARD 00575 * @param[in] targetPosition 32 bit signed value position 00576 * @retval None 00577 * @note The position is at the resolution corresponding to the 00578 * selected step mode. 00579 * STEP_MODE_FULL : step 00580 * STEP_MODE_HALF : 1/2 step 00581 * STEP_MODE_1_4 : 1/4 step 00582 * STEP_MODE_1_8 : 1/8 step 00583 * STEP_MODE_1_16 : 1/16 step 00584 * STEP_MODE_1_32 : 1/32 step 00585 * STEP_MODE_1_128 : 1/128 step 00586 * STEP_MODE_1_256 : 1/256 step 00587 * @note The 1/64 step mode is not allowed 00588 **********************************************************/ 00589 void STSPIN820::STSPIN820_GoToDir(uint8_t deviceId, motor_direction_t direction, int32_t targetPosition) 00590 { 00591 deviceId = 0; 00592 00593 /* Exit from standby if needed */ 00594 if (device_prm.motionState == STANDBY) 00595 { 00596 STSPIN820_ExitDeviceFromStandby(deviceId); 00597 } 00598 /* Deactivate motor if needed */ 00599 else if (device_prm.motionState != INACTIVE) 00600 { 00601 STSPIN820_HardHiZ(deviceId); 00602 } 00603 00604 if (direction != BACKWARD) 00605 { 00606 if (targetPosition > device_prm.currentPosition) 00607 { 00608 device_prm.stepsToTake = targetPosition -\ 00609 device_prm.currentPosition; 00610 } 00611 else 00612 { 00613 device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\ 00614 (targetPosition -\ 00615 device_prm.currentPosition); 00616 } 00617 } 00618 else 00619 { 00620 if (targetPosition > device_prm.currentPosition) 00621 { 00622 device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\ 00623 (device_prm.currentPosition -\ 00624 targetPosition); 00625 } 00626 else 00627 { 00628 device_prm.stepsToTake = device_prm.currentPosition -\ 00629 targetPosition; 00630 } 00631 } 00632 00633 if (device_prm.stepsToTake != 0) 00634 { 00635 device_prm.commandExecuted = MOVE_CMD; 00636 00637 /* Direction setup */ 00638 STSPIN820_SetDirection(deviceId, direction); 00639 00640 STSPIN820_ComputeSpeedProfile(deviceId, device_prm.stepsToTake); 00641 00642 /* Motor activation */ 00643 STSPIN820_StartMovement(deviceId); 00644 } 00645 } 00646 00647 /******************************************************//** 00648 * @brief Immediatly stop the motor and disable the power bridge 00649 * @param[in] deviceId Unused parameter 00650 * @retval None 00651 **********************************************************/ 00652 void STSPIN820::STSPIN820_HardHiZ(uint8_t deviceId) 00653 { 00654 /* Set inactive state */ 00655 device_prm.motionState = INACTIVE; 00656 00657 /* Disable step clock */ 00658 if (STSPIN820_Board_TimStckStop(handle &toggle_odd) == 0) 00659 { 00660 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK); 00661 } 00662 00663 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */ 00664 /* after the last step clock rising edge triggering the last step */ 00665 STSPIN820_Board_Delay(DISABLE_DELAY); 00666 00667 /* Set reference voltage to 0 */ 00668 STSPIN820_SetTorque(0, CURRENT_TORQUE, 0); 00669 00670 /* Disable power bridges */ 00671 STSPIN820_Board_Disable(); 00672 00673 /* Comeback to nominal step mode */ 00674 if (device_prm.stepModeLatched != device_prm.stepMode) 00675 { 00676 motor_step_mode_t StepMode = device_prm.stepModeLatched; 00677 STSPIN820_SetStepMode(0, StepMode); 00678 device_prm.stepMode = device_prm.stepModeLatched; 00679 } 00680 00681 device_prm.commandExecuted = NO_CMD; 00682 device_prm.stepsToTake = 0; 00683 device_prm.speed = 0; 00684 } 00685 00686 /******************************************************//** 00687 * @brief Immediatly stop the motor 00688 * and either set holding torque when stop mode is HOLD_MODE, 00689 * or call STSPIN820_HardHiz function when stop mode is HIZ_MODE, 00690 * or call STSPIN820_PutDeviceInStandby function when stop mode is STANDBY_MODE 00691 * @param[in] deviceId Unused parameter 00692 * @retval None 00693 **********************************************************/ 00694 void STSPIN820::STSPIN820_HardStop(uint8_t deviceId) 00695 { 00696 deviceId = 0; 00697 00698 if (device_prm.stopMode == HOLD_MODE) 00699 { 00700 /* Set inactive state */ 00701 device_prm.motionState = INACTIVE; 00702 00703 /* Disable step clock */ 00704 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0) 00705 { 00706 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK); 00707 } 00708 00709 /* Set holding torque */ 00710 STSPIN820_ApplyTorque(deviceId, HOLD_TORQUE); 00711 00712 /* Comeback to nominal step mode */ 00713 if (device_prm.stepModeLatched != device_prm.stepMode) 00714 { 00715 motor_step_mode_t StepMode = device_prm.stepModeLatched; 00716 STSPIN820_SetStepMode(0, StepMode); 00717 device_prm.stepMode = device_prm.stepModeLatched; 00718 } 00719 00720 device_prm.commandExecuted = NO_CMD; 00721 device_prm.stepsToTake = 0; 00722 device_prm.speed = 0; 00723 } 00724 else if (device_prm.stopMode == HIZ_MODE) 00725 { 00726 STSPIN820_HardHiZ(deviceId); 00727 } 00728 else if (device_prm.stopMode == STANDBY_MODE) 00729 { 00730 STSPIN820_PutDeviceInStandby(deviceId); 00731 } 00732 } 00733 00734 /******************************************************//** 00735 * @brief Moves the motor of the specified number of steps 00736 * @param[in] deviceId Unused parameter 00737 * @param[in] direction FORWARD or BACKWARD 00738 * @param[in] stepCount Number of steps to perform 00739 * @retval None 00740 **********************************************************/ 00741 void STSPIN820::STSPIN820_Move(uint8_t deviceId, motor_direction_t direction, uint32_t stepCount) 00742 { 00743 deviceId = 0; 00744 00745 /* Exit from standby if needed */ 00746 if (device_prm.motionState == STANDBY) 00747 { 00748 STSPIN820_ExitDeviceFromStandby(deviceId); 00749 } 00750 /* Deactivate motor if needed */ 00751 else if (device_prm.motionState != INACTIVE) 00752 { 00753 STSPIN820_HardHiZ(deviceId); 00754 } 00755 00756 if (stepCount != 0) 00757 { 00758 device_prm.stepsToTake = stepCount; 00759 device_prm.commandExecuted = MOVE_CMD; 00760 00761 /* Direction setup */ 00762 STSPIN820_SetDirection(deviceId, direction); 00763 00764 STSPIN820_ComputeSpeedProfile(deviceId, stepCount); 00765 00766 /* Motor activation */ 00767 STSPIN820_StartMovement(deviceId); 00768 } 00769 } 00770 00771 /******************************************************//** 00772 * @brief Put STSPIN820 device in standby (low power consumption) 00773 * @param[in] deviceId Unused parameter 00774 * @retval None 00775 **********************************************************/ 00776 void STSPIN820::STSPIN820_PutDeviceInStandby(uint8_t deviceId) 00777 { 00778 /* Stop movement */ 00779 STSPIN820_HardHiZ(deviceId); 00780 00781 /* Enter standby */ 00782 STSPIN820_Board_Reset(); 00783 00784 device_prm.motionState = STANDBY; 00785 } 00786 00787 /******************************************************//** 00788 * @brief Runs the motor. It will accelerate from the min 00789 * speed up to the max speed by using the device acceleration. 00790 * @param[in] deviceId Unused parameter 00791 * @param[in] direction FORWARD or BACKWARD 00792 * @retval None 00793 **********************************************************/ 00794 void STSPIN820::STSPIN820_Run(uint8_t deviceId, motor_direction_t direction) 00795 { 00796 /* Exit from standby if needed */ 00797 if (device_prm.motionState == STANDBY) 00798 { 00799 STSPIN820_ExitDeviceFromStandby(deviceId); 00800 } 00801 /* Deactivate motor if needed */ 00802 else if (device_prm.motionState != INACTIVE) 00803 { 00804 STSPIN820_HardHiZ(deviceId); 00805 } 00806 00807 /* Direction setup */ 00808 STSPIN820_SetDirection(deviceId,direction); 00809 device_prm.commandExecuted = RUN_CMD; 00810 /* Motor activation */ 00811 STSPIN820_StartMovement(deviceId); 00812 } 00813 00814 /******************************************************//** 00815 * @brief Changes the acceleration of the specified device 00816 * @param[in] deviceId Unused parameter 00817 * @param[in] newAcc New acceleration to apply in pps^2 00818 * @retval true if the command is successfully executed, else false 00819 * @note The command is not performed if the device is executing 00820 * a MOVE or GOTO command (but it can be used during a RUN command) 00821 **********************************************************/ 00822 bool STSPIN820::STSPIN820_SetAcceleration(uint8_t deviceId, uint16_t newAcc) 00823 { 00824 bool cmdExecuted = FALSE; 00825 if ((newAcc != 0)&& 00826 (((device_prm.motionState & INACTIVE) == INACTIVE)|| 00827 (device_prm.commandExecuted == RUN_CMD))) 00828 { 00829 device_prm.acceleration = newAcc; 00830 cmdExecuted = TRUE; 00831 } 00832 return cmdExecuted; 00833 } 00834 00835 /******************************************************//** 00836 * @brief Specifies the decay mode 00837 * @param[in] deviceId Unused parameter 00838 * @param[in] decay SLOW_DECAY or MIXED_DECAY 00839 * @retval true if the command is successfully executed, else false 00840 **********************************************************/ 00841 void STSPIN820::STSPIN820_SetDecayMode(uint8_t deviceId, motor_decay_mode_t decay) 00842 { 00843 if (decay == SLOW_DECAY) 00844 { 00845 STSPIN820_Board_SetDecayGpio(1); 00846 } 00847 else if (decay == MIXED_DECAY) 00848 { 00849 STSPIN820_Board_SetDecayGpio(0); 00850 } 00851 } 00852 00853 /******************************************************//** 00854 * @brief Changes the deceleration of the specified device 00855 * @param[in] deviceId Unused parameter 00856 * @param[in] newDec New deceleration to apply in pps^2 00857 * @retval true if the command is successfully executed, else false 00858 * @note The command is not performed if the device is executing 00859 * a MOVE or GOTO command (but it can be used during a RUN command) 00860 **********************************************************/ 00861 bool STSPIN820::STSPIN820_SetDeceleration(uint8_t deviceId, uint16_t newDec) 00862 { 00863 bool cmdExecuted = FALSE; 00864 if ((newDec != 0)&& 00865 (((device_prm.motionState & INACTIVE) == INACTIVE)|| 00866 (device_prm.commandExecuted == RUN_CMD))) 00867 { 00868 device_prm.deceleration = newDec; 00869 cmdExecuted = TRUE; 00870 } 00871 return cmdExecuted; 00872 } 00873 00874 /******************************************************//** 00875 * @brief Specifies the direction 00876 * @param[in] deviceId Unused parameter 00877 * @param[in] dir FORWARD or BACKWARD 00878 * @note The direction change is applied if the device 00879 * is in INACTIVE or STANDBY state or if the device is 00880 * executing a run command 00881 * @retval None 00882 **********************************************************/ 00883 void STSPIN820::STSPIN820_SetDirection(uint8_t deviceId, motor_direction_t dir) 00884 { 00885 if ((device_prm.motionState == INACTIVE)||\ 00886 (device_prm.motionState == STANDBY)) 00887 { 00888 device_prm.direction = dir; 00889 STSPIN820_Board_SetDirectionGpio(dir); 00890 } 00891 else if ((device_prm.commandExecuted&RUN_CMD)!=0) 00892 { 00893 device_prm.commandExecuted|=STSPIN820_DIR_CHANGE_BIT_MASK; 00894 } 00895 } 00896 00897 /******************************************************//** 00898 * @brief Set current position to be the Home position 00899 * (current position set to 0) 00900 * @param[in] deviceId Unused parameter 00901 * @retval None 00902 **********************************************************/ 00903 void STSPIN820::STSPIN820_SetHome(uint8_t deviceId) 00904 { 00905 device_prm.currentPosition = 0; 00906 } 00907 00908 /******************************************************//** 00909 * @brief Set current position to be the Mark position 00910 * @param[in] deviceId Unused parameter 00911 * @retval None 00912 **********************************************************/ 00913 void STSPIN820::STSPIN820_SetMark(uint8_t deviceId) 00914 { 00915 device_prm.markPosition = device_prm.currentPosition; 00916 } 00917 00918 /******************************************************//** 00919 * @brief Changes the max speed of the specified device 00920 * @param[in] deviceId Unused parameter 00921 * @param[in] newMaxSpeed New max speed to apply in pps 00922 * @retval true if the command is successfully executed, else false 00923 * @note The command is not performed is the device is executing 00924 * a MOVE or GOTO command (but it can be used during a RUN command). 00925 **********************************************************/ 00926 bool STSPIN820::STSPIN820_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed) 00927 { 00928 bool cmdExecuted = FALSE; 00929 if ((newMaxSpeed >= STSPIN820_MIN_STCK_FREQ)&&\ 00930 ((newMaxSpeed <= STSPIN820_MAX_STCK_FREQ)||\ 00931 ((device_prm.torqueBoostEnable != FALSE)&&\ 00932 ((newMaxSpeed>>STSPIN820_GetStepMode(deviceId))<= STSPIN820_MAX_STCK_FREQ)))&&\ 00933 (device_prm.minSpeed <= newMaxSpeed) &&\ 00934 (((device_prm.motionState & INACTIVE) == INACTIVE)||\ 00935 (device_prm.commandExecuted == RUN_CMD))) 00936 { 00937 device_prm.maxSpeed = newMaxSpeed; 00938 cmdExecuted = TRUE; 00939 } 00940 return cmdExecuted; 00941 } 00942 00943 /******************************************************//** 00944 * @brief Changes the min speed of the specified device 00945 * @param[in] deviceId Unused parameter 00946 * @param[in] newMinSpeed New min speed to apply in pps 00947 * @retval true if the command is successfully executed, else false 00948 * @note The command is not performed is the device is executing 00949 * a MOVE or GOTO command (but it can be used during a RUN command). 00950 **********************************************************/ 00951 bool STSPIN820::STSPIN820_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed) 00952 { 00953 bool cmdExecuted = FALSE; 00954 if ((newMinSpeed >= STSPIN820_MIN_STCK_FREQ)&& 00955 (newMinSpeed <= STSPIN820_MAX_STCK_FREQ) && 00956 (newMinSpeed <= device_prm.maxSpeed) && 00957 (((device_prm.motionState & INACTIVE) == INACTIVE)|| 00958 (device_prm.commandExecuted == RUN_CMD))) 00959 { 00960 device_prm.minSpeed = newMinSpeed; 00961 cmdExecuted = TRUE; 00962 } 00963 return cmdExecuted; 00964 } 00965 00966 /******************************************************//** 00967 * @brief Sets the number of devices to be used 00968 * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES) 00969 * @retval TRUE if successfull, FALSE if failure, attempt to set a number of 00970 * devices greater than MAX_NUMBER_OF_DEVICES 00971 **********************************************************/ 00972 bool STSPIN820::STSPIN820_SetNbDevices(uint8_t nbDevices) 00973 { 00974 if (nbDevices <= MAX_NUMBER_OF_DEVICES) 00975 { 00976 number_of_devices = nbDevices; 00977 return TRUE; 00978 } 00979 else 00980 { 00981 return FALSE; 00982 } 00983 } 00984 00985 /******************************************************//** 00986 * @brief Set the stepping mode 00987 * @param[in] deviceId Unused parameter 00988 * @param[in] stepMode from full step to 1/256 microstep 00989 * as specified in enum motor_step_mode_t 00990 * 1/64 microstep mode not supported by STSPIN820 00991 * @retval true if the command is successfully executed, else false 00992 **********************************************************/ 00993 bool STSPIN820::STSPIN820_SetStepMode(uint8_t deviceId, motor_step_mode_t stepMode) 00994 { 00995 /* Store step mode */ 00996 device_prm.stepMode = stepMode; 00997 device_prm.stepModeLatched = stepMode; 00998 00999 /* Set the mode pins to the levels corresponding to the selected step mode */ 01000 switch (stepMode) 01001 { 01002 case STEP_MODE_FULL: 01003 STSPIN820_Board_SetFullStep(); 01004 break; 01005 case STEP_MODE_HALF: 01006 STSPIN820_Board_SetModePins(1, 0, 0); 01007 break; 01008 case STEP_MODE_1_4: 01009 STSPIN820_Board_SetModePins(0, 1, 0); 01010 break; 01011 case STEP_MODE_1_8: 01012 STSPIN820_Board_SetModePins(1, 1, 0); 01013 break; 01014 case STEP_MODE_1_16: 01015 STSPIN820_Board_SetModePins(0, 0, 1); 01016 break; 01017 case STEP_MODE_1_32: 01018 STSPIN820_Board_SetModePins(1, 0, 1); 01019 break; 01020 case STEP_MODE_1_128: 01021 STSPIN820_Board_SetModePins(0, 1, 1); 01022 break; 01023 case STEP_MODE_1_256: 01024 STSPIN820_Board_SetModePins(1, 1, 1); 01025 break; 01026 default: 01027 return FALSE; 01028 } 01029 01030 return TRUE; 01031 01032 } 01033 01034 /******************************************************//** 01035 * @brief Select the mode to stop the motor. 01036 * @param[in] deviceId Unused parameter 01037 * @param[in] stopMode HOLD_MODE to let power bridge enabled 01038 * @retval None 01039 **********************************************************/ 01040 void STSPIN820::STSPIN820_SetStopMode(uint8_t deviceId, motor_stop_mode_t stopMode) 01041 { 01042 device_prm.stopMode = stopMode; 01043 } 01044 01045 /******************************************************//** 01046 * @brief Set the torque of the specified device 01047 * @param[in] deviceId Unused parameter 01048 * @param[in] torqueMode Torque mode as specified in enum motor_torque_mode_t 01049 * @param[in] torqueValue in % (from 0 to 100) 01050 * @retval None 01051 * @note 01052 **********************************************************/ 01053 void STSPIN820::STSPIN820_SetTorque(uint8_t deviceId, motor_torque_mode_t torqueMode, uint8_t torqueValue) 01054 { 01055 device_prm.updateTorque = TRUE; 01056 if (torqueValue>100) 01057 { 01058 torqueValue = 100; 01059 } 01060 switch(torqueMode) 01061 { 01062 case ACC_TORQUE: 01063 device_prm.accelTorque = torqueValue; 01064 break; 01065 case DEC_TORQUE: 01066 device_prm.decelTorque = torqueValue; 01067 break; 01068 case RUN_TORQUE: 01069 device_prm.runTorque = torqueValue; 01070 break; 01071 case HOLD_TORQUE: 01072 device_prm.holdTorque = torqueValue; 01073 if (device_prm.motionState != INACTIVE) 01074 { 01075 break; 01076 } 01077 case CURRENT_TORQUE: 01078 device_prm.currentTorque = torqueValue; 01079 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue); 01080 default: 01081 device_prm.updateTorque = FALSE; 01082 break; //ignore error 01083 } 01084 } 01085 01086 /******************************************************//** 01087 * @brief Enable or disable the torque boost feature 01088 * @param[in] deviceId Unused parameter 01089 * @param[in] enable true to enable torque boost, false to disable 01090 * @retval None 01091 **********************************************************/ 01092 void STSPIN820::STSPIN820_SetTorqueBoostEnable(uint8_t deviceId, bool enable) 01093 { 01094 device_prm.torqueBoostEnable = enable; 01095 } 01096 01097 /******************************************************//** 01098 * @brief Set the torque boost threshold 01099 * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES - 1) 01100 * @param[in] speedThreshold speed threshold above which the step mode is 01101 * changed to full step 01102 * @retval None 01103 **********************************************************/ 01104 void STSPIN820::STSPIN820_SetTorqueBoostThreshold(uint8_t deviceId, uint16_t speedThreshold) 01105 { 01106 device_prm.torqueBoostSpeedThreshold = speedThreshold; 01107 } 01108 01109 /******************************************************//** 01110 * @brief Stops the motor by using the device deceleration 01111 * @param[in] deviceId Unused parameter 01112 * @retval true if the command is successfully executed, else false 01113 * @note The command is not performed if the device is in INACTIVE, 01114 * STANDBYTOINACTIVE or STANDBY state. 01115 **********************************************************/ 01116 bool STSPIN820::STSPIN820_SoftStop(uint8_t deviceId) 01117 { 01118 bool cmdExecuted = FALSE; 01119 if ((device_prm.motionState & INACTIVE) != INACTIVE) 01120 { 01121 device_prm.commandExecuted |= STSPIN820_SOFT_STOP_BIT_MASK; 01122 cmdExecuted = TRUE; 01123 } 01124 return (cmdExecuted); 01125 } 01126 01127 /******************************************************//** 01128 * @brief Get the frequency of REF PWM of the specified device 01129 * @param[in] deviceId Unused parameter 01130 * @retval the frequency of REF PWM in Hz 01131 * @note 01132 **********************************************************/ 01133 uint32_t STSPIN820::STSPIN820_VrefPwmGetFreq(uint8_t deviceId) 01134 { 01135 return device_prm.refPwmFreq; 01136 } 01137 01138 /******************************************************//** 01139 * @brief Set the frequency of REF PWM of the specified device 01140 * @param[in] deviceId Unused parameter 01141 * @param[in] newFreq in Hz 01142 * @retval None 01143 * @note 01144 **********************************************************/ 01145 void STSPIN820::STSPIN820_VrefPwmSetFreq(uint8_t deviceId, uint32_t newFreq) 01146 { 01147 device_prm.refPwmFreq = newFreq; 01148 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(newFreq,device_prm.currentTorque); 01149 } 01150 01151 /******************************************************//** 01152 * @brief Locks until the device state becomes Inactive 01153 * @param[in] deviceId Unused parameter 01154 * @retval None 01155 **********************************************************/ 01156 void STSPIN820::STSPIN820_WaitWhileActive(uint8_t deviceId) 01157 { 01158 /* Wait while motor is running */ 01159 while (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)!=INACTIVE)||\ 01160 (((STSPIN820_GetDeviceState(deviceId)&INACTIVE)==INACTIVE)&&(toggle_odd!=0))); 01161 } 01162 01163 /******************************************************//** 01164 * @brief Updates the current speed of the device 01165 * @param[in] deviceId Unused parameter 01166 * @param[in] newSpeed in pps 01167 * @retval None 01168 **********************************************************/ 01169 void STSPIN820::STSPIN820_ApplySpeed(uint8_t deviceId, uint16_t newSpeed) 01170 { 01171 if (device_prm.torqueBoostEnable != FALSE) 01172 { 01173 if (device_prm.stepMode > STEP_MODE_1_256) 01174 { 01175 STSPIN820_ErrorHandler(STSPIN820_ERROR_APPLY_SPEED); 01176 } 01177 if (device_prm.stepMode != STEP_MODE_FULL) 01178 { 01179 if (((newSpeed>>device_prm.stepModeLatched)>\ 01180 device_prm.torqueBoostSpeedThreshold)&&\ 01181 (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) != MOVE_CMD) ||\ 01182 ((device_prm.stepsToTake-device_prm.relativePos)>=\ 01183 (1<<device_prm.stepModeLatched)))) 01184 { 01185 if ((device_prm.sequencerPosition & 0xFF) == 0x80) 01186 { 01187 STSPIN820_Board_SetFullStep(); 01188 device_prm.stepMode = STEP_MODE_FULL; 01189 device_prm.accu >>= device_prm.stepModeLatched; 01190 newSpeed >>= device_prm.stepModeLatched; 01191 } 01192 } 01193 } 01194 else if (((newSpeed <= device_prm.torqueBoostSpeedThreshold) &&\ 01195 (device_prm.stepModeLatched != STEP_MODE_FULL))||\ 01196 (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) == MOVE_CMD)&&\ 01197 ((device_prm.stepsToTake-device_prm.relativePos)<=\ 01198 (1<<device_prm.stepModeLatched)))) 01199 { 01200 STSPIN820_SetStepMode(0, device_prm.stepModeLatched); 01201 device_prm.stepMode = device_prm.stepModeLatched; 01202 device_prm.accu <<= device_prm.stepModeLatched; 01203 newSpeed <<= device_prm.stepModeLatched; 01204 } 01205 } 01206 else if (device_prm.stepMode != device_prm.stepModeLatched) 01207 { 01208 //torqueBoostEnable has just been disabled 01209 STSPIN820_SetStepMode(0, device_prm.stepModeLatched); 01210 device_prm.stepMode = device_prm.stepModeLatched; 01211 device_prm.accu <<= device_prm.stepModeLatched; 01212 newSpeed <<= device_prm.stepModeLatched; 01213 } 01214 01215 if (newSpeed < STSPIN820_MIN_STCK_FREQ) 01216 { 01217 newSpeed = STSPIN820_MIN_STCK_FREQ; 01218 } 01219 if (newSpeed > STSPIN820_MAX_STCK_FREQ) 01220 { 01221 newSpeed = STSPIN820_MAX_STCK_FREQ; 01222 } 01223 01224 device_prm.speed = newSpeed; 01225 STSPIN820_Board_TimStckSetFreq(newSpeed); 01226 01227 } 01228 01229 /******************************************************//** 01230 * @brief Apply the set torque to the specified device 01231 * @param[in] deviceId Unused parameter 01232 * @param[in] torqueMode torque mode 01233 * @retval None 01234 * @note 01235 **********************************************************/ 01236 void STSPIN820::STSPIN820_ApplyTorque(uint8_t deviceId, motor_torque_mode_t torqueMode) 01237 { 01238 uint8_t torqueValue = 0; 01239 device_prm.updateTorque = FALSE; 01240 switch(torqueMode) 01241 { 01242 case ACC_TORQUE: 01243 device_prm.currentTorque = device_prm.accelTorque; 01244 break; 01245 case DEC_TORQUE: 01246 device_prm.currentTorque = device_prm.decelTorque; 01247 break; 01248 case RUN_TORQUE: 01249 device_prm.currentTorque = device_prm.runTorque; 01250 break; 01251 case HOLD_TORQUE: 01252 device_prm.currentTorque = device_prm.holdTorque; 01253 break; 01254 case CURRENT_TORQUE: 01255 break; 01256 default: 01257 return; //ignore error 01258 } 01259 torqueValue = device_prm.currentTorque; 01260 STSPIN820_Board_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue); 01261 } 01262 01263 /******************************************************//** 01264 * @brief Computes the speed profile according to the number of steps to move 01265 * @param[in] deviceId Unused parameter 01266 * @param[in] nbSteps number of steps to perform 01267 * @retval None 01268 * @note Using the acceleration and deceleration of the device, 01269 * this function determines the duration in steps of the acceleration, 01270 * steady and deceleration phases. 01271 * If the total number of steps to perform is big enough, a trapezoidal move 01272 * is performed (i.e. there is a steady phase where the motor runs at the maximum 01273 * speed. 01274 * Else, a triangular move is performed (no steady phase: the maximum speed is never 01275 * reached. 01276 **********************************************************/ 01277 void STSPIN820::STSPIN820_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps) 01278 { 01279 uint32_t reqAccSteps; 01280 uint32_t reqDecSteps; 01281 01282 /* compute the number of steps to get the targeted speed */ 01283 uint16_t minSpeed = device_prm.minSpeed; 01284 reqAccSteps = (device_prm.maxSpeed - minSpeed); 01285 reqAccSteps *= (device_prm.maxSpeed + minSpeed); 01286 reqDecSteps = reqAccSteps; 01287 reqAccSteps /= (uint32_t)device_prm.acceleration; 01288 reqAccSteps /= 2; 01289 01290 /* compute the number of steps to stop */ 01291 reqDecSteps /= (uint32_t)device_prm.deceleration; 01292 reqDecSteps /= 2; 01293 01294 if(( reqAccSteps + reqDecSteps ) > nbSteps) 01295 { 01296 /* Triangular move */ 01297 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 01298 uint32_t dec = device_prm.deceleration; 01299 uint32_t acc = device_prm.acceleration; 01300 01301 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); 01302 if (reqDecSteps > 1) 01303 { 01304 reqAccSteps = reqDecSteps - 1; 01305 if(reqAccSteps == 0) 01306 { 01307 reqAccSteps = 1; 01308 } 01309 } 01310 else 01311 { 01312 reqAccSteps = 0; 01313 } 01314 device_prm.endAccPos = reqAccSteps; 01315 device_prm.startDecPos = reqDecSteps; 01316 } 01317 else 01318 { 01319 /* Trapezoidal move */ 01320 /* accelerating phase to endAccPos */ 01321 /* steady phase from endAccPos to startDecPos */ 01322 /* decelerating from startDecPos to stepsToTake*/ 01323 device_prm.endAccPos = reqAccSteps; 01324 device_prm.startDecPos = nbSteps - reqDecSteps - 1; 01325 } 01326 } 01327 01328 /******************************************************//** 01329 * @brief Handlers of the flag interrupt which calls the user callback (if defined) 01330 * @retval None 01331 **********************************************************/ 01332 void STSPIN820::STSPIN820_FlagInterruptHandler(void) 01333 { 01334 if (flag_interrupt_callback != 0) 01335 { 01336 flag_interrupt_callback(); 01337 } 01338 } 01339 01340 /******************************************************//** 01341 * @brief Set the parameters of the device whose values are not defined in 01342 * STSPIN820_config.h 01343 * @retval None 01344 **********************************************************/ 01345 void STSPIN820::STSPIN820_SetDeviceParamsOtherValues(void) 01346 { 01347 uint16_t tmp; 01348 01349 device_prm.accu = 0; 01350 device_prm.currentPosition = 0; 01351 device_prm.sequencerPosition = 0; 01352 device_prm.endAccPos = 0; 01353 device_prm.relativePos = 0; 01354 device_prm.startDecPos = 0; 01355 device_prm.stepsToTake = 0; 01356 device_prm.updateTorque = FALSE; 01357 device_prm.speed = 0; 01358 device_prm.commandExecuted = NO_CMD; 01359 device_prm.direction = FORWARD; 01360 tmp = device_prm.minSpeed; 01361 if (((device_prm.torqueBoostEnable != FALSE)&&\ 01362 (device_prm.torqueBoostSpeedThreshold>STSPIN820_MAX_STCK_FREQ))||\ 01363 (tmp>device_prm.maxSpeed)) 01364 { 01365 STSPIN820_ErrorHandler(STSPIN820_ERROR_INIT); 01366 } 01367 } 01368 01369 /******************************************************//** 01370 * @brief Set the parameters of the device to values of initDevicePrm structure 01371 * @param pInitDevicePrm structure containing values to initialize the device 01372 * parameters 01373 * @retval None 01374 **********************************************************/ 01375 void STSPIN820::STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* pInitDevicePrm) 01376 { 01377 device_prm.motionState = STANDBY;; 01378 01379 if (STSPIN820_SetAcceleration(0,pInitDevicePrm->acceleration)==FALSE) 01380 { 01381 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION); 01382 } 01383 if (STSPIN820_SetDeceleration(0,pInitDevicePrm->deceleration)==FALSE) 01384 { 01385 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION); 01386 } 01387 if (STSPIN820_SetMaxSpeed(0,pInitDevicePrm->maxSpeed)==FALSE) 01388 { 01389 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED); 01390 } 01391 if (STSPIN820_SetMinSpeed(0,pInitDevicePrm->minSpeed)==FALSE) 01392 { 01393 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED); 01394 } 01395 01396 STSPIN820_VrefPwmSetFreq(0,pInitDevicePrm->vrefPwmFreq); 01397 STSPIN820_SetTorque(0,ACC_TORQUE,pInitDevicePrm->accelTorque); 01398 STSPIN820_SetTorque(0,DEC_TORQUE,pInitDevicePrm->decelTorque); 01399 STSPIN820_SetTorque(0,RUN_TORQUE,pInitDevicePrm->runTorque); 01400 STSPIN820_SetTorque(0,HOLD_TORQUE,pInitDevicePrm->holdTorque); 01401 device_prm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable; 01402 device_prm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold; 01403 STSPIN820_SetStopMode(0,pInitDevicePrm->stopMode); 01404 01405 STSPIN820_SetDeviceParamsOtherValues(); 01406 01407 /* Eventually deactivate motor */ 01408 if ((device_prm.motionState != INACTIVE)&&\ 01409 (device_prm.motionState != STANDBY)) 01410 { 01411 STSPIN820_HardHiZ(0); 01412 } 01413 01414 /* Enter standby */ 01415 STSPIN820_Board_Reset(); 01416 01417 /* Reset the microstepping sequencer position */ 01418 device_prm.sequencerPosition = 0; 01419 01420 /* Reset current and mark positions */ 01421 device_prm.currentPosition = 0; 01422 device_prm.markPosition = 0; 01423 01424 /* Set predefined step mode */ 01425 STSPIN820_SetStepMode(0, pInitDevicePrm->stepMode); 01426 01427 /* Wait */ 01428 STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY); 01429 01430 /* Exit standby */ 01431 STSPIN820_Board_ReleaseReset(); 01432 01433 /* Let a delay after reset release*/ 01434 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); 01435 01436 } 01437 01438 /******************************************************//** 01439 * @brief Sets the parameters of the device to predefined values 01440 * from STSPIN820_config.h 01441 * @retval None 01442 **********************************************************/ 01443 void STSPIN820::STSPIN820_SetDeviceParamsToPredefinedValues(void) 01444 { 01445 device_prm.motionState = STANDBY; 01446 01447 if (STSPIN820_SetAcceleration(0,STSPIN820_CONF_PARAM_ACC)==FALSE) 01448 { 01449 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION); 01450 } 01451 if (STSPIN820_SetDeceleration(0,STSPIN820_CONF_PARAM_DEC)==FALSE) 01452 { 01453 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION); 01454 } 01455 if (STSPIN820_SetMaxSpeed(0,STSPIN820_CONF_PARAM_RUNNING_SPEED)==FALSE) 01456 { 01457 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED); 01458 } 01459 if (STSPIN820_SetMinSpeed(0,STSPIN820_CONF_PARAM_MIN_SPEED)==FALSE) 01460 { 01461 STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED); 01462 } 01463 01464 STSPIN820_VrefPwmSetFreq(0,STSPIN820_CONF_PARAM_REF_PWM_FREQUENCY); 01465 STSPIN820_SetTorque(0,ACC_TORQUE,STSPIN820_CONF_PARAM_ACC_TORQUE); 01466 STSPIN820_SetTorque(0,DEC_TORQUE,STSPIN820_CONF_PARAM_DEC_TORQUE); 01467 STSPIN820_SetTorque(0,RUN_TORQUE,STSPIN820_CONF_PARAM_RUNNING_TORQUE); 01468 STSPIN820_SetTorque(0,HOLD_TORQUE,STSPIN820_CONF_PARAM_HOLDING_TORQUE); 01469 device_prm.torqueBoostEnable = STSPIN820_CONF_PARAM_TORQUE_BOOST_EN; 01470 device_prm.torqueBoostSpeedThreshold = STSPIN820_CONF_PARAM_TORQUE_BOOST_TH; 01471 STSPIN820_SetStopMode(0,STSPIN820_CONF_PARAM_AUTO_HIZ_STOP); 01472 01473 STSPIN820_SetDeviceParamsOtherValues(); 01474 01475 /* Eventually deactivate motor */ 01476 if ((device_prm.motionState != INACTIVE)&&\ 01477 (device_prm.motionState != STANDBY)) 01478 { 01479 STSPIN820_HardHiZ(0); 01480 } 01481 01482 /* Enter standby */ 01483 STSPIN820_Board_Reset(); 01484 01485 /* Reset the microstepping sequencer position */ 01486 device_prm.sequencerPosition = 0; 01487 01488 /* Reset current and mark positions */ 01489 device_prm.currentPosition = 0; 01490 device_prm.markPosition = 0; 01491 01492 /* Set predefined step mode */ 01493 STSPIN820_SetStepMode(0, STSPIN820_CONF_PARAM_STEP_MODE); 01494 01495 /* Wait */ 01496 STSPIN820_Board_Delay(SELECT_STEP_MODE_DELAY); 01497 01498 /* Exit standby */ 01499 STSPIN820_Board_ReleaseReset(); 01500 01501 /* Let a delay after reset release*/ 01502 STSPIN820_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); 01503 } 01504 01505 /******************************************************//** 01506 * @brief Initialises the bridge parameters to start the movement 01507 * and enable the power bridge 01508 * @param[in] deviceId Unused parameter 01509 * @retval None 01510 **********************************************************/ 01511 void STSPIN820::STSPIN820_StartMovement(uint8_t deviceId) 01512 { 01513 deviceId = 0; 01514 01515 /* Enable STSPIN820 powerstage */ 01516 STSPIN820_Enable(deviceId); 01517 toggle_odd = 0; 01518 device_prm.accu = 0; 01519 device_prm.relativePos = 0; 01520 if ((device_prm.endAccPos == 0)&&\ 01521 (device_prm.commandExecuted != RUN_CMD)) 01522 { 01523 device_prm.motionState = DECELERATING; 01524 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE); 01525 } 01526 else 01527 { 01528 device_prm.motionState = ACCELERATING; 01529 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE); 01530 } 01531 STSPIN820_Board_PwmRefStart(); 01532 /* Initialize the step clock timer */ 01533 STSPIN820_Board_TimStckInit(); 01534 /* Program the step clock */ 01535 STSPIN820_Board_TimStckCompareInit(); 01536 STSPIN820_ApplySpeed(deviceId, device_prm.minSpeed); 01537 STSPIN820_Board_TimStckStart(); 01538 } 01539 01540 /******************************************************//** 01541 * @brief Handles the device state machine at each pulse 01542 * @param[in] deviceId Unused parameter 01543 * @retval None 01544 * @note Must only be called by the timer ISR 01545 **********************************************************/ 01546 void STSPIN820::STSPIN820_StepClockHandler(uint8_t deviceId) 01547 { 01548 uint32_t stepModeShift = device_prm.stepModeLatched - device_prm.stepMode; 01549 uint16_t tmp; 01550 deviceId = 0; 01551 01552 if (device_prm.motionState == STANDBYTOINACTIVE) 01553 { 01554 if (toggle_odd != 0) 01555 { 01556 toggle_odd = 0; 01557 if (device_prm.sequencerPosition == 0) 01558 { 01559 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0) 01560 { 01561 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK); 01562 } 01563 return; 01564 } 01565 } 01566 else 01567 { 01568 toggle_odd = 1; 01569 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode)); 01570 device_prm.sequencerPosition -= tmp; 01571 } 01572 STSPIN820_Board_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ); 01573 return; 01574 } 01575 01576 if (toggle_odd == 0) 01577 { 01578 toggle_odd = 1; 01579 } 01580 else 01581 { 01582 toggle_odd = 0; 01583 /* Incrementation of the relative position */ 01584 device_prm.relativePos += (1 << stepModeShift); 01585 01586 /* Incrementation of the current position */ 01587 if (device_prm.direction != BACKWARD) 01588 { 01589 device_prm.currentPosition += (1 << stepModeShift); 01590 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode)); 01591 device_prm.sequencerPosition += tmp; 01592 if (device_prm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1)) 01593 { 01594 device_prm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1); 01595 } 01596 } 01597 else 01598 { 01599 device_prm.currentPosition -= (1 << stepModeShift); 01600 tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode)); 01601 device_prm.sequencerPosition -= tmp; 01602 if (device_prm.sequencerPosition < 0) 01603 { 01604 device_prm.sequencerPosition += (SEQUENCER_MAX_VALUE+1); 01605 } 01606 } 01607 01608 switch (device_prm.motionState) 01609 { 01610 case ACCELERATING: 01611 { 01612 uint32_t relPos = device_prm.relativePos; 01613 uint32_t endAccPos = device_prm.endAccPos; 01614 uint16_t speed = device_prm.speed; 01615 uint32_t acc = ((uint32_t)device_prm.acceleration << 16)>>stepModeShift; 01616 01617 if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\ 01618 ((device_prm.commandExecuted==MOVE_CMD)&&(relPos>=device_prm.startDecPos))) 01619 { 01620 device_prm.motionState = DECELERATING; 01621 device_prm.accu = 0; 01622 /* Apply decelerating torque */ 01623 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE); 01624 } 01625 else if ((speed>=(device_prm.maxSpeed>>stepModeShift))||\ 01626 ((device_prm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos))) 01627 { 01628 device_prm.motionState = STEADY; 01629 /* Apply running torque */ 01630 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE); 01631 } 01632 else 01633 { 01634 bool speedUpdated = FALSE; 01635 /* Go on accelerating */ 01636 if (speed==0) 01637 { 01638 speed =1; 01639 } 01640 device_prm.accu += acc / speed; 01641 while (device_prm.accu>=(0X10000L)) 01642 { 01643 device_prm.accu -= (0X10000L); 01644 speed +=1; 01645 speedUpdated = TRUE; 01646 } 01647 01648 if (speedUpdated) 01649 { 01650 if (speed>(device_prm.maxSpeed>>stepModeShift)) 01651 { 01652 speed = device_prm.maxSpeed>>stepModeShift; 01653 } 01654 device_prm.speed = speed; 01655 } 01656 01657 if (device_prm.updateTorque!=FALSE) 01658 { 01659 /* Apply accelerating torque */ 01660 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE); 01661 } 01662 } 01663 break; 01664 } 01665 case STEADY: 01666 { 01667 uint16_t maxSpeed = device_prm.maxSpeed>>stepModeShift; 01668 uint32_t relativePos = device_prm.relativePos; 01669 if (device_prm.updateTorque!=FALSE) 01670 { 01671 /* Apply accelerating torque */ 01672 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE); 01673 } 01674 if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\ 01675 ((device_prm.commandExecuted==MOVE_CMD)&&\ 01676 (relativePos>=(device_prm.startDecPos)))||\ 01677 ((device_prm.commandExecuted==RUN_CMD)&&\ 01678 (device_prm.speed>maxSpeed))) 01679 { 01680 device_prm.motionState = DECELERATING; 01681 device_prm.accu = 0; 01682 /* Apply decelerating torque */ 01683 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE); 01684 } 01685 else if ((device_prm.commandExecuted==RUN_CMD)&&(device_prm.speed<maxSpeed)) 01686 { 01687 device_prm.motionState = ACCELERATING; 01688 device_prm.accu = 0; 01689 /* Apply accelerating torque */ 01690 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE); 01691 } 01692 break; 01693 } 01694 case DECELERATING: 01695 { 01696 uint32_t relativePos = device_prm.relativePos; 01697 uint16_t speed = device_prm.speed; 01698 uint32_t dec = ((uint32_t)device_prm.deceleration << 16)>>stepModeShift; 01699 if ((((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)&&\ 01700 (speed<=(device_prm.minSpeed>>stepModeShift)))||\ 01701 ((device_prm.commandExecuted==MOVE_CMD)&&(relativePos>=device_prm.stepsToTake))) 01702 { 01703 /* Motion process complete */ 01704 if ((device_prm.commandExecuted&STSPIN820_DIR_CHANGE_BIT_MASK)!=0) 01705 { 01706 device_prm.commandExecuted&=~STSPIN820_DIR_CHANGE_BIT_MASK; 01707 if (device_prm.direction==BACKWARD) 01708 { 01709 device_prm.direction=FORWARD; 01710 } 01711 else device_prm.direction=BACKWARD; 01712 STSPIN820_Board_SetDirectionGpio(device_prm.direction); 01713 if ((device_prm.commandExecuted&STSPIN820_SOFT_STOP_BIT_MASK)==0) 01714 { 01715 device_prm.motionState = ACCELERATING; 01716 device_prm.accu = 0; 01717 /* Apply accelerating torque */ 01718 STSPIN820_ApplyTorque(deviceId, ACC_TORQUE); 01719 break; 01720 } 01721 } 01722 if (device_prm.stopMode==HOLD_MODE) 01723 { 01724 STSPIN820_HardStop(deviceId); 01725 } 01726 else if (device_prm.stopMode==STANDBY_MODE) 01727 { 01728 STSPIN820_PutDeviceInStandby(deviceId); 01729 } 01730 else 01731 { 01732 STSPIN820_HardHiZ(deviceId); 01733 } 01734 } 01735 else if ((device_prm.commandExecuted==RUN_CMD)&& 01736 (speed<=(device_prm.maxSpeed>>stepModeShift))) 01737 { 01738 device_prm.motionState = STEADY; 01739 /* Apply running torque */ 01740 STSPIN820_ApplyTorque(deviceId, RUN_TORQUE); 01741 } 01742 else 01743 { 01744 /* Go on decelerating */ 01745 if (speed>(device_prm.minSpeed>>stepModeShift)) 01746 { 01747 bool speedUpdated = FALSE; 01748 if (speed==0) 01749 { 01750 speed = 1; 01751 } 01752 device_prm.accu += dec / speed; 01753 while (device_prm.accu>=(0X10000L)) 01754 { 01755 device_prm.accu -= (0X10000L); 01756 if (speed>1) 01757 { 01758 speed -=1; 01759 } 01760 speedUpdated = TRUE; 01761 } 01762 01763 if (speedUpdated) 01764 { 01765 if (speed<(device_prm.minSpeed>>stepModeShift)) 01766 { 01767 speed = device_prm.minSpeed>>stepModeShift; 01768 } 01769 device_prm.speed = speed; 01770 } 01771 01772 if (device_prm.updateTorque!=FALSE) 01773 { 01774 /* Apply decelerating torque */ 01775 STSPIN820_ApplyTorque(deviceId, DEC_TORQUE); 01776 } 01777 } 01778 } 01779 break; 01780 } 01781 default: 01782 { 01783 break; 01784 } 01785 } 01786 } 01787 if ((device_prm.motionState & INACTIVE) != INACTIVE) 01788 { 01789 STSPIN820_ApplySpeed(deviceId, device_prm.speed); 01790 } 01791 else 01792 { 01793 if (STSPIN820_Board_TimStckStop(&toggle_odd) == 0) 01794 { 01795 STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK); 01796 } 01797 } 01798 } 01799 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Sat Jul 16 2022 06:19:57 by
1.7.2