ST
Library to handle the X-NUCLEO-IHM06A1 Motor Control Expansion Board based on the STSPIN220 component.
Dependencies: ST_INTERFACES
Dependents: HelloWorld_IHM06A1
Fork of
X-NUCLEO-IHM06A1
by 
ST Expansion SW Team
Embed:
(wiki syntax)
Show/hide line numbers
STSpin220.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file STSpin220.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.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_get_device_state(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 00404 { 00405 if (devicePrm.motionState != INACTIVE) 00406 { 00407 STSpin220_HardHiZ(); 00408 } 00409 } 00410 00411 if (targetPosition > devicePrm.currentPosition) 00412 { 00413 devicePrm.stepsToTake = targetPosition -\ 00414 devicePrm.currentPosition; 00415 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) 00416 { 00417 direction = FORWARD; 00418 } 00419 else 00420 { 00421 direction = BACKWARD; 00422 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ 00423 devicePrm.stepsToTake; 00424 } 00425 } 00426 else 00427 { 00428 devicePrm.stepsToTake = devicePrm.currentPosition -\ 00429 targetPosition; 00430 if (devicePrm.stepsToTake < (STSPIN220_POSITION_RANGE>>1)) 00431 { 00432 direction = BACKWARD; 00433 } 00434 else 00435 { 00436 direction = FORWARD; 00437 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE -\ 00438 devicePrm.stepsToTake; 00439 } 00440 } 00441 00442 if (devicePrm.stepsToTake != 0) 00443 { 00444 devicePrm.commandExecuted = MOVE_CMD; 00445 00446 /* Direction setup */ 00447 STSpin220_SetDirection(direction); 00448 00449 STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake); 00450 00451 /* Motor activation */ 00452 STSpin220_StartMovement(); 00453 } 00454 } 00455 00456 /******************************************************//** 00457 * @brief move the motor to the absolute position 00458 * @param[in] direction FORWARD or BACKWARD 00459 * @param[in] targetPosition 32 bit signed value position 00460 * @retval None 00461 * @note The position is at the resolution corresponding to the 00462 * selected step mode. 00463 * STEP_MODE_FULL : step 00464 * STEP_MODE_HALF : 1/2 step 00465 * STEP_MODE_1_4 : 1/4 step 00466 * STEP_MODE_1_8 : 1/8 step 00467 * STEP_MODE_1_16 : 1/16 step 00468 * STEP_MODE_1_32 : 1/32 step 00469 * STEP_MODE_1_64 : 1/64 step 00470 * STEP_MODE_1_128 : 1/128 step 00471 * STEP_MODE_1_256 : 1/256 step 00472 **********************************************************/ 00473 void STSpin220::STSpin220_GoToDir(motorDir_t direction, int32_t targetPosition) 00474 { 00475 /* Exit from standby if needed */ 00476 if (devicePrm.motionState == STANDBY) 00477 { 00478 STSpin220_ExitDeviceFromStandby(); 00479 } 00480 /* Deactivate motor if needed */ 00481 else 00482 { 00483 if (devicePrm.motionState != INACTIVE) 00484 { 00485 STSpin220_HardHiZ(); 00486 } 00487 } 00488 00489 if (direction != BACKWARD) 00490 { 00491 if (targetPosition > devicePrm.currentPosition) 00492 { 00493 devicePrm.stepsToTake = targetPosition -\ 00494 devicePrm.currentPosition; 00495 } 00496 else 00497 { 00498 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ 00499 (targetPosition -\ 00500 devicePrm.currentPosition); 00501 } 00502 } 00503 else 00504 { 00505 if (targetPosition > devicePrm.currentPosition) 00506 { 00507 devicePrm.stepsToTake = STSPIN220_POSITION_RANGE +\ 00508 (devicePrm.currentPosition -\ 00509 targetPosition); 00510 } 00511 else 00512 { 00513 devicePrm.stepsToTake = devicePrm.currentPosition -\ 00514 targetPosition; 00515 } 00516 } 00517 00518 if (devicePrm.stepsToTake != 0) 00519 { 00520 devicePrm.commandExecuted = MOVE_CMD; 00521 00522 /* Direction setup */ 00523 STSpin220_SetDirection(direction); 00524 00525 STSpin220_ComputeSpeedProfile(devicePrm.stepsToTake); 00526 00527 /* Motor activation */ 00528 STSpin220_StartMovement(); 00529 } 00530 } 00531 00532 /******************************************************//** 00533 * @brief Immediately stop the motor and disables the power bridges 00534 * @retval None 00535 **********************************************************/ 00536 void STSpin220::STSpin220_HardHiZ(void) 00537 { 00538 /* Set inactive state */ 00539 devicePrm.motionState = INACTIVE; 00540 00541 /* Disable step clock */ 00542 if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) 00543 { 00544 STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 00545 } 00546 00547 /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */ 00548 /* after the last step clock rising edge triggering the last step */ 00549 STSpin220_Board_Delay(DISABLE_DELAY); 00550 00551 /* Set reference voltage to 0 */ 00552 STSpin220_SetTorque(CURRENT_TORQUE, 0); 00553 00554 /* Disable power bridges */ 00555 STSpin220_Board_Disable(); 00556 00557 /* Comeback to nominal step mode */ 00558 if (devicePrm.stepModeLatched != devicePrm.stepMode) 00559 { 00560 STSpin220_Board_UnsetFullStep(); 00561 devicePrm.stepMode = devicePrm.stepModeLatched; 00562 } 00563 00564 devicePrm.commandExecuted = NO_CMD; 00565 devicePrm.stepsToTake = 0; 00566 devicePrm.speed = 0; 00567 } 00568 00569 /******************************************************//** 00570 * @brief Immediatly stop the motor 00571 * and either set holding torque when stop mode is HOLD_MODE, 00572 * or call STSpin220_HardHiz function when stop mode is HIZ_MODE, 00573 * or call STSpin220_PutDeviceInStandby function when stop mode is STANDBY_MODE 00574 * @retval None 00575 **********************************************************/ 00576 void STSpin220::STSpin220_HardStop(void) 00577 { 00578 if (devicePrm.stopMode == HOLD_MODE) 00579 { 00580 /* Set inactive state */ 00581 devicePrm.motionState = INACTIVE; 00582 00583 /* Disable step clock */ 00584 if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) 00585 { 00586 STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 00587 } 00588 00589 /* Set holding torque */ 00590 STSpin220_ApplyTorque(HOLD_TORQUE); 00591 00592 /* Comeback to nominal step mode */ 00593 if (devicePrm.stepModeLatched != devicePrm.stepMode) 00594 { 00595 STSpin220_Board_UnsetFullStep(); 00596 devicePrm.stepMode = devicePrm.stepModeLatched; 00597 } 00598 00599 devicePrm.commandExecuted = NO_CMD; 00600 devicePrm.stepsToTake = 0; 00601 devicePrm.speed = 0; 00602 } 00603 else if (devicePrm.stopMode == HIZ_MODE) 00604 { 00605 STSpin220_HardHiZ(); 00606 } 00607 else if (devicePrm.stopMode == STANDBY_MODE) 00608 { 00609 STSpin220_PutDeviceInStandby(); 00610 } 00611 } 00612 00613 /******************************************************//** 00614 * @brief Moves the motor of the specified number of steps 00615 * @param[in] direction FORWARD or BACKWARD 00616 * @param[in] stepCount Number of steps to perform 00617 * @retval None 00618 **********************************************************/ 00619 void STSpin220::STSpin220_Move(motorDir_t direction, uint32_t stepCount) 00620 { 00621 /* Exit from standby if needed */ 00622 if (devicePrm.motionState == STANDBY) 00623 { 00624 STSpin220_ExitDeviceFromStandby(); 00625 } 00626 /* Deactivate motor if needed */ 00627 else 00628 { 00629 if (devicePrm.motionState != INACTIVE) 00630 { 00631 STSpin220_HardHiZ(); 00632 } 00633 } 00634 00635 if (stepCount != 0) 00636 { 00637 devicePrm.stepsToTake = stepCount; 00638 devicePrm.commandExecuted = MOVE_CMD; 00639 00640 /* Direction setup */ 00641 STSpin220_SetDirection(direction); 00642 00643 STSpin220_ComputeSpeedProfile(stepCount); 00644 00645 /* Motor activation */ 00646 STSpin220_StartMovement(); 00647 } 00648 } 00649 00650 /******************************************************//** 00651 * @brief Put STSpin220 device in standby (low power consumption) 00652 * @retval None 00653 **********************************************************/ 00654 void STSpin220::STSpin220_PutDeviceInStandby(void) 00655 { 00656 /* Stop movement */ 00657 STSpin220_HardHiZ(); 00658 00659 /* Enter standby */ 00660 STSpin220_Board_Reset(); 00661 00662 devicePrm.motionState = STANDBY; 00663 } 00664 00665 /******************************************************//** 00666 * @brief Runs the motor. It will accelerate from the min 00667 * speed up to the max speed by using the device acceleration. 00668 * @param[in] direction FORWARD or BACKWARD 00669 * @retval None 00670 **********************************************************/ 00671 void STSpin220::STSpin220_Run(motorDir_t direction) 00672 { 00673 /* Exit from standby if needed */ 00674 if (devicePrm.motionState == STANDBY) 00675 { 00676 STSpin220_ExitDeviceFromStandby(); 00677 } 00678 /* Deactivate motor if needed */ 00679 else 00680 { 00681 if (devicePrm.motionState != INACTIVE) 00682 { 00683 STSpin220_HardHiZ(); 00684 } 00685 } 00686 00687 /* Direction setup */ 00688 STSpin220_SetDirection(direction); 00689 devicePrm.commandExecuted = RUN_CMD; 00690 /* Motor activation */ 00691 STSpin220_StartMovement(); 00692 } 00693 00694 /******************************************************//** 00695 * @brief Changes the acceleration 00696 * @param[in] newAcc New acceleration to apply in pps^2 00697 * @retval true if the command is successfully executed, else false 00698 * @note The command is not performed if the device is executing 00699 * a MOVE or GOTO command (but it can be used during a RUN command) 00700 **********************************************************/ 00701 bool STSpin220::STSpin220_SetAcceleration(uint16_t newAcc) 00702 { 00703 bool cmdExecuted = false; 00704 if ((newAcc != 0)&& 00705 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00706 (devicePrm.commandExecuted == RUN_CMD))) 00707 { 00708 devicePrm.acceleration = newAcc; 00709 cmdExecuted = true; 00710 } 00711 return cmdExecuted; 00712 } 00713 00714 /******************************************************//** 00715 * @brief Changes the deceleration 00716 * @param[in] newDec New deceleration to apply in pps^2 00717 * @retval true if the command is successfully executed, else false 00718 * @note The command is not performed if the device is executing 00719 * a MOVE or GOTO command (but it can be used during a RUN command) 00720 **********************************************************/ 00721 bool STSpin220::STSpin220_SetDeceleration(uint16_t newDec) 00722 { 00723 bool cmdExecuted = false; 00724 if ((newDec != 0)&& 00725 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00726 (devicePrm.commandExecuted == RUN_CMD))) 00727 { 00728 devicePrm.deceleration = newDec; 00729 cmdExecuted = true; 00730 } 00731 return cmdExecuted; 00732 } 00733 00734 /******************************************************//** 00735 * @brief Specifies the direction 00736 * @param[in] dir FORWARD or BACKWARD 00737 * @note The direction change is applied if the device 00738 * is in INACTIVE or STANDBY state or if the device is 00739 * executing a run command 00740 * @retval None 00741 **********************************************************/ 00742 void STSpin220::STSpin220_SetDirection(motorDir_t dir) 00743 { 00744 if ((devicePrm.motionState == INACTIVE)||\ 00745 (devicePrm.motionState == STANDBY)) 00746 { 00747 devicePrm.direction = dir; 00748 STSpin220_Board_SetDirectionGpio(dir); 00749 } 00750 else 00751 { 00752 if ((devicePrm.commandExecuted&RUN_CMD)!=0) 00753 { 00754 devicePrm.commandExecuted=(deviceCommand_t) 00755 (STSPIN220_DIR_CHANGE_BIT_MASK|devicePrm.commandExecuted); 00756 } 00757 } 00758 } 00759 00760 /******************************************************//** 00761 * @brief Set current position to be the Home position 00762 * (current position set to 0) 00763 * @retval None 00764 **********************************************************/ 00765 void STSpin220::STSpin220_SetHome(void) 00766 { 00767 devicePrm.currentPosition = 0; 00768 } 00769 00770 /******************************************************//** 00771 * @brief Set current position to be the Mark position 00772 * @retval None 00773 **********************************************************/ 00774 void STSpin220::STSpin220_SetMark(void) 00775 { 00776 devicePrm.markPosition = devicePrm.currentPosition; 00777 } 00778 00779 /******************************************************//** 00780 * @brief Changes the max speed 00781 * @param[in] newMaxSpeed New max speed to apply in pps 00782 * @retval true if the command is successfully executed, else false 00783 * @note The command is not performed is the device is executing 00784 * a MOVE or GOTO command (but it can be used during a RUN command). 00785 **********************************************************/ 00786 bool STSpin220::STSpin220_SetMaxSpeed(uint16_t newMaxSpeed) 00787 { 00788 bool cmdExecuted = false; 00789 if ((newMaxSpeed >= STSPIN220_MIN_STCK_FREQ)&&\ 00790 ((newMaxSpeed <= STSPIN220_MAX_STCK_FREQ)||\ 00791 ((devicePrm.torqueBoostEnable != false)&&\ 00792 ((newMaxSpeed>>STSpin220_GetStepMode())<= STSPIN220_MAX_STCK_FREQ)))&&\ 00793 (devicePrm.minSpeed <= newMaxSpeed) &&\ 00794 (((devicePrm.motionState & INACTIVE) == INACTIVE)||\ 00795 (devicePrm.commandExecuted == RUN_CMD))) 00796 { 00797 devicePrm.maxSpeed = newMaxSpeed; 00798 cmdExecuted = true; 00799 } 00800 return cmdExecuted; 00801 } 00802 00803 /******************************************************//** 00804 * @brief Changes the min speed 00805 * @param[in] newMinSpeed New min speed to apply in pps 00806 * @retval true if the command is successfully executed, else false 00807 * @note The command is not performed is the device is executing 00808 * a MOVE or GOTO command (but it can be used during a RUN command). 00809 **********************************************************/ 00810 bool STSpin220::STSpin220_SetMinSpeed(uint16_t newMinSpeed) 00811 { 00812 bool cmdExecuted = false; 00813 if ((newMinSpeed >= STSPIN220_MIN_STCK_FREQ)&& 00814 (newMinSpeed <= STSPIN220_MAX_STCK_FREQ) && 00815 (newMinSpeed <= devicePrm.maxSpeed) && 00816 (((devicePrm.motionState & INACTIVE) == INACTIVE)|| 00817 (devicePrm.commandExecuted == RUN_CMD))) 00818 { 00819 devicePrm.minSpeed = newMinSpeed; 00820 cmdExecuted = true; 00821 } 00822 return cmdExecuted; 00823 } 00824 00825 /******************************************************//** 00826 * @brief Set the stepping mode 00827 * @param[in] stepMode from full step to 1/256 microstep 00828 * as specified in enum motorStepMode_t 00829 * @retval true if the command is successfully executed, else false 00830 **********************************************************/ 00831 bool STSpin220::STSpin220_SetStepMode(motorStepMode_t stepMode) 00832 { 00833 /* Eventually deactivate motor */ 00834 if ((devicePrm.motionState != INACTIVE)&&\ 00835 (devicePrm.motionState != STANDBY)) 00836 { 00837 STSpin220_HardHiZ(); 00838 } 00839 00840 /* Enter standby */ 00841 STSpin220_Board_Reset(); 00842 00843 /* Reset the microstepping sequencer position */ 00844 devicePrm.sequencerPosition = 0; 00845 00846 /* Reset current and mark positions */ 00847 devicePrm.currentPosition = 0; 00848 devicePrm.markPosition = 0; 00849 00850 /* Set the step mode */ 00851 return (STSpin220_SetStepModeWithoutReset(stepMode)); 00852 } 00853 00854 /******************************************************//** 00855 * @brief Select the mode to stop the motor. 00856 * @param[in] stopMode HOLD_MODE to let power bridge enabled 00857 * @retval None 00858 **********************************************************/ 00859 void STSpin220::STSpin220_SetStopMode(motorStopMode_t stopMode) 00860 { 00861 devicePrm.stopMode = stopMode; 00862 } 00863 00864 /******************************************************//** 00865 * @brief Set the torque 00866 * @param[in] torqueMode Torque mode as specified in enum motorTorqueMode_t 00867 * @param[in] torqueValue in % (from 0 to 100) 00868 * @retval None 00869 * @note 00870 **********************************************************/ 00871 void STSpin220::STSpin220_SetTorque(motorTorqueMode_t torqueMode, uint8_t torqueValue) 00872 { 00873 devicePrm.updateTorque = true; 00874 if (torqueValue>100) torqueValue = 100; 00875 switch(torqueMode) 00876 { 00877 case ACC_TORQUE: 00878 devicePrm.accelTorque = torqueValue; 00879 break; 00880 case DEC_TORQUE: 00881 devicePrm.decelTorque = torqueValue; 00882 break; 00883 case RUN_TORQUE: 00884 devicePrm.runTorque = torqueValue; 00885 break; 00886 case HOLD_TORQUE: 00887 devicePrm.holdTorque = torqueValue; 00888 if (devicePrm.motionState != INACTIVE) 00889 { 00890 break; 00891 } 00892 case CURRENT_TORQUE: 00893 devicePrm.currentTorque = torqueValue; 00894 STSpin220_Board_PwmRefSetDutyCycle(torqueValue); 00895 default: 00896 devicePrm.updateTorque = false; 00897 break; //ignore error 00898 } 00899 } 00900 00901 /******************************************************//** 00902 * @brief Enable or disable the torque boost feature 00903 * @param[in] enable true to enable torque boost, false to disable 00904 * @retval None 00905 **********************************************************/ 00906 void STSpin220::STSpin220_SetTorqueBoostEnable(bool enable) 00907 { 00908 devicePrm.torqueBoostEnable = enable; 00909 } 00910 00911 /******************************************************//** 00912 * @brief Set the torque boost threshold 00913 * @param[in] speedThreshold speed threshold above which the step mode is 00914 * changed to full step 00915 * @retval None 00916 **********************************************************/ 00917 void STSpin220::STSpin220_SetTorqueBoostThreshold(uint16_t speedThreshold) 00918 { 00919 devicePrm.torqueBoostSpeedThreshold = speedThreshold; 00920 } 00921 00922 /******************************************************//** 00923 * @brief Stops the motor by using the device deceleration 00924 * @retval true if the command is successfully executed, else false 00925 * @note The command is not performed if the device is in INACTIVE, 00926 * STANDBYTOINACTIVE or STANDBY state. 00927 **********************************************************/ 00928 bool STSpin220::STSpin220_SoftStop(void) 00929 { 00930 bool cmdExecuted = false; 00931 if ((devicePrm.motionState & INACTIVE) != INACTIVE) 00932 { 00933 devicePrm.commandExecuted=(deviceCommand_t) 00934 (STSPIN220_SOFT_STOP_BIT_MASK|devicePrm.commandExecuted); 00935 cmdExecuted = true; 00936 } 00937 return (cmdExecuted); 00938 } 00939 00940 /******************************************************//** 00941 * @brief Get the frequency of REF PWM 00942 * @retval the frequency of REF PWM in Hz 00943 * @note 00944 **********************************************************/ 00945 uint32_t STSpin220::STSpin220_VrefPwmGetFreq(void) 00946 { 00947 return devicePrm.refPwmFreq; 00948 } 00949 00950 /******************************************************//** 00951 * @brief Set the frequency of REF PWM 00952 * @param[in] newFreq in Hz 00953 * @retval None 00954 * @note 00955 **********************************************************/ 00956 void STSpin220::STSpin220_VrefPwmSetFreq(uint32_t newFreq) 00957 { 00958 devicePrm.refPwmFreq = newFreq; 00959 STSpin220_Board_PwmRefSetFreq(newFreq); 00960 } 00961 00962 /******************************************************//** 00963 * @brief Locks until the device state becomes Inactive 00964 * @retval None 00965 **********************************************************/ 00966 void STSpin220::STSpin220_WaitWhileActive(void) 00967 { 00968 /* Wait while motor is running */ 00969 while (((STSpin220_get_device_state()&INACTIVE)!=INACTIVE)||\ 00970 (((STSpin220_get_device_state()&INACTIVE)==INACTIVE)&&(toggleOdd!=0))); 00971 } 00972 00973 /* ------------------------------------------------------------------------- */ 00974 /* Internal functions ------------------------------------------------------ */ 00975 /* ------------------------------------------------------------------------- */ 00976 /******************************************************//** 00977 * @brief Updates the current speed of the device 00978 * @param[in] newSpeed in pps 00979 * @retval None 00980 **********************************************************/ 00981 void STSpin220::STSpin220_ApplySpeed(uint16_t newSpeed) 00982 { 00983 if (devicePrm.torqueBoostEnable != false) 00984 { 00985 if (devicePrm.stepMode > (motorStepMode_t)STEP_MODE_1_256) 00986 { 00987 STSpin220_ErrorHandler(STSPIN220_ERROR_APPLY_SPEED); 00988 } 00989 if (devicePrm.stepMode != (motorStepMode_t)STEP_MODE_FULL) 00990 { 00991 if (((newSpeed>>devicePrm.stepModeLatched)>\ 00992 devicePrm.torqueBoostSpeedThreshold)&&\ 00993 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) != MOVE_CMD) ||\ 00994 ((devicePrm.stepsToTake-devicePrm.relativePos)>=\ 00995 (1<<devicePrm.stepModeLatched)))) 00996 { 00997 if ((devicePrm.sequencerPosition & 0xFF) == 0X80) 00998 { 00999 STSpin220_Board_SetFullStep(); 01000 devicePrm.stepMode = (motorStepMode_t)STEP_MODE_FULL; 01001 devicePrm.accu >>= devicePrm.stepModeLatched; 01002 newSpeed >>= devicePrm.stepModeLatched; 01003 } 01004 } 01005 } 01006 else 01007 { 01008 if (((newSpeed <= devicePrm.torqueBoostSpeedThreshold) &&\ 01009 (devicePrm.stepModeLatched != (motorStepMode_t)STEP_MODE_FULL))||\ 01010 (((devicePrm.commandExecuted & STSPIN220_MOVE_BIT_MASK) == MOVE_CMD)&&\ 01011 ((devicePrm.stepsToTake-devicePrm.relativePos)<=\ 01012 (1<<devicePrm.stepModeLatched)))) 01013 { 01014 STSpin220_Board_UnsetFullStep(); 01015 devicePrm.stepMode = devicePrm.stepModeLatched; 01016 devicePrm.accu <<= devicePrm.stepModeLatched; 01017 newSpeed <<= devicePrm.stepModeLatched; 01018 } 01019 } 01020 } 01021 else 01022 { 01023 if (devicePrm.stepMode != devicePrm.stepModeLatched) 01024 { 01025 //torqueBoostEnable has just been disabled 01026 STSpin220_Board_UnsetFullStep(); 01027 devicePrm.stepMode = devicePrm.stepModeLatched; 01028 devicePrm.accu <<= devicePrm.stepModeLatched; 01029 newSpeed <<= devicePrm.stepModeLatched; 01030 } 01031 } 01032 01033 if (newSpeed < STSPIN220_MIN_STCK_FREQ) 01034 { 01035 newSpeed = STSPIN220_MIN_STCK_FREQ; 01036 } 01037 if (newSpeed > STSPIN220_MAX_STCK_FREQ) 01038 { 01039 newSpeed = STSPIN220_MAX_STCK_FREQ; 01040 } 01041 01042 devicePrm.speed = newSpeed; 01043 STSpin220_Board_TimStckSetFreq(newSpeed); 01044 } 01045 01046 /******************************************************//** 01047 * @brief Computes the speed profile according to the number of steps to move 01048 * @param[in] nbSteps number of steps to perform 01049 * @retval None 01050 * @note Using the acceleration and deceleration of the device, 01051 * this function determines the duration in steps of the acceleration, 01052 * steady and deceleration phases. 01053 * If the total number of steps to perform is big enough, a trapezoidal move 01054 * is performed (i.e. there is a steady phase where the motor runs at the maximum 01055 * speed. 01056 * Else, a triangular move is performed (no steady phase: the maximum speed is never 01057 * reached. 01058 **********************************************************/ 01059 void STSpin220::STSpin220_ComputeSpeedProfile(uint32_t nbSteps) 01060 { 01061 uint32_t reqAccSteps; 01062 uint32_t reqDecSteps; 01063 01064 /* compute the number of steps to get the targeted speed */ 01065 uint16_t minSpeed = devicePrm.minSpeed; 01066 reqAccSteps = (devicePrm.maxSpeed - minSpeed); 01067 reqAccSteps *= (devicePrm.maxSpeed + minSpeed); 01068 reqDecSteps = reqAccSteps; 01069 reqAccSteps /= (uint32_t)devicePrm.acceleration; 01070 reqAccSteps /= 2; 01071 01072 /* compute the number of steps to stop */ 01073 reqDecSteps /= (uint32_t)devicePrm.deceleration; 01074 reqDecSteps /= 2; 01075 01076 if(( reqAccSteps + reqDecSteps ) > nbSteps) 01077 { 01078 /* Triangular move */ 01079 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 01080 uint32_t dec = devicePrm.deceleration; 01081 uint32_t acc = devicePrm.acceleration; 01082 01083 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); 01084 if (reqDecSteps > 1) 01085 { 01086 reqAccSteps = reqDecSteps - 1; 01087 if(reqAccSteps == 0) 01088 { 01089 reqAccSteps = 1; 01090 } 01091 } 01092 else 01093 { 01094 reqAccSteps = 0; 01095 } 01096 devicePrm.endAccPos = reqAccSteps; 01097 devicePrm.startDecPos = reqDecSteps; 01098 } 01099 else 01100 { 01101 /* Trapezoidal move */ 01102 /* accelerating phase to endAccPos */ 01103 /* steady phase from endAccPos to startDecPos */ 01104 /* decelerating from startDecPos to stepsToTake*/ 01105 devicePrm.endAccPos = reqAccSteps; 01106 devicePrm.startDecPos = nbSteps - reqDecSteps - 1; 01107 } 01108 } 01109 01110 /******************************************************//** 01111 * @brief Set the parameters of the device whose values are not defined in 01112 * stspin220_target_config.h 01113 * @retval None 01114 **********************************************************/ 01115 void STSpin220::STSpin220_SetDeviceParamsOtherValues(void) 01116 { 01117 uint16_t tmp; 01118 01119 devicePrm.accu = 0; 01120 devicePrm.currentPosition = 0; 01121 devicePrm.sequencerPosition = 0; 01122 devicePrm.endAccPos = 0; 01123 devicePrm.relativePos = 0; 01124 devicePrm.startDecPos = 0; 01125 devicePrm.stepsToTake = 0; 01126 devicePrm.updateTorque = false; 01127 devicePrm.speed = 0; 01128 devicePrm.commandExecuted = NO_CMD; 01129 devicePrm.direction = FORWARD; 01130 tmp = devicePrm.minSpeed; 01131 if (((devicePrm.torqueBoostEnable != false)&&\ 01132 (devicePrm.torqueBoostSpeedThreshold>STSPIN220_MAX_STCK_FREQ))||\ 01133 (tmp>devicePrm.maxSpeed)) 01134 { 01135 STSpin220_ErrorHandler(STSPIN220_ERROR_INIT); 01136 } 01137 } 01138 01139 /******************************************************//** 01140 * @brief Set the parameters of the device to values of the structure pointed 01141 * by pInitDevicePrm. Set GPIO according to these values. 01142 * @param pInitDevicePrm pointer onto the structure containing values to 01143 * initialize the device parameters. 01144 * @retval None 01145 **********************************************************/ 01146 void STSpin220::STSpin220_SetDeviceParamsToGivenValues(STSpin220_init_t* pInitDevicePrm) 01147 { 01148 devicePrm.motionState = STANDBY; 01149 01150 if (STSpin220_SetAcceleration(pInitDevicePrm->acceleration)==false) 01151 { 01152 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); 01153 } 01154 if (STSpin220_SetDeceleration(pInitDevicePrm->deceleration)==false) 01155 { 01156 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); 01157 } 01158 if (STSpin220_SetMaxSpeed(pInitDevicePrm->maxSpeed)==false) 01159 { 01160 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); 01161 } 01162 if (STSpin220_SetMinSpeed(pInitDevicePrm->minSpeed)==false) 01163 { 01164 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); 01165 } 01166 01167 STSpin220_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq); 01168 STSpin220_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque); 01169 STSpin220_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque); 01170 STSpin220_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque); 01171 STSpin220_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque); 01172 devicePrm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable; 01173 devicePrm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold; 01174 STSpin220_SetStopMode(pInitDevicePrm->stopMode); 01175 01176 STSpin220_SetDeviceParamsOtherValues(); 01177 01178 /* Set predefined step mode */ 01179 /* Standby-reset deactivation included to latch the MODEX inputs */ 01180 STSpin220_SetStepMode(pInitDevicePrm->stepMode); 01181 } 01182 01183 /******************************************************//** 01184 * @brief Sets the parameters of the device to predefined values 01185 * from stspin220_target_config.h 01186 * @retval None 01187 **********************************************************/ 01188 void STSpin220::STSpin220_SetDeviceParamsToPredefinedValues(void) 01189 { 01190 devicePrm.motionState = STANDBY; 01191 01192 if (STSpin220_SetAcceleration(STSPIN220_CONF_PARAM_ACC)==false) 01193 { 01194 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_ACCELERATION); 01195 } 01196 if (STSpin220_SetDeceleration(STSPIN220_CONF_PARAM_DEC)==false) 01197 { 01198 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_DECELERATION); 01199 } 01200 if (STSpin220_SetMaxSpeed(STSPIN220_CONF_PARAM_RUNNING_SPEED)==false) 01201 { 01202 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MAX_SPEED); 01203 } 01204 if (STSpin220_SetMinSpeed(STSPIN220_CONF_PARAM_MIN_SPEED)==false) 01205 { 01206 STSpin220_ErrorHandler(STSPIN220_ERROR_SET_MIN_SPEED); 01207 } 01208 01209 STSpin220_VrefPwmSetFreq(STSPIN220_CONF_PARAM_REF_PWM_FREQUENCY); 01210 STSpin220_SetTorque(ACC_TORQUE,STSPIN220_CONF_PARAM_ACC_TORQUE); 01211 STSpin220_SetTorque(DEC_TORQUE,STSPIN220_CONF_PARAM_DEC_TORQUE); 01212 STSpin220_SetTorque(RUN_TORQUE,STSPIN220_CONF_PARAM_RUNNING_TORQUE); 01213 STSpin220_SetTorque(HOLD_TORQUE,STSPIN220_CONF_PARAM_HOLDING_TORQUE); 01214 devicePrm.torqueBoostEnable = STSPIN220_CONF_PARAM_TORQUE_BOOST_EN; 01215 devicePrm.torqueBoostSpeedThreshold = STSPIN220_CONF_PARAM_TORQUE_BOOST_TH; 01216 STSpin220_SetStopMode(STSPIN220_CONF_PARAM_AUTO_HIZ_STOP); 01217 01218 STSpin220_SetDeviceParamsOtherValues(); 01219 01220 /* Set predefined step mode */ 01221 /* Standby-reset deactivation included to latch the MODEX inputs */ 01222 STSpin220_SetStepMode((motorStepMode_t)STSPIN220_CONF_PARAM_STEP_MODE); 01223 } 01224 01225 /******************************************************//** 01226 * @brief Set the stepping mode without reset 01227 * @param[in] stepMode from full step to 1/256 microstep 01228 * as specified in enum motorStepMode_t 01229 * @retval true if the command is successfully executed, else false 01230 **********************************************************/ 01231 bool STSpin220::STSpin220_SetStepModeWithoutReset(motorStepMode_t stepMode) 01232 { 01233 /* Store step mode */ 01234 devicePrm.stepMode = stepMode; 01235 devicePrm.stepModeLatched = stepMode; 01236 01237 /* Set the mode pins to the levels corresponding to the selected step mode */ 01238 switch (stepMode) 01239 { 01240 case STEP_MODE_FULL: 01241 STSpin220_Board_SetFullStep(); 01242 break; 01243 case STEP_MODE_HALF: 01244 STSpin220_Board_SetModePins(1, 0, 1, 0); 01245 break; 01246 case STEP_MODE_1_4: 01247 STSpin220_Board_SetModePins(0, 1, 0, 1); 01248 break; 01249 case STEP_MODE_1_8: 01250 STSpin220_Board_SetModePins(1, 1, 1, 0); 01251 break; 01252 case STEP_MODE_1_16: 01253 STSpin220_Board_SetModePins(1, 1, 1, 1); 01254 break; 01255 case STEP_MODE_1_32: 01256 STSpin220_Board_SetModePins(0, 0, 0, 1); 01257 break; 01258 case STEP_MODE_1_64: 01259 STSpin220_Board_SetModePins(1, 1, 0, 1); 01260 break; 01261 case STEP_MODE_1_128: 01262 STSpin220_Board_SetModePins(0, 0, 1, 0); 01263 break; 01264 case STEP_MODE_1_256: 01265 STSpin220_Board_SetModePins(1, 1, 0, 0); 01266 break; 01267 default: 01268 return false; 01269 } 01270 01271 /* Wait */ 01272 STSpin220_Board_Delay(SELECT_STEP_MODE_DELAY); 01273 01274 /* Exit standby, selected step mode is latched */ 01275 STSpin220_Board_ReleaseReset(); 01276 01277 /* Let a delay after reset release and step mode latching*/ 01278 STSpin220_Board_Delay(AFTER_STANDBY_EXIT_DEAD_TIME); 01279 01280 /* If full step mode is not selected, do not keep MODE1 = MODE2 = 0 */ 01281 /* after the device quit the standby condition */ 01282 if (stepMode!=(motorStepMode_t)STEP_MODE_FULL) 01283 { 01284 STSpin220_Board_UnsetFullStep(); 01285 } 01286 01287 return true; 01288 } 01289 01290 /******************************************************//** 01291 * @brief Initialises the bridge parameters to start the movement 01292 * and enable the power bridge 01293 * @retval None 01294 **********************************************************/ 01295 void STSpin220::STSpin220_StartMovement(void) 01296 { 01297 /* Enable STSpin220 powerstage */ 01298 STSpin220_Enable(); 01299 toggleOdd = 0; 01300 devicePrm.accu = 0; 01301 devicePrm.relativePos = 0; 01302 STSpin220_Board_TimStckInit(true); 01303 if ((devicePrm.endAccPos == 0)&&\ 01304 (devicePrm.commandExecuted != RUN_CMD)) 01305 { 01306 devicePrm.motionState = DECELERATING; 01307 STSpin220_Board_PwmRefStart(devicePrm.refPwmFreq, DEC_TORQUE); 01308 } 01309 else 01310 { 01311 devicePrm.motionState = ACCELERATING; 01312 STSpin220_Board_PwmRefStart(devicePrm.refPwmFreq, ACC_TORQUE); 01313 } 01314 /* Program the step clock */ 01315 STSpin220_ApplySpeed(devicePrm.minSpeed); 01316 } 01317 01318 /******************************************************//** 01319 * @brief Handles the device state machine at each pulse 01320 * @retval None 01321 * @note Must only be called by the timer ISR 01322 **********************************************************/ 01323 void STSpin220::STSpin220_StepClockHandler(void) 01324 { 01325 uint32_t stepModeShift = devicePrm.stepModeLatched - devicePrm.stepMode; 01326 uint16_t tmp; 01327 STSpin220_Board_Monitor_Set(); 01328 if (devicePrm.motionState == STANDBYTOINACTIVE) 01329 { 01330 if (toggleOdd != 0) 01331 { 01332 STSpin220_Board_StckMode3_Reset(); 01333 toggleOdd = 0; 01334 if (devicePrm.sequencerPosition == 0) 01335 { 01336 if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) 01337 { 01338 STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 01339 } 01340 return; 01341 } 01342 } 01343 else 01344 { 01345 STSpin220_Board_StckMode3_Set(); 01346 toggleOdd = 1; 01347 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01348 devicePrm.sequencerPosition -= tmp; 01349 } 01350 STSpin220_Board_TimStckSetFreq(STSPIN220_MAX_STCK_FREQ); 01351 return; 01352 } 01353 01354 if (toggleOdd == 0) 01355 { 01356 STSpin220_Board_StckMode3_Set(); 01357 toggleOdd = 1; 01358 } 01359 else 01360 { 01361 STSpin220_Board_StckMode3_Reset(); 01362 toggleOdd = 0; 01363 /* Incrementation of the relative position */ 01364 devicePrm.relativePos += (1 << stepModeShift); 01365 01366 /* Incrementation of the current position */ 01367 if (devicePrm.direction != BACKWARD) 01368 { 01369 devicePrm.currentPosition += (1 << stepModeShift); 01370 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01371 devicePrm.sequencerPosition += tmp; 01372 if (devicePrm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1)) 01373 { 01374 devicePrm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1); 01375 } 01376 } 01377 else 01378 { 01379 devicePrm.currentPosition -= (1 << stepModeShift); 01380 tmp = (1 << ((motorStepMode_t)STEP_MODE_1_256-devicePrm.stepMode)); 01381 devicePrm.sequencerPosition -= tmp; 01382 if (devicePrm.sequencerPosition < 0) 01383 { 01384 devicePrm.sequencerPosition += (SEQUENCER_MAX_VALUE+1); 01385 } 01386 } 01387 01388 switch (devicePrm.motionState) 01389 { 01390 case ACCELERATING: 01391 { 01392 uint32_t relPos = devicePrm.relativePos; 01393 uint32_t endAccPos = devicePrm.endAccPos; 01394 uint16_t speed = devicePrm.speed; 01395 uint32_t acc = ((uint32_t)devicePrm.acceleration << 16)>>stepModeShift; 01396 01397 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ 01398 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos>=devicePrm.startDecPos))) 01399 { 01400 devicePrm.motionState = DECELERATING; 01401 devicePrm.accu = 0; 01402 /* Apply decelerating torque */ 01403 STSpin220_ApplyTorque(DEC_TORQUE); 01404 } 01405 else 01406 { 01407 if ((speed>=(devicePrm.maxSpeed>>stepModeShift))||\ 01408 ((devicePrm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos))) 01409 { 01410 devicePrm.motionState = STEADY; 01411 /* Apply running torque */ 01412 STSpin220_ApplyTorque(RUN_TORQUE); 01413 } 01414 else 01415 { 01416 bool speedUpdated = false; 01417 /* Go on accelerating */ 01418 if (speed==0) 01419 { 01420 speed =1; 01421 } 01422 devicePrm.accu += acc / speed; 01423 while (devicePrm.accu>=(0X10000L)) 01424 { 01425 devicePrm.accu -= (0X10000L); 01426 speed +=1; 01427 speedUpdated = true; 01428 } 01429 01430 if (speedUpdated) 01431 { 01432 if (speed>(devicePrm.maxSpeed>>stepModeShift)) 01433 { 01434 speed = devicePrm.maxSpeed>>stepModeShift; 01435 } 01436 devicePrm.speed = speed; 01437 } 01438 01439 if (devicePrm.updateTorque!=false) 01440 { 01441 /* Apply accelerating torque */ 01442 STSpin220_ApplyTorque(ACC_TORQUE); 01443 } 01444 } 01445 } 01446 break; 01447 } 01448 case STEADY: 01449 { 01450 uint16_t maxSpeed = devicePrm.maxSpeed>>stepModeShift; 01451 uint32_t relativePos = devicePrm.relativePos; 01452 if (devicePrm.updateTorque!=false) 01453 { 01454 /* Apply accelerating torque */ 01455 STSpin220_ApplyTorque(RUN_TORQUE); 01456 } 01457 if (((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)||\ 01458 ((devicePrm.commandExecuted==MOVE_CMD)&&\ 01459 (relativePos>=(devicePrm.startDecPos)))||\ 01460 ((devicePrm.commandExecuted==RUN_CMD)&&\ 01461 (devicePrm.speed>maxSpeed))) 01462 { 01463 devicePrm.motionState = DECELERATING; 01464 devicePrm.accu = 0; 01465 /* Apply decelerating torque */ 01466 STSpin220_ApplyTorque(DEC_TORQUE); 01467 } 01468 else 01469 { 01470 if ((devicePrm.commandExecuted==RUN_CMD)&&(devicePrm.speed<maxSpeed)) 01471 { 01472 devicePrm.motionState = ACCELERATING; 01473 devicePrm.accu = 0; 01474 /* Apply accelerating torque */ 01475 STSpin220_ApplyTorque(ACC_TORQUE); 01476 } 01477 } 01478 break; 01479 } 01480 case DECELERATING: 01481 { 01482 uint32_t relativePos = devicePrm.relativePos; 01483 uint16_t speed = devicePrm.speed; 01484 uint32_t dec = ((uint32_t)devicePrm.deceleration << 16)>>stepModeShift; 01485 if ((((devicePrm.commandExecuted&(STSPIN220_SOFT_STOP_BIT_MASK|STSPIN220_DIR_CHANGE_BIT_MASK))!=0)&&\ 01486 (speed<=(devicePrm.minSpeed>>stepModeShift)))||\ 01487 ((devicePrm.commandExecuted==MOVE_CMD)&&(relativePos>=devicePrm.stepsToTake))) 01488 { 01489 /* Motion process complete */ 01490 if ((devicePrm.commandExecuted&STSPIN220_DIR_CHANGE_BIT_MASK)!=0) 01491 { 01492 devicePrm.commandExecuted=(deviceCommand_t)((~STSPIN220_DIR_CHANGE_BIT_MASK)&devicePrm.commandExecuted); 01493 if (devicePrm.direction==BACKWARD) 01494 { 01495 devicePrm.direction=FORWARD; 01496 } 01497 else 01498 { 01499 devicePrm.direction=BACKWARD; 01500 } 01501 STSpin220_Board_SetDirectionGpio(devicePrm.direction); 01502 if ((devicePrm.commandExecuted&STSPIN220_SOFT_STOP_BIT_MASK)==0) 01503 { 01504 devicePrm.motionState = ACCELERATING; 01505 devicePrm.accu = 0; 01506 /* Apply accelerating torque */ 01507 STSpin220_ApplyTorque(ACC_TORQUE); 01508 break; 01509 } 01510 } 01511 if (devicePrm.stopMode==HOLD_MODE) 01512 { 01513 STSpin220_HardStop(); 01514 } 01515 else if (devicePrm.stopMode==STANDBY_MODE) 01516 { 01517 STSpin220_PutDeviceInStandby(); 01518 } 01519 else 01520 { 01521 STSpin220_HardHiZ(); 01522 } 01523 } 01524 else 01525 { 01526 if ((devicePrm.commandExecuted==RUN_CMD)&& 01527 (speed<=(devicePrm.maxSpeed>>stepModeShift))) 01528 { 01529 devicePrm.motionState = STEADY; 01530 /* Apply running torque */ 01531 STSpin220_ApplyTorque(RUN_TORQUE); 01532 } 01533 else 01534 { 01535 /* Go on decelerating */ 01536 if (speed>(devicePrm.minSpeed>>stepModeShift)) 01537 { 01538 bool speedUpdated = false; 01539 if (speed==0) 01540 { 01541 speed =1; 01542 } 01543 devicePrm.accu += dec / speed; 01544 while (devicePrm.accu>=(0X10000L)) 01545 { 01546 devicePrm.accu -= (0X10000L); 01547 if (speed>1) 01548 { 01549 speed -=1; 01550 } 01551 speedUpdated = true; 01552 } 01553 01554 if (speedUpdated) 01555 { 01556 if (speed<(devicePrm.minSpeed>>stepModeShift)) 01557 { 01558 speed = devicePrm.minSpeed>>stepModeShift; 01559 } 01560 devicePrm.speed = speed; 01561 } 01562 01563 if (devicePrm.updateTorque!=false) 01564 { 01565 /* Apply decelerating torque */ 01566 STSpin220_ApplyTorque(DEC_TORQUE); 01567 } 01568 } 01569 } 01570 } 01571 break; 01572 } 01573 default: 01574 { 01575 break; 01576 } 01577 } 01578 } 01579 if ((devicePrm.motionState & INACTIVE) != INACTIVE) 01580 { 01581 STSpin220_ApplySpeed(devicePrm.speed); 01582 } 01583 else 01584 { 01585 if (STSpin220_Board_TimStckStop(&toggleOdd) == 0) 01586 { 01587 STSpin220_ErrorHandler(STSPIN220_ERROR_STEP_CLOCK); 01588 } 01589 } 01590 STSpin220_Board_Monitor_Reset(); 01591 } 01592 01593 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Tue Jul 12 2022 15:16:41 by
1.7.2
X-NUCLEO-IHM06A1 Low Voltage Stepper Motor Driver