Davide Aliprandi
/
X-NUCLEO-IHM06A1
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.
Fork of
X_NUCLEO_IHM06A1
by 
ST
Embed:
(wiki syntax)
Show/hide line numbers
stspin220_class.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file stspin220_class.cpp 00004 * @author IPC Rennes 00005 * @version V1.0.0 00006 * @date July 27th, 2016 00007 * @brief STSPIN220 product related routines 00008 * @note (C) COPYRIGHT 2016 STMicroelectronics 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2016 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 /* Includes ------------------------------------------------------------------*/ 00040 #include "stspin220_class.h" 00041 00042 /* Definitions ---------------------------------------------------------------*/ 00043 00044 /* Variables ----------------------------------------------------------------*/ 00045 /* Number of devices. */ 00046 uint8_t STSPIN220::numberOfDevices = 0; 00047 00048 /* Methods -------------------------------------------------------------------*/ 00049 /******************************************************//** 00050 * @brief Start the STSPIN220 library 00051 * @param[in] pInit pointer to the initialization data 00052 * @retval COMPONENT_OK in case of success. 00053 **********************************************************/ 00054 Status_t STSPIN220::Stspin220_Init(void* pInit) 00055 { 00056 Stspin220_Disable(); 00057 if (pInit == NULL) 00058 { 00059 /* Set context variables to the predefined values from Stspin220_target_config.h */ 00060 /* Set GPIO according to these values */ 00061 Stspin220_SetDeviceParamsToPredefinedValues(); 00062 } 00063 else 00064 { 00065 Stspin220_SetDeviceParamsToGivenValues((Stspin220_Init_t*) pInit); 00066 } 00067 Stspin220_Board_TimStckInit(false); 00068 return COMPONENT_OK; 00069 } 00070 00071 /********************************************************** 00072 * @brief Read id 00073 * @param id pointer to the identifier to be read. 00074 * @retval COMPONENT_OK in case of success. 00075 **********************************************************/ 00076 Status_t STSPIN220::Stspin220_ReadID(uint8_t *id) 00077 { 00078 *id = deviceInstance; 00079 00080 return COMPONENT_OK; 00081 } 00082 00083 /********************************************************** 00084 * @brief Attaches a user callback to the error Handler. 00085 * The call back will be then called each time the library 00086 * detects an error 00087 * @param[in] callback Name of the callback to attach 00088 * to the error Hanlder 00089 * @retval None 00090 **********************************************************/ 00091 void STSPIN220::Stspin220_AttachErrorHandler(void (*callback)(uint16_t error)) 00092 { 00093 errorHandlerCallback = (void (*)(uint16_t error)) callback; 00094 } 00095 00096 /******************************************************//** 00097 * @brief Apply the set torque 00098 * @param[in] torqueMode torque mode 00099 * @retval None 00100 * @note 00101 **********************************************************/ 00102 void STSPIN220::Stspin220_ApplyTorque(motorTorqueMode_t torqueMode) 00103 { 00104 uint8_t torqueValue = 0; 00105 devicePrm.updateTorque = false; 00106 switch(torqueMode) 00107 { 00108 case ACC_TORQUE: 00109 devicePrm.currentTorque = devicePrm.accelTorque; 00110 break; 00111 case DEC_TORQUE: 00112 devicePrm.currentTorque = devicePrm.decelTorque; 00113 break; 00114 case RUN_TORQUE: 00115 devicePrm.currentTorque = devicePrm.runTorque; 00116 break; 00117 case HOLD_TORQUE: 00118 devicePrm.currentTorque = devicePrm.holdTorque; 00119 break; 00120 case CURRENT_TORQUE: 00121 break; 00122 default: 00123 return; //ignore error 00124 } 00125 torqueValue = devicePrm.currentTorque; 00126 Stspin220_Board_PwmRefSetDutyCycle(torqueValue); 00127 } 00128 00129 /******************************************************//** 00130 * @brief Disable the power bridges (leave the output bridges HiZ) 00131 * @retval None 00132 **********************************************************/ 00133 void STSPIN220::Stspin220_Disable(void) 00134 { 00135 Stspin220_Board_Disable(); 00136 } 00137 00138 /******************************************************//** 00139 * @brief Enable the power bridges 00140 * @retval None 00141 **********************************************************/ 00142 void STSPIN220::Stspin220_Enable(void) 00143 { 00144 Stspin220_Board_Enable(); 00145 } 00146 00147 /******************************************************//** 00148 * @brief Error handler which calls the user callback (if defined) 00149 * @param[in] error Number of the error 00150 * @retval None 00151 **********************************************************/ 00152 void STSPIN220::Stspin220_ErrorHandler(uint16_t error) 00153 { 00154 if (errorHandlerCallback != 0) 00155 { 00156 errorHandlerCallback(error); 00157 } 00158 else 00159 { 00160 while(1) 00161 { 00162 /* Infinite loop */ 00163 } 00164 } 00165 } 00166 00167 /******************************************************//** 00168 * @brief Exit STSPIN220 device from standby (low power consumption) 00169 * @retval None 00170 **********************************************************/ 00171 void STSPIN220::Stspin220_ExitDeviceFromStandby(void) 00172 { 00173 uint32_t sequencerPosition = devicePrm.sequencerPosition; 00174 00175 /* Exit standby and set step mode */ 00176 Stspin220_SetStepModeWithoutReset(devicePrm.stepMode); 00177 00178 if (devicePrm.sequencerPosition != 0) 00179 { 00180 /* Set direction to FORWARD to ensure the HW sequencer is increased at */ 00181 /* each step clock rising edge */ 00182 Stspin220_SetDirection(FORWARD); 00183 /* Going out of standby */ 00184 devicePrm.motionState = STANDBYTOINACTIVE; 00185 /* Program the step clock */ 00186 Stspin220_Board_TimStckInit(true); 00187 Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); 00188 toggleOdd = 0; 00189 while (devicePrm.sequencerPosition != 0); 00190 while (toggleOdd!=0); 00191 devicePrm.sequencerPosition = sequencerPosition; 00192 } 00193 00194 devicePrm.motionState = INACTIVE; 00195 } 00196 00197 /******************************************************//** 00198 * @brief Return the acceleration 00199 * @retval Acceleration in pps^2 00200 **********************************************************/ 00201 uint16_t STSPIN220::Stspin220_GetAcceleration(void) 00202 { 00203 return (devicePrm.acceleration); 00204 } 00205 00206 /******************************************************//** 00207 * @brief Return the current speed 00208 * @retval Speed in pps 00209 **********************************************************/ 00210 uint16_t STSPIN220::Stspin220_GetCurrentSpeed(void) 00211 { 00212 return devicePrm.speed; 00213 } 00214 00215 /******************************************************//** 00216 * @brief Return the deceleration 00217 * @retval Deceleration in pps^2 00218 **********************************************************/ 00219 uint16_t STSPIN220::Stspin220_GetDeceleration(void) 00220 { 00221 return (devicePrm.deceleration); 00222 } 00223 00224 /******************************************************//** 00225 * @brief Return the device state 00226 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) 00227 **********************************************************/ 00228 motorState_t STSPIN220::Stspin220_GetDeviceState(void) 00229 { 00230 return devicePrm.motionState; 00231 } 00232 00233 /******************************************************//** 00234 * @brief Get the motor current direction 00235 * @retval direction 00236 **********************************************************/ 00237 motorDir_t STSPIN220::Stspin220_GetDirection(void) 00238 { 00239 return devicePrm.direction; 00240 } 00241 00242 /******************************************************//** 00243 * @brief Return the FW version. 00244 * @retval FW version 00245 **********************************************************/ 00246 uint32_t STSPIN220::Stspin220_GetFwVersion(void) 00247 { 00248 return (STSPIN220_FW_VERSION); 00249 } 00250 00251 /******************************************************//** 00252 * @brief Get the mark position (32b signed) 00253 * @retval mark position 00254 **********************************************************/ 00255 int32_t STSPIN220::Stspin220_GetMark(void) 00256 { 00257 return devicePrm.markPosition; 00258 } 00259 00260 /******************************************************//** 00261 * @brief Return the max speed 00262 * @retval maxSpeed in pps 00263 **********************************************************/ 00264 uint16_t STSPIN220::Stspin220_GetMaxSpeed(void) 00265 { 00266 return (devicePrm.maxSpeed); 00267 } 00268 00269 /******************************************************//** 00270 * @brief Get the min speed 00271 * in step/s for full, half and wave modes 00272 * in microsteps/s for microstep modes 00273 * @retval return the min speed in step/s or microstep/s 00274 * @note 00275 **********************************************************/ 00276 uint16_t STSPIN220::Stspin220_GetMinSpeed(void) 00277 { 00278 return (devicePrm.minSpeed); 00279 } 00280 00281 /******************************************************//** 00282 * @brief Get the current position (32b signed) 00283 * @retval current position value 00284 **********************************************************/ 00285 int32_t STSPIN220::Stspin220_GetPosition(void) 00286 { 00287 return devicePrm.currentPosition; 00288 } 00289 00290 /******************************************************//** 00291 * @brief Get the motor step mode 00292 * @retval step mode 00293 **********************************************************/ 00294 motorStepMode_t STSPIN220::Stspin220_GetStepMode(void) 00295 { 00296 return devicePrm.stepMode; 00297 } 00298 00299 /******************************************************//** 00300 * @brief Get the selected stop mode 00301 * @retval the selected stop mode 00302 **********************************************************/ 00303 motorStopMode_t STSPIN220::Stspin220_GetStopMode(void) 00304 { 00305 return devicePrm.stopMode; 00306 } 00307 00308 /******************************************************//** 00309 * @brief Get the torque 00310 * @param[in] torqueMode torque mode 00311 * @retval the torqueValue in % (from 0 to 100) 00312 * @note 00313 **********************************************************/ 00314 uint8_t STSPIN220::Stspin220_GetTorque(motorTorqueMode_t torqueMode) 00315 { 00316 uint8_t torqueValue = 0; 00317 switch(torqueMode) 00318 { 00319 case ACC_TORQUE: 00320 torqueValue = devicePrm.accelTorque; 00321 break; 00322 case DEC_TORQUE: 00323 torqueValue = devicePrm.decelTorque; 00324 break; 00325 case RUN_TORQUE: 00326 torqueValue = devicePrm.runTorque; 00327 break; 00328 case HOLD_TORQUE: 00329 torqueValue = devicePrm.holdTorque; 00330 break; 00331 case CURRENT_TORQUE: 00332 torqueValue = devicePrm.currentTorque; 00333 break; 00334 default: 00335 break; 00336 } 00337 return torqueValue; 00338 } 00339 00340 /******************************************************//** 00341 * @brief Get the torque boost feature status 00342 * @retval TRUE if enabled, FALSE if disabled 00343 **********************************************************/ 00344 bool STSPIN220::Stspin220_GetTorqueBoostEnable(void) 00345 { 00346 return devicePrm.torqueBoostEnable; 00347 } 00348 00349 /******************************************************//** 00350 * @brief Get the torque boost threshold 00351 * @retval The torque boost threshold above which the step mode is 00352 * changed to full step 00353 **********************************************************/ 00354 uint16_t STSPIN220::Stspin220_GetTorqueBoostThreshold(void) 00355 { 00356 return devicePrm.torqueBoostSpeedThreshold; 00357 } 00358 00359 /******************************************************//** 00360 * @brief Go to the home position 00361 * @retval None 00362 **********************************************************/ 00363 void STSPIN220::Stspin220_GoHome(void) 00364 { 00365 Stspin220_GoTo(0); 00366 } 00367 00368 /******************************************************//** 00369 * @brief Go to the Mark position 00370 * @retval None 00371 **********************************************************/ 00372 void STSPIN220::Stspin220_GoMark(void) 00373 { 00374 Stspin220_GoTo(devicePrm.markPosition); 00375 } 00376 00377 /******************************************************//** 00378 * @brief Request the motor to move to the specified position 00379 * @param[in] targetPosition absolute position in steps 00380 * @retval None 00381 * @note The position is at the resolution corresponding to the 00382 * selected step mode. 00383 * STEP_MODE_FULL : step 00384 * STEP_MODE_HALF : 1/2 step 00385 * STEP_MODE_1_4 : 1/4 step 00386 * STEP_MODE_1_8 : 1/8 step 00387 * STEP_MODE_1_16 : 1/16 step 00388 * STEP_MODE_1_32 : 1/32 step 00389 * STEP_MODE_1_64 : 1/64 step 00390 * STEP_MODE_1_128 : 1/128 step 00391 * STEP_MODE_1_256 : 1/256 step 00392 **********************************************************/ 00393 void STSPIN220::Stspin220_GoTo(int32_t targetPosition) 00394 { 00395 motorDir_t direction; 00396 00397 /* Exit from standby if needed */ 00398 if (devicePrm.motionState == STANDBY) 00399 { 00400 Stspin220_ExitDeviceFromStandby(); 00401 } 00402 /* Deactivate motor if needed */ 00403 else if (devicePrm.motionState != INACTIVE) 00404 { 00405 Stspin220_HardHiZ(); 00406 } 00407 00408 if (targetPosition > devicePrm.currentPosition) 00409 { 00410 devicePrm.stepsToTake = targetPosition -\ 00411 devicePrm.currentPosition; 00412 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) 00413 { 00414 direction = FORWARD; 00415 } 00416 else 00417 { 00418 direction = BACKWARD; 00419 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ 00420 devicePrm.stepsToTake; 00421 } 00422 } 00423 else 00424 { 00425 devicePrm.stepsToTake = devicePrm.currentPosition -\ 00426 targetPosition; 00427 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) 00428 { 00429 direction = BACKWARD; 00430 } 00431 else 00432 { 00433 direction = FORWARD; 00434 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ 00435 devicePrm.stepsToTake; 00436 } 00437 } 00438 00439 if (devicePrm.stepsToTake != 0) 00440 { 00441 devicePrm.commandExecuted = MOVE_CMD; 00442 00443 /* Direction setup */ 00444 Stspin220_SetDirection(direction); 00445 00446 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake); 00447 00448 /* Motor activation */ 00449 Stspin220_StartMovement(); 00450 } 00451 } 00452 00453 /******************************************************//** 00454 * @brief Move the motor to the absolute position 00455 * @param[in] direction FORWARD or BACKWARD 00456 * @param[in] targetPosition 32 bit signed value position 00457 * @retval None 00458 * @note The position is at the resolution corresponding to the 00459 * selected step mode. 00460 * STEP_MODE_FULL : step 00461 * STEP_MODE_HALF : 1/2 step 00462 * STEP_MODE_1_4 : 1/4 step 00463 * STEP_MODE_1_8 : 1/8 step 00464 * STEP_MODE_1_16 : 1/16 step 00465 * STEP_MODE_1_32 : 1/32 step 00466 * STEP_MODE_1_64 : 1/64 step 00467 * STEP_MODE_1_128 : 1/128 step 00468 * STEP_MODE_1_256 : 1/256 step 00469 **********************************************************/ 00470 void STSPIN220::Stspin220_GoToDir(motorDir_t direction, int32_t targetPosition) 00471 { 00472 /* Exit from standby if needed */ 00473 if (devicePrm.motionState == STANDBY) 00474 { 00475 Stspin220_ExitDeviceFromStandby(); 00476 } 00477 /* Deactivate motor if needed */ 00478 else if (devicePrm.motionState != INACTIVE) 00479 { 00480 Stspin220_HardHiZ(); 00481 } 00482 00483 if (direction != BACKWARD) 00484 { 00485 if (targetPosition > devicePrm.currentPosition) 00486 { 00487 devicePrm.stepsToTake = targetPosition -\ 00488 devicePrm.currentPosition; 00489 } 00490 else 00491 { 00492 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ 00493 (targetPosition -\ 00494 devicePrm.currentPosition); 00495 } 00496 } 00497 else 00498 { 00499 if (targetPosition > devicePrm.currentPosition) 00500 { 00501 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ 00502 (devicePrm.currentPosition -\ 00503 targetPosition); 00504 } 00505 else 00506 { 00507 devicePrm.stepsToTake = devicePrm.currentPosition -\ 00508 targetPosition; 00509 } 00510 } 00511 00512 if (devicePrm.stepsToTake != 0) 00513 { 00514 devicePrm.commandExecuted = MOVE_CMD; 00515 00516 /* Direction setup */ 00517 Stspin220_SetDirection(direction); 00518 00519 Stspin220_ComputeSpeedProfile(devicePrm.stepsToTake); 00520 00521 /* Motor activation */ 00522 Stspin220_StartMovement(); 00523 } 00524 } 00525 00526 /******************************************************//** 00527 * @brief Immediately stop the motor and disables the power bridges 00528 * @retval None 00529 **********************************************************/ 00530 void STSPIN220::Stspin220_HardHiZ(void) 00531 { 00532 /* Set inactive state */ 00533 devicePrm.motionState = INACTIVE; 00534 00535 /* Disable step clock */ 00536 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) 00537 { 00538 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 00539 } 00540 00541 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */ 00542 /* after the last step clock rising edge triggering the last step */ 00543 Stspin220_Board_Delay(DISABLE_DELAY); 00544 00545 /* Set reference voltage to 0 */ 00546 Stspin220_SetTorque(CURRENT_TORQUE, 0); 00547 00548 /* Disable power bridges */ 00549 Stspin220_Board_Disable(); 00550 00551 /* Comeback to nominal step mode */ 00552 if (devicePrm.stepModeLatched != devicePrm.stepMode) 00553 { 00554 Stspin220_Board_UnsetFullStep(); 00555 devicePrm.stepMode = devicePrm.stepModeLatched; 00556 } 00557 00558 devicePrm.commandExecuted = NO_CMD; 00559 devicePrm.stepsToTake = 0; 00560 devicePrm.speed = 0; 00561 } 00562 00563 /******************************************************//** 00564 * @brief Immediatly stop the motor 00565 * and either set holding torque when stop mode is HOLD_MODE, 00566 * or call Stspin220_HardHiz function when stop mode is HIZ_MODE, 00567 * or call Stspin220_PutDeviceInStandby function when stop mode is STANDBY_MODE 00568 * @retval None 00569 **********************************************************/ 00570 void STSPIN220::Stspin220_HardStop(void) 00571 { 00572 if (devicePrm.stopMode == HOLD_MODE) 00573 { 00574 /* Set inactive state */ 00575 devicePrm.motionState = INACTIVE; 00576 00577 /* Disable step clock */ 00578 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) 00579 { 00580 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 00581 } 00582 00583 /* Set holding torque */ 00584 Stspin220_ApplyTorque(HOLD_TORQUE); 00585 00586 /* Comeback to nominal step mode */ 00587 if (devicePrm.stepModeLatched != devicePrm.stepMode) 00588 { 00589 Stspin220_Board_UnsetFullStep(); 00590 devicePrm.stepMode = devicePrm.stepModeLatched; 00591 } 00592 00593 devicePrm.commandExecuted = NO_CMD; 00594 devicePrm.stepsToTake = 0; 00595 devicePrm.speed = 0; 00596 } 00597 else if (devicePrm.stopMode == HIZ_MODE) 00598 { 00599 Stspin220_HardHiZ(); 00600 } 00601 else if (devicePrm.stopMode == STANDBY_MODE) 00602 { 00603 Stspin220_PutDeviceInStandby(); 00604 } 00605 } 00606 00607 /******************************************************//** 00608 * @brief Moves the motor of the specified number of steps 00609 * @param[in] direction FORWARD or BACKWARD 00610 * @param[in] stepCount Number of steps to perform 00611 * @retval None 00612 **********************************************************/ 00613 void STSPIN220::Stspin220_Move(motorDir_t direction, uint32_t stepCount) 00614 { 00615 /* Exit from standby if needed */ 00616 if (devicePrm.motionState == STANDBY) 00617 { 00618 Stspin220_ExitDeviceFromStandby(); 00619 } 00620 /* Deactivate motor if needed */ 00621 else if (devicePrm.motionState != INACTIVE) 00622 { 00623 Stspin220_HardHiZ(); 00624 } 00625 00626 if (stepCount != 0) 00627 { 00628 devicePrm.stepsToTake = stepCount; 00629 devicePrm.commandExecuted = MOVE_CMD; 00630 00631 /* Direction setup */ 00632 Stspin220_SetDirection(direction); 00633 00634 Stspin220_ComputeSpeedProfile(stepCount); 00635 00636 /* Motor activation */ 00637 Stspin220_StartMovement(); 00638 } 00639 } 00640 00641 /******************************************************//** 00642 * @brief Put STSPIN220 device in standby (low power consumption) 00643 * @retval None 00644 **********************************************************/ 00645 void STSPIN220::Stspin220_PutDeviceInStandby(void) 00646 { 00647 /* Stop movement */ 00648 Stspin220_HardHiZ(); 00649 00650 /* Enter standby */ 00651 Stspin220_Board_Reset(); 00652 00653 devicePrm.motionState = STANDBY; 00654 } 00655 00656 /******************************************************//** 00657 * @brief Runs the motor. It will accelerate from the min 00658 * speed up to the max speed by using the device acceleration. 00659 * @param[in] direction FORWARD or BACKWARD 00660 * @retval None 00661 **********************************************************/ 00662 void STSPIN220::Stspin220_Run(motorDir_t direction) 00663 { 00664 /* Exit from standby if needed */ 00665 if (devicePrm.motionState == STANDBY) 00666 { 00667 Stspin220_ExitDeviceFromStandby(); 00668 } 00669 /* Deactivate motor if needed */ 00670 else if (devicePrm.motionState != INACTIVE) 00671 { 00672 Stspin220_HardHiZ(); 00673 } 00674 00675 /* Direction setup */ 00676 Stspin220_SetDirection(direction); 00677 devicePrm.commandExecuted = RUN_CMD; 00678 /* Motor activation */ 00679 Stspin220_StartMovement(); 00680 } 00681 00682 /******************************************************//** 00683 * @brief Changes the acceleration 00684 * @param[in] newAcc New acceleration to apply in pps^2 00685 * @retval true if the command is successfully executed, else false 00686 * @note The command is not performed if the device is executing 00687 * a MOVE or GOTO command (but it can be used during a RUN command) 00688 **********************************************************/ 00689 bool STSPIN220::Stspin220_SetAcceleration(uint16_t newAcc) 00690 { 00691 bool cmdExecuted = false; 00692 if ((newAcc != 0)&& 00693 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00694 (devicePrm.commandExecuted == RUN_CMD))) 00695 { 00696 devicePrm.acceleration = newAcc; 00697 cmdExecuted = true; 00698 } 00699 return cmdExecuted; 00700 } 00701 00702 /******************************************************//** 00703 * @brief Changes the deceleration 00704 * @param[in] newDec New deceleration to apply in pps^2 00705 * @retval true if the command is successfully executed, else false 00706 * @note The command is not performed if the device is executing 00707 * a MOVE or GOTO command (but it can be used during a RUN command) 00708 **********************************************************/ 00709 bool STSPIN220::Stspin220_SetDeceleration(uint16_t newDec) 00710 { 00711 bool cmdExecuted = false; 00712 if ((newDec != 0)&& 00713 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00714 (devicePrm.commandExecuted == RUN_CMD))) 00715 { 00716 devicePrm.deceleration = newDec; 00717 cmdExecuted = true; 00718 } 00719 return cmdExecuted; 00720 } 00721 00722 /******************************************************//** 00723 * @brief Specifies the direction 00724 * @param[in] dir FORWARD or BACKWARD 00725 * @note The direction change is applied if the device 00726 * is in INACTIVE or STANDBY state or if the device is 00727 * executing a run command 00728 * @retval None 00729 **********************************************************/ 00730 void STSPIN220::Stspin220_SetDirection(motorDir_t dir) 00731 { 00732 if ((devicePrm.motionState == INACTIVE)||\ 00733 (devicePrm.motionState == STANDBY)) 00734 { 00735 devicePrm.direction = dir; 00736 Stspin220_Board_SetDirectionGpio(dir); 00737 } 00738 else if ((devicePrm.commandExecuted&RUN_CMD)!=0) 00739 { 00740 devicePrm.commandExecuted=(deviceCommand_t) 00741 (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted); 00742 } 00743 } 00744 00745 /******************************************************//** 00746 * @brief Set current position to be the Home position 00747 * (current position set to 0) 00748 * @retval None 00749 **********************************************************/ 00750 void STSPIN220::Stspin220_SetHome(void) 00751 { 00752 devicePrm.currentPosition = 0; 00753 } 00754 00755 /******************************************************//** 00756 * @brief Set current position to be the Mark position 00757 * @retval None 00758 **********************************************************/ 00759 void STSPIN220::Stspin220_SetMark(void) 00760 { 00761 devicePrm.markPosition = devicePrm.currentPosition; 00762 } 00763 00764 /******************************************************//** 00765 * @brief Changes the max speed 00766 * @param[in] newMaxSpeed New max speed to apply in pps 00767 * @retval true if the command is successfully executed, else false 00768 * @note The command is not performed is the device is executing 00769 * a MOVE or GOTO command (but it can be used during a RUN command). 00770 **********************************************************/ 00771 bool STSPIN220::Stspin220_SetMaxSpeed(uint16_t newMaxSpeed) 00772 { 00773 bool cmdExecuted = false; 00774 if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\ 00775 ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\ 00776 ((devicePrm.torqueBoostEnable != false)&&\ 00777 ((newMaxSpeed>>Stspin220_GetStepMode())<= STSPIN220_MAX_STCK_FREQ)))&&\ 00778 (devicePrm.minSpeed <= newMaxSpeed) &&\ 00779 (((devicePrm.motionState & INACTIVE) == INACTIVE)||\ 00780 (devicePrm.commandExecuted == RUN_CMD))) 00781 { 00782 devicePrm.maxSpeed = newMaxSpeed; 00783 cmdExecuted = true; 00784 } 00785 return cmdExecuted; 00786 } 00787 00788 /******************************************************//** 00789 * @brief Changes the min speed 00790 * @param[in] newMinSpeed New min speed to apply in pps 00791 * @retval true if the command is successfully executed, else false 00792 * @note The command is not performed is the device is executing 00793 * a MOVE or GOTO command (but it can be used during a RUN command). 00794 **********************************************************/ 00795 bool STSPIN220::Stspin220_SetMinSpeed(uint16_t newMinSpeed) 00796 { 00797 bool cmdExecuted = false; 00798 if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&& 00799 (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) && 00800 (newMinSpeed <= devicePrm.maxSpeed) && 00801 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00802 (devicePrm.commandExecuted == RUN_CMD))) 00803 { 00804 devicePrm.minSpeed = newMinSpeed; 00805 cmdExecuted = true; 00806 } 00807 return cmdExecuted; 00808 } 00809 00810 /******************************************************//** 00811 * @brief Set the stepping mode 00812 * @param[in] stepMode from full step to 1/256 microstep 00813 * as specified in enum motorStepMode_t 00814 * @retval true if the command is successfully executed, else false 00815 **********************************************************/ 00816 bool STSPIN220::Stspin220_SetStepMode(motorStepMode_t stepMode) 00817 { 00818 /* Eventually deactivate motor */ 00819 if ((devicePrm.motionState != INACTIVE)&&\ 00820 (devicePrm.motionState != STANDBY)) 00821 { 00822 Stspin220_HardHiZ(); 00823 } 00824 00825 /* Enter standby */ 00826 Stspin220_Board_Reset(); 00827 00828 /* Reset the microstepping sequencer position */ 00829 devicePrm.sequencerPosition = 0; 00830 00831 /* Reset current and mark positions */ 00832 devicePrm.currentPosition = 0; 00833 devicePrm.markPosition = 0; 00834 00835 /* Set the step mode */ 00836 return (Stspin220_SetStepModeWithoutReset(stepMode)); 00837 } 00838 00839 /******************************************************//** 00840 * @brief Select the mode to stop the motor. 00841 * @param[in] stopMode HOLD_MODE to let power bridge enabled 00842 * @retval None 00843 **********************************************************/ 00844 void STSPIN220::Stspin220_SetStopMode(motorStopMode_t stopMode) 00845 { 00846 devicePrm.stopMode = stopMode; 00847 } 00848 00849 /******************************************************//** 00850 * @brief Set the torque 00851 * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t 00852 * @param[in] torqueValue in % (from 0 to 100) 00853 * @retval None 00854 * @note 00855 **********************************************************/ 00856 void STSPIN220::Stspin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue) 00857 { 00858 devicePrm.updateTorque = true; 00859 if (torqueValue>100) torqueValue = 100; 00860 switch(torqueMode) 00861 { 00862 case ACC_TORQUE: 00863 devicePrm.accelTorque = torqueValue; 00864 break; 00865 case DEC_TORQUE: 00866 devicePrm.decelTorque = torqueValue; 00867 break; 00868 case RUN_TORQUE: 00869 devicePrm.runTorque = torqueValue; 00870 break; 00871 case HOLD_TORQUE: 00872 devicePrm.holdTorque = torqueValue; 00873 if (devicePrm.motionState != INACTIVE) 00874 { 00875 break; 00876 } 00877 case CURRENT_TORQUE: 00878 devicePrm.currentTorque = torqueValue; 00879 Stspin220_Board_PwmRefSetDutyCycle(torqueValue); 00880 default: 00881 devicePrm.updateTorque = false; 00882 break; //ignore error 00883 } 00884 } 00885 00886 /******************************************************//** 00887 * @brief Enable or disable the torque boost feature 00888 * @param[in] enable true to enable torque boost, false to disable 00889 * @retval None 00890 **********************************************************/ 00891 void STSPIN220::Stspin220_SetTorqueBoostEnable(bool enable) 00892 { 00893 devicePrm.torqueBoostEnable = enable; 00894 } 00895 00896 /******************************************************//** 00897 * @brief Set the torque boost threshold 00898 * @param[in] speedThreshold speed threshold above which the step mode is 00899 * changed to full step 00900 * @retval None 00901 **********************************************************/ 00902 void STSPIN220::Stspin220_SetTorqueBoostThreshold(uint16_t speedThreshold) 00903 { 00904 devicePrm.torqueBoostSpeedThreshold = speedThreshold; 00905 } 00906 00907 /******************************************************//** 00908 * @brief Stops the motor by using the device deceleration 00909 * @retval true if the command is successfully executed, else false 00910 * @note The command is not performed if the device is in INACTIVE, 00911 * STANDBYTOINACTIVE or STANDBY state. 00912 **********************************************************/ 00913 bool STSPIN220::Stspin220_SoftStop(void) 00914 { 00915 bool cmdExecuted = false; 00916 if ((devicePrm.motionState & INACTIVE) != INACTIVE) 00917 { 00918 devicePrm.commandExecuted=(deviceCommand_t) 00919 (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted); 00920 cmdExecuted = true; 00921 } 00922 return (cmdExecuted); 00923 } 00924 00925 /******************************************************//** 00926 * @brief Get the frequency of REF PWM 00927 * @retval the frequency of REF PWM in Hz 00928 * @note 00929 **********************************************************/ 00930 uint32_t STSPIN220::Stspin220_VrefPwmGetFreq(void) 00931 { 00932 return devicePrm.refPwmFreq; 00933 } 00934 00935 /******************************************************//** 00936 * @brief Set the frequency of REF PWM 00937 * @param[in] newFreq in Hz 00938 * @retval None 00939 * @note 00940 **********************************************************/ 00941 void STSPIN220::Stspin220_VrefPwmSetFreq(uint32_t newFreq) 00942 { 00943 devicePrm.refPwmFreq = newFreq; 00944 Stspin220_Board_PwmRefSetFreq(newFreq); 00945 } 00946 00947 /******************************************************//** 00948 * @brief Locks until the device state becomes Inactive 00949 * @retval None 00950 **********************************************************/ 00951 void STSPIN220::Stspin220_WaitWhileActive(void) 00952 { 00953 /* Wait while motor is running */ 00954 while (((Stspin220_GetDeviceState()&INACTIVE)!=INACTIVE)||\ 00955 (((Stspin220_GetDeviceState()&INACTIVE)==INACTIVE)&&(toggleOdd!=0))); 00956 } 00957 00958 /* ------------------------------------------------------------------------- */ 00959 /* Internal functions ------------------------------------------------------ */ 00960 /* ------------------------------------------------------------------------- */ 00961 /******************************************************//** 00962 * @brief Updates the current speed of the device 00963 * @param[in] newSpeed in pps 00964 * @retval None 00965 **********************************************************/ 00966 void STSPIN220::Stspin220_ApplySpeed(uint16_t newSpeed) 00967 { 00968 if (devicePrm.torqueBoostEnable != false) 00969 { 00970 if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256) 00971 { 00972 Stspin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED); 00973 } 00974 if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL) 00975 { 00976 if (((newSpeed>>devicePrm.stepModeLatched)>\ 00977 devicePrm.torqueBoostSpeedThreshold)&&\ 00978 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\ 00979 ((devicePrm.stepsToTake-devicePrm.relativePos)>=\ 00980 (1<<devicePrm.stepModeLatched)))) 00981 { 00982 if ((devicePrm.sequencerPosition & 0xFF) == 0X80) 00983 { 00984 Stspin220_Board_SetFullStep(); 00985 devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL; 00986 devicePrm.accu >>= devicePrm.stepModeLatched; 00987 newSpeed >>= devicePrm.stepModeLatched; 00988 } 00989 } 00990 } 00991 else if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\ 00992 (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\ 00993 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) == MOVE_CMD)&&\ 00994 ((devicePrm.stepsToTake-devicePrm.relativePos)<=\ 00995 (1<<devicePrm.stepModeLatched)))) 00996 { 00997 Stspin220_Board_UnsetFullStep(); 00998 devicePrm.stepMode = devicePrm.stepModeLatched; 00999 devicePrm.accu <<= devicePrm.stepModeLatched; 01000 newSpeed <<= devicePrm.stepModeLatched; 01001 } 01002 } 01003 else if (devicePrm.stepMode != devicePrm.stepModeLatched) 01004 { 01005 //torqueBoostEnable has just been disabled 01006 Stspin220_Board_UnsetFullStep(); 01007 devicePrm.stepMode = devicePrm.stepModeLatched; 01008 devicePrm.accu <<= devicePrm.stepModeLatched; 01009 newSpeed <<= devicePrm.stepModeLatched; 01010 } 01011 01012 if (newSpeed < STSPIN220_MIN_STCK_FREQ) 01013 { 01014 newSpeed = STSPIN220_MIN_STCK_FREQ; 01015 } 01016 if (newSpeed > STSPIN220_MAX_STCK_FREQ) 01017 { 01018 newSpeed = STSPIN220_MAX_STCK_FREQ; 01019 } 01020 01021 devicePrm.speed = newSpeed; 01022 Stspin220_Board_TimStckSetFreq(newSpeed); 01023 01024 } 01025 01026 /******************************************************//** 01027 * @brief Computes the speed profile according to the number of steps to move 01028 * @param[in] nbSteps number of steps to perform 01029 * @retval None 01030 * @note Using the acceleration and deceleration of the device, 01031 * this function determines the duration in steps of the acceleration, 01032 * steady and deceleration phases. 01033 * If the total number of steps to perform is big enough, a trapezoidal move 01034 * is performed (i.e. there is a steady phase where the motor runs at the maximum 01035 * speed. 01036 * Else, a triangular move is performed (no steady phase: the maximum speed is never 01037 * reached. 01038 **********************************************************/ 01039 void STSPIN220::Stspin220_ComputeSpeedProfile(uint32_t nbSteps) 01040 { 01041 uint32_t reqAccSteps; 01042 uint32_t reqDecSteps; 01043 01044 /* compute the number of steps to get the targeted speed */ 01045 uint16_t minSpeed = devicePrm.minSpeed; 01046 reqAccSteps = (devicePrm.maxSpeed - minSpeed); 01047 reqAccSteps *= (devicePrm.maxSpeed + minSpeed); 01048 reqDecSteps = reqAccSteps; 01049 reqAccSteps /= (uint32_t)devicePrm.acceleration; 01050 reqAccSteps /= 2; 01051 01052 /* compute the number of steps to stop */ 01053 reqDecSteps /= (uint32_t)devicePrm.deceleration; 01054 reqDecSteps /= 2; 01055 01056 if(( reqAccSteps + reqDecSteps ) > nbSteps) 01057 { 01058 /* Triangular move */ 01059 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 01060 uint32_t dec = devicePrm.deceleration; 01061 uint32_t acc = devicePrm.acceleration; 01062 01063 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); 01064 if (reqDecSteps > 1) 01065 { 01066 reqAccSteps = reqDecSteps - 1; 01067 if(reqAccSteps == 0) 01068 { 01069 reqAccSteps = 1; 01070 } 01071 } 01072 else 01073 { 01074 reqAccSteps = 0; 01075 } 01076 devicePrm.endAccPos = reqAccSteps; 01077 devicePrm.startDecPos = reqDecSteps; 01078 } 01079 else 01080 { 01081 /* Trapezoidal move */ 01082 /* accelerating phase to endAccPos */ 01083 /* steady phase from endAccPos to startDecPos */ 01084 /* decelerating from startDecPos to stepsToTake*/ 01085 devicePrm.endAccPos = reqAccSteps; 01086 devicePrm.startDecPos = nbSteps - reqDecSteps - 1; 01087 } 01088 } 01089 01090 /******************************************************//** 01091 * @brief Set the parameters of the device whose values are not defined in 01092 * stspin220_target_config.h 01093 * @retval None 01094 **********************************************************/ 01095 void STSPIN220::Stspin220_SetDeviceParamsOtherValues(void) 01096 { 01097 uint16_t tmp; 01098 01099 devicePrm.accu = 0; 01100 devicePrm.currentPosition = 0; 01101 devicePrm.sequencerPosition = 0; 01102 devicePrm.endAccPos = 0; 01103 devicePrm.relativePos = 0; 01104 devicePrm.startDecPos = 0; 01105 devicePrm.stepsToTake = 0; 01106 devicePrm.updateTorque = false; 01107 devicePrm.speed = 0; 01108 devicePrm.commandExecuted = NO_CMD; 01109 devicePrm.direction = FORWARD; 01110 tmp = devicePrm.minSpeed; 01111 if (((devicePrm.torqueBoostEnable != false)&&\ 01112 (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\ 01113 (tmp>devicePrm.maxSpeed)) 01114 { 01115 Stspin220_ErrorHandler(STSPIN220_ERROR_INIT); 01116 } 01117 } 01118 01119 /******************************************************//** 01120 * @brief Set the parameters of the device to values of the structure pointed 01121 * by pInitDevicePrm. Set GPIO according to these values. 01122 * @param pInitDevicePrm pointer onto the structure containing values to 01123 * initialize the device parameters. 01124 * @retval None 01125 **********************************************************/ 01126 void STSPIN220::Stspin220_SetDeviceParamsToGivenValues(Stspin220_Init_t* pInitDevicePrm) 01127 { 01128 devicePrm.motionState = STANDBY; 01129 01130 if (Stspin220_SetAcceleration(pInitDevicePrm->acceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); 01131 if (Stspin220_SetDeceleration(pInitDevicePrm->deceleration)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); 01132 if (Stspin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); 01133 if (Stspin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); 01134 01135 Stspin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq); 01136 Stspin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque); 01137 Stspin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque); 01138 Stspin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque); 01139 Stspin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque); 01140 devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable; 01141 devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold; 01142 Stspin220_SetStopMode(pInitDevicePrm->stopMode); 01143 01144 Stspin220_SetDeviceParamsOtherValues(); 01145 01146 /* Set predefined step mode */ 01147 /* Standby-reset deactivation included to latch the MODEX inputs */ 01148 Stspin220_SetStepMode(pInitDevicePrm->stepMode); 01149 } 01150 01151 /******************************************************//** 01152 * @brief Sets the parameters of the device to predefined values 01153 * from stspin220_target_config.h 01154 * @retval None 01155 **********************************************************/ 01156 void STSPIN220::Stspin220_SetDeviceParamsToPredefinedValues(void) 01157 { 01158 devicePrm.motionState = STANDBY; 01159 01160 if (Stspin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); 01161 if (Stspin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); 01162 if (Stspin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); 01163 if (Stspin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) Stspin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); 01164 01165 Stspin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY); 01166 Stspin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE); 01167 Stspin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE); 01168 Stspin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE); 01169 Stspin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE); 01170 devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN; 01171 devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH; 01172 Stspin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP); 01173 01174 Stspin220_SetDeviceParamsOtherValues(); 01175 01176 /* Set predefined step mode */ 01177 /* Standby-reset deactivation included to latch the MODEX inputs */ 01178 Stspin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE); 01179 } 01180 01181 /******************************************************//** 01182 * @brief Set the stepping mode without reset 01183 * @param[in] stepMode from full step to 1/256 microstep 01184 * as specified in enum motorStepMode_t 01185 * @retval true if the command is successfully executed, else false 01186 **********************************************************/ 01187 bool STSPIN220::Stspin220_SetStepModeWithoutReset(motorStepMode_t stepMode) 01188 { 01189 /* Store step mode */ 01190 devicePrm.stepMode = stepMode; 01191 devicePrm.stepModeLatched = stepMode; 01192 01193 /* Set the mode pins to the levels corresponding to the selected step mode */ 01194 switch (stepMode) 01195 { 01196 case STEP_MODE_FULL: 01197 Stspin220_Board_SetFullStep(); 01198 break; 01199 case STEP_MODE_HALF: 01200 Stspin220_Board_SetModePins(1, 0, 1, 0); 01201 break; 01202 case STEP_MODE_1_4: 01203 Stspin220_Board_SetModePins(0, 1, 0, 1); 01204 break; 01205 case STEP_MODE_1_8: 01206 Stspin220_Board_SetModePins(1, 1, 1, 0); 01207 break; 01208 case STEP_MODE_1_16: 01209 Stspin220_Board_SetModePins(1, 1, 1, 1); 01210 break; 01211 case STEP_MODE_1_32: 01212 Stspin220_Board_SetModePins(0, 0, 0, 1); 01213 break; 01214 case STEP_MODE_1_64: 01215 Stspin220_Board_SetModePins(1, 1, 0, 1); 01216 break; 01217 case STEP_MODE_1_128: 01218 Stspin220_Board_SetModePins(0, 0, 1, 0); 01219 break; 01220 case STEP_MODE_1_256: 01221 Stspin220_Board_SetModePins(1, 1, 0, 0); 01222 break; 01223 default: 01224 return false; 01225 } 01226 01227 /* Wait */ 01228 Stspin220_Board_Delay(SELECT_STEP_MODE_DELAY); 01229 01230 /* Exit standby, selected step mode is latched */ 01231 Stspin220_Board_ReleaseReset(); 01232 01233 /* Let a delay after reset release and step mode latching*/ 01234 Stspin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); 01235 01236 /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */ 01237 /* after the device quit the standby condition */ 01238 if (stepMode!=(motorStepMode_t)STEP_MODE_FULL) 01239 { 01240 Stspin220_Board_UnsetFullStep(); 01241 } 01242 01243 return true; 01244 } 01245 01246 /******************************************************//** 01247 * @brief Initialises the bridge parameters to start the movement 01248 * and enable the power bridge 01249 * @retval None 01250 **********************************************************/ 01251 void STSPIN220::Stspin220_StartMovement(void) 01252 { 01253 /* Enable STSPIN220 powerstage */ 01254 Stspin220_Enable(); 01255 toggleOdd = 0; 01256 devicePrm.accu = 0; 01257 devicePrm.relativePos = 0; 01258 Stspin220_Board_TimStckInit(true); 01259 if ((devicePrm.endAccPos == 0)&&\ 01260 (devicePrm.commandExecuted != RUN_CMD)) 01261 { 01262 devicePrm.motionState = DECELERATING; 01263 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE); 01264 } 01265 else 01266 { 01267 devicePrm.motionState = ACCELERATING; 01268 Stspin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE); 01269 } 01270 /* Program the step clock */ 01271 Stspin220_ApplySpeed(devicePrm.minSpeed); 01272 } 01273 01274 /******************************************************//** 01275 * @brief Handles the device state machine at each pulse 01276 * @retval None 01277 * @note Must only be called by the timer ISR 01278 **********************************************************/ 01279 void STSPIN220::Stspin220_StepClockHandler(void) 01280 { 01281 uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode; 01282 uint16_t tmp; 01283 Stspin220_Board_Monitor_Set(); 01284 if (devicePrm.motionState == STANDBYTOINACTIVE) 01285 { 01286 if (toggleOdd != 0) 01287 { 01288 Stspin220_Board_StckMode3_Reset(); 01289 toggleOdd = 0; 01290 if (devicePrm.sequencerPosition == 0) 01291 { 01292 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) 01293 { 01294 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 01295 } 01296 return; 01297 } 01298 } 01299 else 01300 { 01301 Stspin220_Board_StckMode3_Set(); 01302 toggleOdd = 1; 01303 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01304 devicePrm.sequencerPosition -= tmp; 01305 } 01306 Stspin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); 01307 return; 01308 } 01309 01310 if (toggleOdd == 0) 01311 { 01312 Stspin220_Board_StckMode3_Set(); 01313 toggleOdd = 1; 01314 } 01315 else 01316 { 01317 Stspin220_Board_StckMode3_Reset(); 01318 toggleOdd = 0; 01319 /* Incrementation of the relative position */ 01320 devicePrm.relativePos += (1 << stepModeShift); 01321 01322 /* Incrementation of the current position */ 01323 if (devicePrm.direction != BACKWARD) 01324 { 01325 devicePrm.currentPosition += (1 << stepModeShift); 01326 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01327 devicePrm.sequencerPosition += tmp; 01328 if (devicePrm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1)) 01329 { 01330 devicePrm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1); 01331 } 01332 } 01333 else 01334 { 01335 devicePrm.currentPosition -= (1 << stepModeShift); 01336 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01337 devicePrm.sequencerPosition -= tmp; 01338 if (devicePrm.sequencerPosition < 0) 01339 { 01340 devicePrm.sequencerPosition += (SEQUENCER_MAX_VALUE+1); 01341 } 01342 } 01343 01344 switch (devicePrm.motionState) 01345 { 01346 case ACCELERATING: 01347 { 01348 uint32_t relPos = devicePrm.relativePos; 01349 uint32_t endAccPos = devicePrm.endAccPos; 01350 uint16_t speed = devicePrm.speed; 01351 uint32_t acc = ((uint32_t)devicePrm.acceleration << 16)>>stepModeShift; 01352 01353 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ 01354 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos))) 01355 { 01356 devicePrm.motionState = DECELERATING; 01357 devicePrm.accu = 0; 01358 /* Apply decelerating torque */ 01359 Stspin220_ApplyTorque(DEC_TORQUE); 01360 } 01361 else if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\ 01362 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos))) 01363 { 01364 devicePrm.motionState = STEADY; 01365 /* Apply running torque */ 01366 Stspin220_ApplyTorque(RUN_TORQUE); 01367 } 01368 else 01369 { 01370 bool speedUpdated = false; 01371 /* Go on accelerating */ 01372 if (speed==0) speed =1; 01373 devicePrm.accu += acc / speed; 01374 while (devicePrm.accu>=(0X10000L)) 01375 { 01376 devicePrm.accu -= (0X10000L); 01377 speed +=1; 01378 speedUpdated = true; 01379 } 01380 01381 if (speedUpdated) 01382 { 01383 if (speed>(devicePrm.maxSpeed>>stepModeShift)) 01384 { 01385 speed = devicePrm.maxSpeed>>stepModeShift; 01386 } 01387 devicePrm.speed = speed; 01388 } 01389 01390 if (devicePrm.updateTorque!=false) 01391 { 01392 /* Apply accelerating torque */ 01393 Stspin220_ApplyTorque(ACC_TORQUE); 01394 } 01395 } 01396 break; 01397 } 01398 case STEADY: 01399 { 01400 uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift; 01401 uint32_t relativePos = devicePrm.relativePos; 01402 if (devicePrm.updateTorque!=false) 01403 { 01404 /* Apply accelerating torque */ 01405 Stspin220_ApplyTorque(RUN_TORQUE); 01406 } 01407 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ 01408 ((devicePrm.commandExecuted==MOVE_CMD)&&\ 01409 (relativePos>=(devicePrm.startDecPos)))||\ 01410 ((devicePrm.commandExecuted==RUN_CMD)&&\ 01411 (devicePrm.speed>maxSpeed))) 01412 { 01413 devicePrm.motionState = DECELERATING; 01414 devicePrm.accu = 0; 01415 /* Apply decelerating torque */ 01416 Stspin220_ApplyTorque(DEC_TORQUE); 01417 } 01418 else if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed)) 01419 { 01420 devicePrm.motionState = ACCELERATING; 01421 devicePrm.accu = 0; 01422 /* Apply accelerating torque */ 01423 Stspin220_ApplyTorque(ACC_TORQUE); 01424 } 01425 break; 01426 } 01427 case DECELERATING: 01428 { 01429 uint32_t relativePos = devicePrm.relativePos; 01430 uint16_t speed = devicePrm.speed; 01431 uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift; 01432 if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\ 01433 (speed<=(devicePrm.minSpeed>>stepModeShift)))||\ 01434 ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake))) 01435 { 01436 /* Motion process complete */ 01437 if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0) 01438 { 01439 devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted); 01440 if (devicePrm.direction==BACKWARD) devicePrm.direction=FORWARD; 01441 else devicePrm.direction=BACKWARD; 01442 Stspin220_Board_SetDirectionGpio(devicePrm.direction); 01443 if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0) 01444 { 01445 devicePrm.motionState = ACCELERATING; 01446 devicePrm.accu = 0; 01447 /* Apply accelerating torque */ 01448 Stspin220_ApplyTorque(ACC_TORQUE); 01449 break; 01450 } 01451 } 01452 if (devicePrm.stopMode==HOLD_MODE) 01453 { 01454 Stspin220_HardStop(); 01455 } 01456 else if (devicePrm.stopMode==STANDBY_MODE) 01457 { 01458 Stspin220_PutDeviceInStandby(); 01459 } 01460 else 01461 { 01462 Stspin220_HardHiZ(); 01463 } 01464 } 01465 else if ((devicePrm.commandExecuted==RUN_CMD)&& 01466 (speed<=(devicePrm.maxSpeed>>stepModeShift))) 01467 { 01468 devicePrm.motionState = STEADY; 01469 /* Apply running torque */ 01470 Stspin220_ApplyTorque(RUN_TORQUE); 01471 } 01472 else 01473 { 01474 /* Go on decelerating */ 01475 if (speed>(devicePrm.minSpeed>>stepModeShift)) 01476 { 01477 bool speedUpdated = false; 01478 if (speed==0) speed =1; 01479 devicePrm.accu += dec / speed; 01480 while (devicePrm.accu>=(0X10000L)) 01481 { 01482 devicePrm.accu -= (0X10000L); 01483 if (speed>1) 01484 { 01485 speed -=1; 01486 } 01487 speedUpdated = true; 01488 } 01489 01490 if (speedUpdated) 01491 { 01492 if (speed<(devicePrm.minSpeed>>stepModeShift)) 01493 { 01494 speed = devicePrm.minSpeed>>stepModeShift; 01495 } 01496 devicePrm.speed = speed; 01497 } 01498 01499 if (devicePrm.updateTorque!=false) 01500 { 01501 /* Apply decelerating torque */ 01502 Stspin220_ApplyTorque(DEC_TORQUE); 01503 } 01504 } 01505 } 01506 break; 01507 } 01508 default: 01509 { 01510 break; 01511 } 01512 } 01513 } 01514 if ((devicePrm.motionState & INACTIVE) != INACTIVE) 01515 { 01516 Stspin220_ApplySpeed(devicePrm.speed); 01517 } 01518 else 01519 { 01520 if (Stspin220_Board_TimStckStop(&toggleOdd) == 0) 01521 { 01522 Stspin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 01523 } 01524 } 01525 Stspin220_Board_Monitor_Reset(); 01526 } 01527 01528 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Fri Jul 15 2022 21:58:35 by
1.7.2