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