Arkadi Rafalovich
Personal fork of the library for direct control instead of library control
Dependents: Thesis_Rotating_Platform
Fork of
X_NUCLEO_IHM01A1
by 
Arkadi Rafalovich
Embed:
(wiki syntax)
Show/hide line numbers
l6474_class.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file l6474_class.cpp 00004 * @author IPC Rennes 00005 * @version V1.5.0 00006 * @date November 12, 2014 00007 * @brief L6474 driver (fully integrated microstepping motor driver) 00008 * @note (C) COPYRIGHT 2014 STMicroelectronics 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2> 00013 * 00014 * Redistribution and use in source and binary forms, with or without modification, 00015 * are permitted provided that the following conditions are met: 00016 * 1. Redistributions of source code must retain the above copyright notice, 00017 * this list of conditions and the following disclaimer. 00018 * 2. Redistributions in binary form must reproduce the above copyright notice, 00019 * this list of conditions and the following disclaimer in the documentation 00020 * and/or other materials provided with the distribution. 00021 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00022 * may be used to endorse or promote products derived from this software 00023 * without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00026 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00027 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00028 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00029 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00030 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00031 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00033 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00034 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 ****************************************************************************** 00037 */ 00038 00039 00040 /* Generated with STM32CubeTOO -----------------------------------------------*/ 00041 00042 00043 /* Revision ------------------------------------------------------------------*/ 00044 /* 00045 Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev 00046 Branch/Trunk/Tag: trunk 00047 Based on: X-CUBE-SPN1/trunk/Drivers/BSP/Components/l6474/l6474.c 00048 Revision: 0 00049 */ 00050 00051 00052 /* Includes ------------------------------------------------------------------*/ 00053 00054 #include "l6474_class.h" 00055 00056 00057 /* Definitions ---------------------------------------------------------------*/ 00058 00059 /* Error while initialising the SPI. */ 00060 #define L6474_ERROR_0 (0x8000) 00061 00062 /* Error of bad SPI transaction. */ 00063 #define L6474_ERROR_1 (0x8001) 00064 00065 /* Maximum number of steps. */ 00066 #define MAX_STEPS (0x7FFFFFFF) 00067 00068 /* Maximum frequency of the PWMs in Hz. */ 00069 #define L6474_MAX_PWM_FREQ (10000) 00070 00071 /* Minimum frequency of the PWMs in Hz. */ 00072 #define L6474_MIN_PWM_FREQ (2) 00073 00074 00075 /* Variables ----------------------------------------------------------------*/ 00076 00077 /* Number of devices. */ 00078 uint8_t L6474::number_of_devices = 0; 00079 00080 /* ISR flags used to restart an interrupted SPI transfer when an error is reported. */ 00081 bool L6474::spi_preemtion_by_isr = FALSE; 00082 bool L6474::isr_flag = FALSE; 00083 00084 /* SPI Transmission for Daisy-Chain Configuration. */ 00085 uint8_t L6474::spi_tx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; 00086 uint8_t L6474::spi_rx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; 00087 00088 00089 /* Methods -------------------------------------------------------------------*/ 00090 00091 /********************************************************** 00092 * @brief Attaches a user callback to the error Handler. 00093 * The call back will be then called each time the library 00094 * detects an error 00095 * @param[in] callback Name of the callback to attach 00096 * to the error Hanlder 00097 * @retval None 00098 **********************************************************/ 00099 void L6474::L6474_AttachErrorHandler(void (*callback)(uint16_t error)) 00100 { 00101 error_handler_callback = (void (*)(uint16_t error)) callback; 00102 } 00103 00104 /********************************************************** 00105 * @brief Starts the L6474 library 00106 * @param init Initialization structure. 00107 * @retval COMPONENT_OK in case of success. 00108 **********************************************************/ 00109 Status_t L6474::L6474_Init(void *init) 00110 { 00111 /* Initialise the PWMs used for the Step clocks ----------------------------*/ 00112 00113 ///////////////////////////////////////////////////////// !!!!!!!!!!!!!!!!!!!!!!!!!!! 00114 // Disable Library control 00115 //L6474_PwmInit(); 00116 // Disable Library control 00117 ////////////////////////////////////////////////////////// !!!!!!!!!!!!!!!!!!!!!!!!!! 00118 00119 /* Initialise the L6474s ------------------------------------------------*/ 00120 00121 /* Standby-reset deactivation */ 00122 L6474_ReleaseReset(); 00123 00124 /* Let a delay after reset */ 00125 L6474_Delay(0.25); // original 1sec delay 00126 00127 /* Set device parameters to the predefined values from "l6474_target_config.h". */ 00128 L6474_SetDeviceParamsToPredefinedValues(); 00129 00130 if (init == NULL) 00131 /* Set device registers to the predefined values from "l6474_target_config.h". */ 00132 L6474_SetRegisterToPredefinedValues(); 00133 else 00134 /* Set device registers to the passed initialization values. */ 00135 L6474_SetRegisterToInitializationValues((L6474_Init_t *) init); 00136 00137 /* Disable L6474 powerstage */ 00138 L6474_CmdDisable(); 00139 00140 /* Get Status to clear flags after start up */ 00141 L6474_CmdGetStatus(); 00142 00143 return COMPONENT_OK; 00144 } 00145 00146 /********************************************************** 00147 * @brief Read id 00148 * @param id pointer to the identifier to be read. 00149 * @retval COMPONENT_OK in case of success. 00150 **********************************************************/ 00151 Status_t L6474::L6474_ReadID(uint8_t *id) 00152 { 00153 *id = device_instance; 00154 00155 return COMPONENT_OK; 00156 } 00157 00158 /********************************************************** 00159 * @brief Returns the acceleration of the specified device 00160 * @retval Acceleration in pps^2 00161 **********************************************************/ 00162 uint16_t L6474::L6474_GetAcceleration(void) 00163 { 00164 return (device_prm.acceleration); 00165 } 00166 00167 /********************************************************** 00168 * @brief Returns the current speed of the specified device 00169 * @retval Speed in pps 00170 **********************************************************/ 00171 uint16_t L6474::L6474_GetCurrentSpeed(void) 00172 { 00173 return device_prm.speed; 00174 } 00175 00176 /********************************************************** 00177 * @brief Returns the deceleration of the specified device 00178 * @retval Deceleration in pps^2 00179 **********************************************************/ 00180 uint16_t L6474::L6474_GetDeceleration(void) 00181 { 00182 return (device_prm.deceleration); 00183 } 00184 00185 /********************************************************** 00186 * @brief Returns the device state 00187 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) 00188 **********************************************************/ 00189 motorState_t L6474::L6474_GetDeviceState(void) 00190 { 00191 return device_prm.motionState; 00192 } 00193 00194 /********************************************************** 00195 * @brief Returns the FW version of the library 00196 * @param None 00197 * @retval L6474_FW_VERSION 00198 **********************************************************/ 00199 uint8_t L6474::L6474_GetFwVersion(void) 00200 { 00201 return (L6474_FW_VERSION); 00202 } 00203 00204 /********************************************************** 00205 * @brief Returns the mark position of the specified device 00206 * @retval Mark register value converted in a 32b signed integer 00207 **********************************************************/ 00208 int32_t L6474::L6474_GetMark(void) 00209 { 00210 return L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); 00211 } 00212 00213 /********************************************************** 00214 * @brief Returns the max speed of the specified device 00215 * @retval maxSpeed in pps 00216 **********************************************************/ 00217 uint16_t L6474::L6474_GetMaxSpeed(void) 00218 { 00219 return (device_prm.maxSpeed); 00220 } 00221 00222 /********************************************************** 00223 * @brief Returns the min speed of the specified device 00224 * @retval minSpeed in pps 00225 **********************************************************/ 00226 uint16_t L6474::L6474_GetMinSpeed(void) 00227 { 00228 return (device_prm.minSpeed); 00229 } 00230 00231 /********************************************************** 00232 * @brief Returns the ABS_POSITION of the specified device 00233 * @retval ABS_POSITION register value converted in a 32b signed integer 00234 **********************************************************/ 00235 int32_t L6474::L6474_GetPosition(void) 00236 { 00237 return L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); 00238 } 00239 00240 /********************************************************** 00241 * @brief Requests the motor to move to the home position (ABS_POSITION = 0) 00242 * @retval None 00243 **********************************************************/ 00244 void L6474::L6474_GoHome(void) 00245 { 00246 L6474_GoTo(0); 00247 } 00248 00249 /********************************************************** 00250 * @brief Requests the motor to move to the mark position 00251 * @retval None 00252 **********************************************************/ 00253 void L6474::L6474_GoMark(void) 00254 { 00255 uint32_t mark; 00256 00257 mark = L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK)); 00258 L6474_GoTo(mark); 00259 } 00260 00261 /********************************************************** 00262 * @brief Requests the motor to move to the specified position 00263 * @param[in] targetPosition absolute position in steps 00264 * @retval None 00265 **********************************************************/ 00266 void L6474::L6474_GoTo(int32_t targetPosition) 00267 { 00268 motorDir_t direction; 00269 int32_t steps; 00270 00271 /* Eventually deactivate motor */ 00272 if (device_prm.motionState != INACTIVE) 00273 { 00274 L6474_HardStop(); 00275 } 00276 00277 /* Get current position */ 00278 device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); 00279 00280 /* Compute the number of steps to perform */ 00281 steps = targetPosition - device_prm.currentPosition; 00282 00283 if (steps >= 0) 00284 { 00285 device_prm.stepsToTake = steps; 00286 direction = FORWARD; 00287 } 00288 else 00289 { 00290 device_prm.stepsToTake = -steps; 00291 direction = BACKWARD; 00292 } 00293 00294 if (steps != 0) 00295 { 00296 device_prm.commandExecuted = MOVE_CMD; 00297 00298 /* Direction setup */ 00299 L6474_SetDirection(direction); 00300 00301 L6474_ComputeSpeedProfile(device_prm.stepsToTake); 00302 00303 /* Motor activation */ 00304 L6474_StartMovement(); 00305 } 00306 } 00307 00308 /********************************************************** 00309 * @brief Immediatly stops the motor and disable the power bridge 00310 * @retval None 00311 **********************************************************/ 00312 void L6474::L6474_HardStop(void) 00313 { 00314 /* Disable corresponding PWM */ 00315 L6474_PwmStop(); 00316 00317 /* Set inactive state */ 00318 device_prm.motionState = INACTIVE; 00319 device_prm.commandExecuted = NO_CMD; 00320 device_prm.stepsToTake = MAX_STEPS; 00321 } 00322 00323 /********************************************************** 00324 * @brief Moves the motor of the specified number of steps 00325 * @param[in] direction FORWARD or BACKWARD 00326 * @param[in] stepCount Number of steps to perform 00327 * @retval None 00328 **********************************************************/ 00329 void L6474::L6474_Move(motorDir_t direction, uint32_t stepCount) 00330 { 00331 /* Eventually deactivate motor */ 00332 if (device_prm.motionState != INACTIVE) 00333 { 00334 L6474_HardStop(); 00335 } 00336 00337 if (stepCount != 0) 00338 { 00339 device_prm.stepsToTake = stepCount; 00340 00341 device_prm.commandExecuted = MOVE_CMD; 00342 00343 device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS)); 00344 00345 /* Direction setup */ 00346 L6474_SetDirection(direction); 00347 00348 L6474_ComputeSpeedProfile(stepCount); 00349 00350 /* Motor activation */ 00351 L6474_StartMovement(); 00352 } 00353 } 00354 00355 /********************************************************** 00356 * @brief Runs the motor. It will accelerate from the min 00357 * speed up to the max speed by using the device acceleration. 00358 * @param[in] direction FORWARD or BACKWARD 00359 * @retval None 00360 **********************************************************/ 00361 void L6474::L6474_Run(motorDir_t direction) 00362 { 00363 /* Eventually deactivate motor */ 00364 if (device_prm.motionState != INACTIVE) 00365 { 00366 L6474_HardStop(); 00367 } 00368 00369 /* Direction setup */ 00370 L6474_SetDirection(direction); 00371 00372 device_prm.commandExecuted = RUN_CMD; 00373 00374 /* Motor activation */ 00375 L6474_StartMovement(); 00376 } 00377 00378 /********************************************************** 00379 * @brief Changes the acceleration of the specified device 00380 * @param[in] newAcc New acceleration to apply in pps^2 00381 * @retval true if the command is successfully executed, else false 00382 * @note The command is not performed is the device is executing 00383 * a MOVE or GOTO command (but it can be used during a RUN command) 00384 **********************************************************/ 00385 bool L6474::L6474_SetAcceleration(uint16_t newAcc) 00386 { 00387 bool cmdExecuted = FALSE; 00388 if ((newAcc != 0)&& 00389 ((device_prm.motionState == INACTIVE)|| 00390 (device_prm.commandExecuted == RUN_CMD))) 00391 { 00392 device_prm.acceleration = newAcc; 00393 cmdExecuted = TRUE; 00394 } 00395 return cmdExecuted; 00396 } 00397 00398 /********************************************************** 00399 * @brief Changes the deceleration of the specified device 00400 * @param[in] newDec New deceleration to apply in pps^2 00401 * @retval true if the command is successfully executed, else false 00402 * @note The command is not performed is the device is executing 00403 * a MOVE or GOTO command (but it can be used during a RUN command) 00404 **********************************************************/ 00405 bool L6474::L6474_SetDeceleration(uint16_t newDec) 00406 { 00407 bool cmdExecuted = FALSE; 00408 if ((newDec != 0)&& 00409 ((device_prm.motionState == INACTIVE)|| 00410 (device_prm.commandExecuted == RUN_CMD))) 00411 { 00412 device_prm.deceleration = newDec; 00413 cmdExecuted = TRUE; 00414 } 00415 return cmdExecuted; 00416 } 00417 00418 /********************************************************** 00419 * @brief Set current position to be the Home position (ABS pos set to 0) 00420 * @retval None 00421 **********************************************************/ 00422 void L6474::L6474_SetHome(void) 00423 { 00424 L6474_CmdSetParam(L6474_ABS_POS, 0); 00425 } 00426 00427 /********************************************************** 00428 * @brief Sets current position to be the Mark position 00429 * @retval None 00430 **********************************************************/ 00431 void L6474::L6474_SetMark(void) 00432 { 00433 uint32_t mark = L6474_CmdGetParam(L6474_ABS_POS); 00434 L6474_CmdSetParam(L6474_MARK, mark); 00435 } 00436 00437 /********************************************************** 00438 * @brief Changes the max speed of the specified device 00439 * @param[in] newMaxSpeed New max speed to apply in pps 00440 * @retval true if the command is successfully executed, else false 00441 * @note The command is not performed is the device is executing 00442 * a MOVE or GOTO command (but it can be used during a RUN command). 00443 **********************************************************/ 00444 bool L6474::L6474_SetMaxSpeed(uint16_t newMaxSpeed) 00445 { 00446 bool cmdExecuted = FALSE; 00447 if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&& 00448 (newMaxSpeed <= L6474_MAX_PWM_FREQ) && 00449 (device_prm.minSpeed <= newMaxSpeed) && 00450 ((device_prm.motionState == INACTIVE)|| 00451 (device_prm.commandExecuted == RUN_CMD))) 00452 { 00453 device_prm.maxSpeed = newMaxSpeed; 00454 cmdExecuted = TRUE; 00455 } 00456 return cmdExecuted; 00457 } 00458 00459 /********************************************************** 00460 * @brief Changes the min speed of the specified device 00461 * @param[in] newMinSpeed New min speed to apply in pps 00462 * @retval true if the command is successfully executed, else false 00463 * @note The command is not performed is the device is executing 00464 * a MOVE or GOTO command (but it can be used during a RUN command). 00465 **********************************************************/ 00466 bool L6474::L6474_SetMinSpeed(uint16_t newMinSpeed) 00467 { 00468 bool cmdExecuted = FALSE; 00469 if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&& 00470 (newMinSpeed <= L6474_MAX_PWM_FREQ) && 00471 (newMinSpeed <= device_prm.maxSpeed) && 00472 ((device_prm.motionState == INACTIVE)|| 00473 (device_prm.commandExecuted == RUN_CMD))) 00474 { 00475 device_prm.minSpeed = newMinSpeed; 00476 cmdExecuted = TRUE; 00477 } 00478 return cmdExecuted; 00479 } 00480 00481 /********************************************************** 00482 * @brief Stops the motor by using the device deceleration 00483 * @retval true if the command is successfully executed, else false 00484 * @note The command is not performed is the device is in INACTIVE state. 00485 **********************************************************/ 00486 bool L6474::L6474_SoftStop(void) 00487 { 00488 bool cmdExecuted = FALSE; 00489 if (device_prm.motionState != INACTIVE) 00490 { 00491 device_prm.commandExecuted = SOFT_STOP_CMD; 00492 cmdExecuted = TRUE; 00493 } 00494 return (cmdExecuted); 00495 } 00496 00497 /********************************************************** 00498 * @brief Locks until the device state becomes Inactive 00499 * @retval None 00500 **********************************************************/ 00501 void L6474::L6474_WaitWhileActive(void) 00502 { 00503 /* Wait while motor is running */ 00504 while (L6474_GetDeviceState() != INACTIVE); 00505 } 00506 00507 /********************************************************** 00508 * @brief Issue the Disable command to the L6474 of the specified device 00509 * @retval None 00510 **********************************************************/ 00511 void L6474::L6474_CmdDisable(void) 00512 { 00513 L6474_SendCommand(L6474_DISABLE); 00514 } 00515 00516 /********************************************************** 00517 * @brief Issues the Enable command to the L6474 of the specified device 00518 * @retval None 00519 **********************************************************/ 00520 void L6474::L6474_CmdEnable(void) 00521 { 00522 L6474_SendCommand(L6474_ENABLE); 00523 } 00524 00525 /********************************************************** 00526 * @brief Issues the GetParam command to the L6474 of the specified device 00527 * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...) 00528 * @retval Register value 00529 **********************************************************/ 00530 uint32_t L6474::L6474_CmdGetParam(L6474_Registers_t parameter) 00531 { 00532 uint32_t i; 00533 uint32_t spiRxData; 00534 uint8_t maxArgumentNbBytes = 0; 00535 uint8_t spiIndex = number_of_devices - device_instance - 1; 00536 bool itDisable = FALSE; 00537 00538 do 00539 { 00540 spi_preemtion_by_isr = FALSE; 00541 if (itDisable) 00542 { 00543 /* re-enable L6474_EnableIrq if disable in previous iteration */ 00544 L6474_EnableIrq(); 00545 itDisable = FALSE; 00546 } 00547 00548 for (i = 0; i < number_of_devices; i++) 00549 { 00550 spi_tx_bursts[0][i] = L6474_NOP; 00551 spi_tx_bursts[1][i] = L6474_NOP; 00552 spi_tx_bursts[2][i] = L6474_NOP; 00553 spi_tx_bursts[3][i] = L6474_NOP; 00554 spi_rx_bursts[1][i] = 0; 00555 spi_rx_bursts[2][i] = 0; 00556 spi_rx_bursts[3][i] = 0; 00557 } 00558 00559 switch (parameter) 00560 { 00561 case L6474_ABS_POS: ; 00562 case L6474_MARK: 00563 spi_tx_bursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); 00564 maxArgumentNbBytes = 3; 00565 break; 00566 case L6474_EL_POS: ; 00567 case L6474_CONFIG: ; 00568 case L6474_STATUS: 00569 spi_tx_bursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); 00570 maxArgumentNbBytes = 2; 00571 break; 00572 default: 00573 spi_tx_bursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter); 00574 maxArgumentNbBytes = 1; 00575 } 00576 00577 /* Disable interruption before checking */ 00578 /* pre-emption by ISR and SPI transfers*/ 00579 L6474_DisableIrq(); 00580 itDisable = TRUE; 00581 } while (spi_preemtion_by_isr); // check pre-emption by ISR 00582 00583 for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00584 i < L6474_CMD_ARG_MAX_NB_BYTES; 00585 i++) 00586 { 00587 L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]); 00588 } 00589 00590 spiRxData = ((uint32_t)spi_rx_bursts[1][spiIndex] << 16) | 00591 (spi_rx_bursts[2][spiIndex] << 8) | 00592 (spi_rx_bursts[3][spiIndex]); 00593 00594 /* re-enable L6474_EnableIrq after SPI transfers*/ 00595 L6474_EnableIrq(); 00596 00597 return (spiRxData); 00598 } 00599 00600 /********************************************************** 00601 * @brief Issues the GetStatus command to the L6474 of the specified device 00602 * @retval Status Register value 00603 * @note Once the GetStatus command is performed, the flags of the status register 00604 * are reset. This is not the case when the status register is read with the 00605 * GetParam command (via the functions L6474ReadStatusRegister or L6474_CmdGetParam). 00606 **********************************************************/ 00607 uint16_t L6474::L6474_CmdGetStatus(void) 00608 { 00609 uint32_t i; 00610 uint16_t status; 00611 uint8_t spiIndex = number_of_devices - device_instance - 1; 00612 bool itDisable = FALSE; 00613 00614 do 00615 { 00616 spi_preemtion_by_isr = FALSE; 00617 if (itDisable) 00618 { 00619 /* re-enable L6474_EnableIrq if disable in previous iteration */ 00620 L6474_EnableIrq(); 00621 itDisable = FALSE; 00622 } 00623 00624 for (i = 0; i < number_of_devices; i++) 00625 { 00626 spi_tx_bursts[0][i] = L6474_NOP; 00627 spi_tx_bursts[1][i] = L6474_NOP; 00628 spi_tx_bursts[2][i] = L6474_NOP; 00629 spi_rx_bursts[1][i] = 0; 00630 spi_rx_bursts[2][i] = 0; 00631 } 00632 spi_tx_bursts[0][spiIndex] = L6474_GET_STATUS; 00633 00634 /* Disable interruption before checking */ 00635 /* pre-emption by ISR and SPI transfers*/ 00636 L6474_DisableIrq(); 00637 itDisable = TRUE; 00638 } while (spi_preemtion_by_isr); // check pre-emption by ISR 00639 00640 for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++) 00641 { 00642 L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]); 00643 } 00644 status = (spi_rx_bursts[1][spiIndex] << 8) | (spi_rx_bursts[2][spiIndex]); 00645 00646 /* re-enable L6474_EnableIrq after SPI transfers*/ 00647 L6474_EnableIrq(); 00648 00649 return (status); 00650 } 00651 00652 /********************************************************** 00653 * @brief Issues the Nop command to the L6474 of the specified device 00654 * @retval None 00655 **********************************************************/ 00656 void L6474::L6474_CmdNop(void) 00657 { 00658 L6474_SendCommand(L6474_NOP); 00659 } 00660 00661 /********************************************************** 00662 * @brief Issues the SetParam command to the L6474 of the specified device 00663 * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...) 00664 * @param[in] value Value to set in the register 00665 * @retval None 00666 **********************************************************/ 00667 void L6474::L6474_CmdSetParam(L6474_Registers_t parameter, uint32_t value) 00668 { 00669 uint32_t i; 00670 uint8_t maxArgumentNbBytes = 0; 00671 uint8_t spiIndex = number_of_devices - device_instance - 1; 00672 bool itDisable = FALSE; 00673 do 00674 { 00675 spi_preemtion_by_isr = FALSE; 00676 if (itDisable) 00677 { 00678 /* re-enable L6474_EnableIrq if disable in previous iteration */ 00679 L6474_EnableIrq(); 00680 itDisable = FALSE; 00681 } 00682 00683 for (i = 0; i < number_of_devices; i++) 00684 { 00685 spi_tx_bursts[0][i] = L6474_NOP; 00686 spi_tx_bursts[1][i] = L6474_NOP; 00687 spi_tx_bursts[2][i] = L6474_NOP; 00688 spi_tx_bursts[3][i] = L6474_NOP; 00689 } 00690 00691 switch (parameter) 00692 { 00693 case L6474_ABS_POS: ; 00694 case L6474_MARK: 00695 spi_tx_bursts[0][spiIndex] = parameter; 00696 spi_tx_bursts[1][spiIndex] = (uint8_t)(value >> 16); 00697 spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8); 00698 maxArgumentNbBytes = 3; 00699 break; 00700 case L6474_EL_POS: ; 00701 case L6474_CONFIG: 00702 spi_tx_bursts[1][spiIndex] = parameter; 00703 spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8); 00704 maxArgumentNbBytes = 2; 00705 break; 00706 default: 00707 spi_tx_bursts[2][spiIndex] = parameter; 00708 maxArgumentNbBytes = 1; 00709 break; 00710 } 00711 spi_tx_bursts[3][spiIndex] = (uint8_t)(value); 00712 00713 /* Disable interruption before checking */ 00714 /* pre-emption by ISR and SPI transfers*/ 00715 L6474_DisableIrq(); 00716 itDisable = TRUE; 00717 } while (spi_preemtion_by_isr); // check pre-emption by ISR 00718 00719 /* SPI transfer */ 00720 for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00721 i < L6474_CMD_ARG_MAX_NB_BYTES; 00722 i++) 00723 { 00724 L6474_WriteBytes(&spi_tx_bursts[i][0],&spi_rx_bursts[i][0]); 00725 } 00726 /* re-enable L6474_EnableIrq after SPI transfers*/ 00727 L6474_EnableIrq(); 00728 } 00729 00730 /********************************************************** 00731 * @brief Reads the Status Register value 00732 * @retval Status register valued 00733 * @note The status register flags are not cleared 00734 * at the difference with L6474CmdGetStatus() 00735 **********************************************************/ 00736 uint16_t L6474::L6474_ReadStatusRegister(void) 00737 { 00738 return (L6474_CmdGetParam(L6474_STATUS)); 00739 } 00740 00741 /********************************************************** 00742 * @brief Set the stepping mode 00743 * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t 00744 * @retval None 00745 **********************************************************/ 00746 void L6474::L6474_SelectStepMode(motorStepMode_t stepMod) 00747 { 00748 uint8_t stepModeRegister; 00749 L6474_STEP_SEL_t l6474StepMod; 00750 00751 switch (stepMod) 00752 { 00753 case STEP_MODE_FULL: 00754 l6474StepMod = L6474_STEP_SEL_1; 00755 break; 00756 case STEP_MODE_HALF: 00757 l6474StepMod = L6474_STEP_SEL_1_2; 00758 break; 00759 case STEP_MODE_1_4: 00760 l6474StepMod = L6474_STEP_SEL_1_4; 00761 break; 00762 case STEP_MODE_1_8: 00763 l6474StepMod = L6474_STEP_SEL_1_8; 00764 break; 00765 case STEP_MODE_1_16: 00766 default: 00767 l6474StepMod = L6474_STEP_SEL_1_16; 00768 break; 00769 } 00770 00771 /* Eventually deactivate motor */ 00772 if (device_prm.motionState != INACTIVE) 00773 { 00774 L6474_HardStop(); 00775 } 00776 00777 /* Read Step mode register and clear STEP_SEL field */ 00778 stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(L6474_STEP_MODE)) ; 00779 00780 /* Apply new step mode */ 00781 L6474_CmdSetParam(L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod); 00782 00783 /* Reset abs pos register */ 00784 L6474_SetHome(); 00785 } 00786 00787 /********************************************************** 00788 * @brief Get the direction 00789 * @param None 00790 * @retval direction FORWARD or BACKWARD 00791 **********************************************************/ 00792 motorDir_t L6474::L6474_GetDirection(void) 00793 { 00794 return device_prm.direction; 00795 } 00796 00797 /********************************************************** 00798 * @brief Specifies the direction 00799 * @param[in] dir FORWARD or BACKWARD 00800 * @note The direction change is only applied if the device 00801 * is in INACTIVE state 00802 * @retval None 00803 **********************************************************/ 00804 void L6474::L6474_SetDirection(motorDir_t direction) 00805 { 00806 if (device_prm.motionState == INACTIVE) 00807 { 00808 device_prm.direction = direction; 00809 L6474_SetDirectionGpio(direction); 00810 } 00811 } 00812 00813 /********************************************************** 00814 * @brief Updates the current speed of the device 00815 * @param[in] newSpeed in pps 00816 * @retval None 00817 **********************************************************/ 00818 void L6474::L6474_ApplySpeed(uint16_t newSpeed) 00819 { 00820 if (newSpeed < L6474_MIN_PWM_FREQ) 00821 { 00822 newSpeed = L6474_MIN_PWM_FREQ; 00823 } 00824 if (newSpeed > L6474_MAX_PWM_FREQ) 00825 { 00826 newSpeed = L6474_MAX_PWM_FREQ; 00827 } 00828 00829 device_prm.speed = newSpeed; 00830 00831 L6474_PwmSetFreq(newSpeed); 00832 } 00833 00834 /********************************************************** 00835 * @brief Computes the speed profile according to the number of steps to move 00836 * @param[in] nbSteps number of steps to perform 00837 * @retval None 00838 * @note Using the acceleration and deceleration of the device, 00839 * this function determines the duration in steps of the acceleration, 00840 * steady and deceleration phases. 00841 * If the total number of steps to perform is big enough, a trapezoidal move 00842 * is performed (i.e. there is a steady phase where the motor runs at the maximum 00843 * speed. 00844 * Else, a triangular move is performed (no steady phase: the maximum speed is never 00845 * reached. 00846 **********************************************************/ 00847 void L6474::L6474_ComputeSpeedProfile(uint32_t nbSteps) 00848 { 00849 uint32_t reqAccSteps; 00850 uint32_t reqDecSteps; 00851 00852 /* compute the number of steps to get the targeted speed */ 00853 uint16_t minSpeed = device_prm.minSpeed; 00854 reqAccSteps = (device_prm.maxSpeed - minSpeed); 00855 reqAccSteps *= (device_prm.maxSpeed + minSpeed); 00856 reqDecSteps = reqAccSteps; 00857 reqAccSteps /= (uint32_t)device_prm.acceleration; 00858 reqAccSteps /= 2; 00859 00860 /* compute the number of steps to stop */ 00861 reqDecSteps /= (uint32_t)device_prm.deceleration; 00862 reqDecSteps /= 2; 00863 00864 if(( reqAccSteps + reqDecSteps ) > nbSteps) 00865 { 00866 /* Triangular move */ 00867 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 00868 uint32_t dec = device_prm.deceleration; 00869 uint32_t acc = device_prm.acceleration; 00870 00871 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); 00872 if (reqDecSteps > 1) 00873 { 00874 reqAccSteps = reqDecSteps - 1; 00875 if(reqAccSteps == 0) 00876 { 00877 reqAccSteps = 1; 00878 } 00879 } 00880 else 00881 { 00882 reqAccSteps = 0; 00883 } 00884 device_prm.endAccPos = reqAccSteps; 00885 device_prm.startDecPos = reqDecSteps; 00886 } 00887 else 00888 { 00889 /* Trapezoidal move */ 00890 /* accelerating phase to endAccPos */ 00891 /* steady phase from endAccPos to startDecPos */ 00892 /* decelerating from startDecPos to stepsToTake*/ 00893 device_prm.endAccPos = reqAccSteps; 00894 device_prm.startDecPos = nbSteps - reqDecSteps - 1; 00895 } 00896 } 00897 00898 /********************************************************** 00899 * @brief Converts the ABS_POSITION register value to a 32b signed integer 00900 * @param[in] abs_position_reg value of the ABS_POSITION register 00901 * @retval operation_result 32b signed integer corresponding to the absolute position 00902 **********************************************************/ 00903 int32_t L6474::L6474_ConvertPosition(uint32_t abs_position_reg) 00904 { 00905 int32_t operation_result; 00906 00907 if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) 00908 { 00909 /* Negative register value */ 00910 abs_position_reg = ~abs_position_reg; 00911 abs_position_reg += 1; 00912 00913 operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK); 00914 operation_result = -operation_result; 00915 } 00916 else 00917 { 00918 operation_result = (int32_t) abs_position_reg; 00919 } 00920 00921 return operation_result; 00922 } 00923 00924 /********************************************************** 00925 * @brief Error handler which calls the user callback (if defined) 00926 * @param[in] error Number of the error 00927 * @retval None 00928 **********************************************************/ 00929 void L6474::L6474_ErrorHandler(uint16_t error) 00930 { 00931 if (error_handler_callback != 0) 00932 { 00933 (void) error_handler_callback(error); 00934 } 00935 else 00936 { 00937 /* Aborting the program. */ 00938 exit(EXIT_FAILURE); 00939 } 00940 } 00941 00942 /********************************************************** 00943 * @brief Sends a command without arguments to the L6474 via the SPI 00944 * @param[in] param Command to send 00945 * @retval None 00946 **********************************************************/ 00947 void L6474::L6474_SendCommand(uint8_t param) 00948 { 00949 uint32_t i; 00950 bool itDisable = FALSE; 00951 uint8_t spiIndex = number_of_devices - device_instance - 1; 00952 00953 do 00954 { 00955 spi_preemtion_by_isr = FALSE; 00956 if (itDisable) 00957 { 00958 /* re-enable L6474_EnableIrq if disable in previous iteration */ 00959 L6474_EnableIrq(); 00960 itDisable = FALSE; 00961 } 00962 00963 for (i = 0; i < number_of_devices; i++) 00964 { 00965 spi_tx_bursts[3][i] = L6474_NOP; 00966 } 00967 spi_tx_bursts[3][spiIndex] = param; 00968 00969 /* Disable interruption before checking */ 00970 /* pre-emption by ISR and SPI transfers*/ 00971 L6474_DisableIrq(); 00972 itDisable = TRUE; 00973 } while (spi_preemtion_by_isr); // check pre-emption by ISR 00974 00975 L6474_WriteBytes(&spi_tx_bursts[3][0], &spi_rx_bursts[3][0]); 00976 00977 /* re-enable L6474_EnableIrq after SPI transfers*/ 00978 L6474_EnableIrq(); 00979 } 00980 00981 /********************************************************** 00982 * @brief Sets the registers of the L6474 to their predefined values 00983 * from l6474_target_config.h 00984 * @retval None 00985 **********************************************************/ 00986 void L6474::L6474_SetRegisterToPredefinedValues(void) 00987 { 00988 L6474_CmdSetParam( 00989 L6474_ABS_POS, 00990 0); 00991 L6474_CmdSetParam( 00992 L6474_EL_POS, 00993 0); 00994 L6474_CmdSetParam( 00995 L6474_MARK, 00996 0); 00997 switch (device_instance) 00998 { 00999 case 0: 01000 L6474_CmdSetParam( 01001 L6474_TVAL, 01002 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); 01003 L6474_CmdSetParam( 01004 L6474_T_FAST, 01005 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 | 01006 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0); 01007 L6474_CmdSetParam( 01008 L6474_TON_MIN, 01009 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0) 01010 ); 01011 L6474_CmdSetParam( 01012 L6474_TOFF_MIN, 01013 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0)); 01014 L6474_CmdSetParam( 01015 L6474_OCD_TH, 01016 L6474_CONF_PARAM_OCD_TH_DEVICE_0); 01017 L6474_CmdSetParam( 01018 L6474_STEP_MODE, 01019 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 | 01020 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0); 01021 L6474_CmdSetParam( 01022 L6474_ALARM_EN, 01023 L6474_CONF_PARAM_ALARM_EN_DEVICE_0); 01024 L6474_CmdSetParam( 01025 L6474_CONFIG, 01026 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | 01027 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 | 01028 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 | 01029 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 | 01030 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0); 01031 break; 01032 case 1: 01033 L6474_CmdSetParam( 01034 L6474_TVAL, 01035 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); 01036 L6474_CmdSetParam( 01037 L6474_T_FAST, 01038 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 | 01039 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1); 01040 L6474_CmdSetParam( 01041 L6474_TON_MIN, 01042 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1)); 01043 L6474_CmdSetParam( 01044 L6474_TOFF_MIN, 01045 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1)); 01046 L6474_CmdSetParam( 01047 L6474_OCD_TH, 01048 L6474_CONF_PARAM_OCD_TH_DEVICE_1); 01049 L6474_CmdSetParam( 01050 L6474_STEP_MODE, 01051 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 | 01052 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1); 01053 L6474_CmdSetParam( 01054 L6474_ALARM_EN, 01055 L6474_CONF_PARAM_ALARM_EN_DEVICE_1); 01056 L6474_CmdSetParam( 01057 L6474_CONFIG, 01058 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | 01059 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 | 01060 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 | 01061 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 | 01062 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1); 01063 break; 01064 case 2: 01065 L6474_CmdSetParam( 01066 L6474_TVAL, 01067 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); 01068 L6474_CmdSetParam( 01069 L6474_T_FAST, 01070 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 | 01071 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2); 01072 L6474_CmdSetParam( 01073 L6474_TON_MIN, 01074 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2)); 01075 L6474_CmdSetParam( 01076 L6474_TOFF_MIN, 01077 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2)); 01078 L6474_CmdSetParam( 01079 L6474_OCD_TH, 01080 L6474_CONF_PARAM_OCD_TH_DEVICE_2); 01081 L6474_CmdSetParam( 01082 L6474_STEP_MODE, 01083 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 | 01084 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2); 01085 L6474_CmdSetParam( 01086 L6474_ALARM_EN, 01087 L6474_CONF_PARAM_ALARM_EN_DEVICE_2); 01088 L6474_CmdSetParam( 01089 L6474_CONFIG, 01090 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | 01091 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 | 01092 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 | 01093 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 | 01094 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2); 01095 break; 01096 default: ; 01097 } 01098 } 01099 01100 /********************************************************** 01101 * @brief Sets the registers of the L6474 to initialization values. 01102 * @param init Initialization structure. 01103 * @retval None. 01104 **********************************************************/ 01105 void L6474::L6474_SetRegisterToInitializationValues(L6474_Init_t *init) 01106 { 01107 L6474_CmdSetParam( 01108 L6474_ABS_POS, 01109 0 01110 ); 01111 L6474_CmdSetParam( 01112 L6474_EL_POS, 01113 0 01114 ); 01115 L6474_CmdSetParam( 01116 L6474_MARK, 01117 0 01118 ); 01119 L6474_CmdSetParam( 01120 L6474_TVAL, 01121 L6474_Tval_Current_to_Par(init->torque_regulation_current_mA) 01122 ); 01123 L6474_CmdSetParam( 01124 L6474_T_FAST, 01125 (uint8_t) init->maximum_fast_decay_time | 01126 (uint8_t) init->fall_time 01127 ); 01128 L6474_CmdSetParam( 01129 L6474_TON_MIN, 01130 L6474_Tmin_Time_to_Par(init->minimum_ON_time_us) 01131 ); 01132 L6474_CmdSetParam( 01133 L6474_TOFF_MIN, 01134 L6474_Tmin_Time_to_Par(init->minimum_OFF_time_us) 01135 ); 01136 L6474_CmdSetParam( 01137 L6474_OCD_TH, 01138 init->overcurrent_threshold 01139 ); 01140 L6474_CmdSetParam( 01141 L6474_STEP_MODE, 01142 (uint8_t) init->step_selection | 01143 (uint8_t) init->sync_selection 01144 ); 01145 L6474_CmdSetParam( 01146 L6474_ALARM_EN, 01147 init->alarm 01148 ); 01149 L6474_CmdSetParam( 01150 L6474_CONFIG, 01151 (uint16_t) init->clock | 01152 (uint16_t) init->torque_regulation_method | 01153 (uint16_t) init->overcurrent_shutwdown | 01154 (uint16_t) init->slew_rate | 01155 (uint16_t) init->target_swicthing_period 01156 ); 01157 } 01158 01159 /********************************************************** 01160 * @brief Sets the parameters of the device to predefined values 01161 * from l6474_target_config.h 01162 * @param None 01163 * @retval None 01164 **********************************************************/ 01165 void L6474::L6474_SetDeviceParamsToPredefinedValues(void) 01166 { 01167 switch (device_instance) 01168 { 01169 case 0: 01170 device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_0; 01171 device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_0; 01172 device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0; 01173 device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0; 01174 break; 01175 01176 case 1: 01177 device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_1; 01178 device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_1; 01179 device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1; 01180 device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1; 01181 break; 01182 01183 case 2: 01184 device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_2; 01185 device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_2; 01186 device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2; 01187 device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2; 01188 break; 01189 } 01190 01191 device_prm.accu = 0; 01192 device_prm.currentPosition = 0; 01193 device_prm.endAccPos = 0; 01194 device_prm.relativePos = 0; 01195 device_prm.startDecPos = 0; 01196 device_prm.stepsToTake = 0; 01197 device_prm.speed = 0; 01198 device_prm.commandExecuted = NO_CMD; 01199 device_prm.direction = FORWARD; 01200 device_prm.motionState = INACTIVE; 01201 } 01202 01203 /********************************************************** 01204 * @brief Initialises the bridge parameters to start the movement 01205 * and enable the power bridge 01206 * @retval None 01207 **********************************************************/ 01208 void L6474::L6474_StartMovement(void) 01209 { 01210 /* Enable L6474 powerstage */ 01211 L6474_CmdEnable(); 01212 if (device_prm.endAccPos != 0) 01213 { 01214 device_prm.motionState = ACCELERATING; 01215 } 01216 else 01217 { 01218 device_prm.motionState = DECELERATING; 01219 } 01220 device_prm.accu = 0; 01221 device_prm.relativePos = 0; 01222 L6474_ApplySpeed(device_prm.minSpeed); 01223 } 01224 01225 /********************************************************** 01226 * @brief Handles the device state machine at each ste 01227 * @retval None 01228 * @note Must only be called by the timer ISR 01229 **********************************************************/ 01230 void L6474::L6474_StepClockHandler(void) 01231 { 01232 /* Set isr flag */ 01233 isr_flag = TRUE; 01234 01235 /* Incrementation of the relative position */ 01236 device_prm.relativePos++; 01237 01238 switch (device_prm.motionState) 01239 { 01240 case ACCELERATING: 01241 { 01242 uint32_t relPos = device_prm.relativePos; 01243 uint32_t endAccPos = device_prm.endAccPos; 01244 uint16_t speed = device_prm.speed; 01245 uint32_t acc = ((uint32_t)device_prm.acceleration << 16); 01246 01247 if ((device_prm.commandExecuted == SOFT_STOP_CMD)|| 01248 ((device_prm.commandExecuted != RUN_CMD)&& 01249 (relPos == device_prm.startDecPos))) 01250 { 01251 device_prm.motionState = DECELERATING; 01252 device_prm.accu = 0; 01253 } 01254 else if ((speed >= device_prm.maxSpeed)|| 01255 ((device_prm.commandExecuted != RUN_CMD)&& 01256 (relPos == endAccPos))) 01257 { 01258 device_prm.motionState = STEADY; 01259 } 01260 else 01261 { 01262 bool speedUpdated = FALSE; 01263 /* Go on accelerating */ 01264 if (speed == 0) speed =1; 01265 device_prm.accu += acc / speed; 01266 while (device_prm.accu >= (0X10000L)) 01267 { 01268 device_prm.accu -= (0X10000L); 01269 speed +=1; 01270 speedUpdated = TRUE; 01271 } 01272 01273 if (speedUpdated) 01274 { 01275 if (speed > device_prm.maxSpeed) 01276 { 01277 speed = device_prm.maxSpeed; 01278 } 01279 device_prm.speed = speed; 01280 L6474_ApplySpeed(device_prm.speed); 01281 } 01282 } 01283 break; 01284 } 01285 case STEADY: 01286 { 01287 uint16_t maxSpeed = device_prm.maxSpeed; 01288 uint32_t relativePos = device_prm.relativePos; 01289 if ((device_prm.commandExecuted == SOFT_STOP_CMD)|| 01290 ((device_prm.commandExecuted != RUN_CMD)&& 01291 (relativePos >= (device_prm.startDecPos))) || 01292 ((device_prm.commandExecuted == RUN_CMD)&& 01293 (device_prm.speed > maxSpeed))) 01294 { 01295 device_prm.motionState = DECELERATING; 01296 device_prm.accu = 0; 01297 } 01298 else if ((device_prm.commandExecuted == RUN_CMD)&& 01299 (device_prm.speed < maxSpeed)) 01300 { 01301 device_prm.motionState = ACCELERATING; 01302 device_prm.accu = 0; 01303 } 01304 break; 01305 } 01306 case DECELERATING: 01307 { 01308 uint32_t relativePos = device_prm.relativePos; 01309 uint16_t speed = device_prm.speed; 01310 uint32_t deceleration = ((uint32_t)device_prm.deceleration << 16); 01311 if (((device_prm.commandExecuted == SOFT_STOP_CMD)&&(speed <= device_prm.minSpeed))|| 01312 ((device_prm.commandExecuted != RUN_CMD)&& 01313 (relativePos >= device_prm.stepsToTake))) 01314 { 01315 /* Motion process complete */ 01316 L6474_HardStop(); 01317 } 01318 else if ((device_prm.commandExecuted == RUN_CMD)&& 01319 (speed <= device_prm.maxSpeed)) 01320 { 01321 device_prm.motionState = STEADY; 01322 } 01323 else 01324 { 01325 /* Go on decelerating */ 01326 if (speed > device_prm.minSpeed) 01327 { 01328 bool speedUpdated = FALSE; 01329 if (speed == 0) speed =1; 01330 device_prm.accu += deceleration / speed; 01331 while (device_prm.accu >= (0X10000L)) 01332 { 01333 device_prm.accu -= (0X10000L); 01334 if (speed > 1) 01335 { 01336 speed -=1; 01337 } 01338 speedUpdated = TRUE; 01339 } 01340 01341 if (speedUpdated) 01342 { 01343 if (speed < device_prm.minSpeed) 01344 { 01345 speed = device_prm.minSpeed; 01346 } 01347 device_prm.speed = speed; 01348 L6474_ApplySpeed(device_prm.speed); 01349 } 01350 } 01351 } 01352 break; 01353 } 01354 default: 01355 { 01356 break; 01357 } 01358 } 01359 /* Set isr flag */ 01360 isr_flag = FALSE; 01361 } 01362 01363 /********************************************************** 01364 * @brief Converts current in mA to values for TVAL register 01365 * @param[in] current_mA current in mA 01366 * @retval value for TVAL register 01367 **********************************************************/ 01368 float L6474::L6474_Tval_Current_to_Par(float current_mA) 01369 { 01370 return ((float)(((current_mA - 31.25f) / 31.25f) + 0.5f)); 01371 } 01372 01373 /********************************************************** 01374 * @brief Converts values from TVAL register to mA 01375 * @param[in] Tval value from TVAL register 01376 * @retval current in mA 01377 **********************************************************/ 01378 float L6474::L6474_Par_to_Tval_Current(float Tval) 01379 { 01380 return ((float)((Tval - 0.5f) * 31.25f + 31.25f)); 01381 } 01382 01383 /********************************************************** 01384 * @brief Convert time in us to values for TON_MIN register 01385 * @param[in] ton_min_us time in us 01386 * @retval value for TON_MIN register 01387 **********************************************************/ 01388 float L6474::L6474_Tmin_Time_to_Par(float ton_min_us) 01389 { 01390 return ((float)(((ton_min_us - 0.5f) * 2.0f) + 0.5f)); 01391 } 01392 01393 /********************************************************** 01394 * @brief Convert values for TON_MIN register to time in us 01395 * @param[in] Tmin value from TON_MIN register 01396 * @retval time in us 01397 **********************************************************/ 01398 float L6474::L6474_Par_to_Tmin_Time(float Tmin) 01399 { 01400 return ((float)(((Tmin - 0.5f) / 2.0f) + 0.5f)); 01401 } 01402 01403 /********************************************************** 01404 * @brief Write and receive a byte via SPI 01405 * @param[in] pByteToTransmit pointer to the byte to transmit 01406 * @param[in] pReceivedByte pointer to the received byte 01407 * @retval None 01408 **********************************************************/ 01409 void L6474::L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) 01410 { 01411 if (L6474_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0) 01412 { 01413 L6474_ErrorHandler(L6474_ERROR_1); 01414 } 01415 01416 if (isr_flag) 01417 { 01418 spi_preemtion_by_isr = TRUE; 01419 } 01420 } 01421 01422 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Fri Jul 15 2022 12:57:41 by
1.7.2