David Lowe
X-CUBE-SPN1-20150128 example source code for one motor compiled under mbed. Tested OK on Nucleo F401. l6474.cpp is modified from original with defines in l6474_target_config.h to select the original behaviour (motor de-energised when halted), or new mode to continue powering with a (reduced) current in the coils (braking/position hold capability). On F401 avoid using mbed's InterruptIn on pins 10-15 (any port). Beware of other conflicts! L0 & F0 are included but untested.
Embed:
(wiki syntax)
Show/hide line numbers
l6474.cpp
00001 /** 00002 ****************************************************************************** 00003 * @file l6474.c 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 /* Includes ------------------------------------------------------------------*/ 00040 #include "l6474.h" 00041 00042 /* Private constants ---------------------------------------------------------*/ 00043 00044 00045 /** @addtogroup BSP 00046 * @{ 00047 */ 00048 00049 /** @defgroup L6474 00050 * @{ 00051 */ 00052 00053 /* Private constants ---------------------------------------------------------*/ 00054 00055 /** @defgroup L6474_Private_Constants 00056 * @{ 00057 */ 00058 00059 /// Error while initialising the SPI 00060 #define L6474_ERROR_0 (0x8000) 00061 /// Error: Bad SPI transaction 00062 #define L6474_ERROR_1 (0x8001) 00063 00064 /// Maximum number of steps 00065 #define MAX_STEPS (0x7FFFFFFF) 00066 00067 /// Maximum frequency of the PWMs in Hz 00068 #define L6474_MAX_PWM_FREQ (10000) 00069 00070 /// Minimum frequency of the PWMs in Hz 00071 #define L6474_MIN_PWM_FREQ (2) 00072 00073 /** 00074 * @} 00075 */ 00076 00077 /* Private variables ---------------------------------------------------------*/ 00078 00079 /** @defgroup L6474_Private_Variables 00080 * @{ 00081 */ 00082 00083 /// Function pointer to flag interrupt call back 00084 void (*flagInterruptCallback)(void); 00085 /// Function pointer to error handler call back 00086 void (*errorHandlerCallback)(uint16_t); 00087 static volatile uint8_t numberOfDevices; 00088 static uint8_t spiTxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; 00089 static uint8_t spiRxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES]; 00090 static volatile bool spiPreemtionByIsr = FALSE; 00091 static volatile bool isrFlag = FALSE; 00092 static uint16_t l6474DriverInstance = 0; 00093 00094 /// L6474 Device Paramaters structure 00095 deviceParams_t devicePrm[MAX_NUMBER_OF_DEVICES]; 00096 00097 00098 /** 00099 * @} 00100 */ 00101 00102 /* Private function prototypes -----------------------------------------------*/ 00103 00104 /** @defgroup L6474_Private_functions 00105 * @{ 00106 */ 00107 void L6474_ApplySpeed(uint8_t pwmId, uint16_t newSpeed); 00108 void L6474_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps); 00109 int32_t L6474_ConvertPosition(uint32_t abs_position_reg); 00110 void L6474_ErrorHandler(uint16_t error); 00111 void L6474_FlagInterruptHandler(void); 00112 void L6474_SendCommand(uint8_t deviceId, uint8_t param); 00113 void L6474_SetRegisterToPredefinedValues(uint8_t deviceId); 00114 void L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte); 00115 void L6474_SetDeviceParamsToPredefinedValues(void); 00116 void L6474_StartMovement(uint8_t deviceId); 00117 void L6474_StepClockHandler(uint8_t deviceId); 00118 uint8_t L6474_Tval_Current_to_Par(double Tval); 00119 uint8_t L6474_Tmin_Time_to_Par(double Tmin); 00120 00121 /** 00122 * @} 00123 */ 00124 00125 00126 /** @defgroup L6474_Exported_Variables 00127 * @{ 00128 */ 00129 00130 /// L6474 motor driver functions pointer structure 00131 motorDrv_t l6474Drv = 00132 { 00133 L6474_Init, 00134 L6474_ReadId, 00135 L6474_AttachErrorHandler, 00136 L6474_AttachFlagInterrupt, 00137 0, 00138 L6474_FlagInterruptHandler, 00139 L6474_GetAcceleration, 00140 L6474_GetCurrentSpeed, 00141 L6474_GetDeceleration, 00142 L6474_GetDeviceState, 00143 L6474_GetFwVersion, 00144 L6474_GetMark, 00145 L6474_GetMaxSpeed, 00146 L6474_GetMinSpeed, 00147 L6474_GetPosition, 00148 L6474_GoHome, 00149 L6474_GoMark, 00150 L6474_GoTo, 00151 L6474_HardStop, 00152 L6474_Move, 00153 L6474_ResetAllDevices, 00154 L6474_Run, 00155 L6474_SetAcceleration, 00156 L6474_SetDeceleration, 00157 L6474_SetHome, 00158 L6474_SetMark, 00159 L6474_SetMaxSpeed, 00160 L6474_SetMinSpeed, 00161 L6474_SoftStop, 00162 L6474_StepClockHandler, 00163 L6474_WaitWhileActive, 00164 L6474_CmdDisable, 00165 L6474_CmdEnable, 00166 L6474_CmdGetParam, 00167 L6474_CmdGetStatus, 00168 L6474_CmdNop, 00169 L6474_CmdSetParam, 00170 L6474_ReadStatusRegister, 00171 L6474_ReleaseReset, 00172 L6474_Reset, 00173 L6474_SelectStepMode, 00174 L6474_SetDirection, 00175 0, 00176 0, 00177 0, 00178 0, 00179 0, 00180 0, 00181 0, 00182 0, 00183 0, 00184 0, 00185 0, 00186 0, 00187 0, 00188 0, 00189 0, 00190 0, 00191 0, 00192 0, 00193 L6474_ErrorHandler, 00194 0 00195 }; 00196 00197 /** 00198 * @} 00199 */ 00200 00201 /** @defgroup Device_Control_Functions 00202 * @{ 00203 */ 00204 00205 /******************************************************//** 00206 * @brief Attaches a user callback to the error Handler. 00207 * The call back will be then called each time the library 00208 * detects an error 00209 * @param[in] callback Name of the callback to attach 00210 * to the error Hanlder 00211 * @retval None 00212 **********************************************************/ 00213 void L6474_AttachErrorHandler(void (*callback)(uint16_t)) 00214 { 00215 errorHandlerCallback = (void (*)(uint16_t))callback; 00216 } 00217 00218 /******************************************************//** 00219 * @brief Attaches a user callback to the flag Interrupt 00220 * The call back will be then called each time the status 00221 * flag pin will be pulled down due to the occurrence of 00222 * a programmed alarms ( OCD, thermal pre-warning or 00223 * shutdown, UVLO, wrong command, non-performable command) 00224 * @param[in] callback Name of the callback to attach 00225 * to the Flag Interrupt 00226 * @retval None 00227 **********************************************************/ 00228 void L6474_AttachFlagInterrupt(void (*callback)(void)) 00229 { 00230 flagInterruptCallback = (void (*)())callback; 00231 } 00232 00233 /******************************************************//** 00234 * @brief Starts the L6474 library 00235 * @param[in] nbDevices Number of L6474 devices to use (from 1 to 3) 00236 * @retval None 00237 **********************************************************/ 00238 void L6474_Init(uint8_t nbDevices) 00239 { 00240 uint32_t i; 00241 numberOfDevices = nbDevices; 00242 00243 l6474DriverInstance++; 00244 00245 /* Initialise the GPIOs */ 00246 BSP_MotorControlBoard_GpioInit(nbDevices); 00247 00248 if(BSP_MotorControlBoard_SpiInit() != 0) 00249 { 00250 /* Initialization Error */ 00251 L6474_ErrorHandler(L6474_ERROR_0); 00252 } 00253 00254 /* Initialise the PWMs used for the Step clocks ----------------------------*/ 00255 switch (nbDevices) 00256 { 00257 case 3: 00258 BSP_MotorControlBoard_PwmInit(2); 00259 case 2: 00260 BSP_MotorControlBoard_PwmInit(1); 00261 case 1: 00262 BSP_MotorControlBoard_PwmInit(0); 00263 default: 00264 ; 00265 } 00266 00267 /* Initialise the L6474s ------------------------------------------------*/ 00268 00269 /* Standby-reset deactivation */ 00270 BSP_MotorControlBoard_ReleaseReset(); 00271 00272 /* Let a delay after reset */ 00273 BSP_MotorControlBoard_Delay(1); 00274 00275 /* Set all registers and context variables to the predefined values from l6474_target_config.h */ 00276 L6474_SetDeviceParamsToPredefinedValues(); 00277 00278 /* Disable L6474 powerstage */ 00279 for (i = 0; i < nbDevices; i++) 00280 { 00281 L6474_CmdDisable(i); 00282 /* Get Status to clear flags after start up */ 00283 L6474_CmdGetStatus(i); 00284 } 00285 } 00286 00287 /******************************************************//** 00288 * @brief Returns the acceleration of the specified device 00289 * @param[in] deviceId (from 0 to 2) 00290 * @retval Acceleration in pps^2 00291 **********************************************************/ 00292 uint16_t L6474_GetAcceleration(uint8_t deviceId) 00293 { 00294 return (devicePrm[deviceId].acceleration); 00295 } 00296 00297 /******************************************************//** 00298 * @brief Returns the current speed of the specified device 00299 * @param[in] deviceId (from 0 to 2) 00300 * @retval Speed in pps 00301 **********************************************************/ 00302 uint16_t L6474_GetCurrentSpeed(uint8_t deviceId) 00303 { 00304 return devicePrm[deviceId].speed; 00305 } 00306 00307 /******************************************************//** 00308 * @brief Returns the deceleration of the specified device 00309 * @param[in] deviceId (from 0 to 2) 00310 * @retval Deceleration in pps^2 00311 **********************************************************/ 00312 uint16_t L6474_GetDeceleration(uint8_t deviceId) 00313 { 00314 return (devicePrm[deviceId].deceleration); 00315 } 00316 00317 /******************************************************//** 00318 * @brief Returns the device state 00319 * @param[in] deviceId (from 0 to 2) 00320 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) 00321 **********************************************************/ 00322 motorState_t L6474_GetDeviceState(uint8_t deviceId) 00323 { 00324 return devicePrm[deviceId].motionState; 00325 } 00326 00327 /******************************************************//** 00328 * @brief Returns the FW version of the library 00329 * @param None 00330 * @retval L6474_FW_VERSION 00331 **********************************************************/ 00332 uint8_t L6474_GetFwVersion(void) 00333 { 00334 return (L6474_FW_VERSION); 00335 } 00336 00337 /******************************************************//** 00338 * @brief Return motor handle (pointer to the L6474 motor driver structure) 00339 * @param None 00340 * @retval Pointer to the motorDrv_t structure 00341 **********************************************************/ 00342 motorDrv_t* L6474_GetMotorHandle(void) 00343 { 00344 return (&l6474Drv); 00345 } 00346 00347 /******************************************************//** 00348 * @brief Returns the mark position of the specified device 00349 * @param[in] deviceId (from 0 to 2) 00350 * @retval Mark register value converted in a 32b signed integer 00351 **********************************************************/ 00352 int32_t L6474_GetMark(uint8_t deviceId) 00353 { 00354 return L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_MARK)); 00355 } 00356 00357 /******************************************************//** 00358 * @brief Returns the max speed of the specified device 00359 * @param[in] deviceId (from 0 to 2) 00360 * @retval maxSpeed in pps 00361 **********************************************************/ 00362 uint16_t L6474_GetMaxSpeed(uint8_t deviceId) 00363 { 00364 return (devicePrm[deviceId].maxSpeed); 00365 } 00366 00367 /******************************************************//** 00368 * @brief Returns the min speed of the specified device 00369 * @param[in] deviceId (from 0 to 2) 00370 * @retval minSpeed in pps 00371 **********************************************************/ 00372 uint16_t L6474_GetMinSpeed(uint8_t deviceId) 00373 { 00374 return (devicePrm[deviceId].minSpeed); 00375 } 00376 00377 /******************************************************//** 00378 * @brief Returns the ABS_POSITION of the specified device 00379 * @param[in] deviceId (from 0 to 2) 00380 * @retval ABS_POSITION register value converted in a 32b signed integer 00381 **********************************************************/ 00382 int32_t L6474_GetPosition(uint8_t deviceId) 00383 { 00384 return L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); 00385 } 00386 00387 00388 00389 /******************************************************//** 00390 * @brief Requests the motor to move to the home position (ABS_POSITION = 0) 00391 * @param[in] deviceId (from 0 to 2) 00392 * @retval None 00393 **********************************************************/ 00394 void L6474_GoHome(uint8_t deviceId) 00395 { 00396 L6474_GoTo(deviceId, 0); 00397 } 00398 00399 /******************************************************//** 00400 * @brief Requests the motor to move to the mark position 00401 * @param[in] deviceId (from 0 to 2) 00402 * @retval None 00403 **********************************************************/ 00404 void L6474_GoMark(uint8_t deviceId) 00405 { 00406 uint32_t mark; 00407 00408 mark = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_MARK)); 00409 L6474_GoTo(deviceId,mark); 00410 } 00411 00412 /******************************************************//** 00413 * @brief Requests the motor to move to the specified position 00414 * @param[in] deviceId (from 0 to 2) 00415 * @param[in] targetPosition absolute position in steps 00416 * @retval None 00417 **********************************************************/ 00418 void L6474_GoTo(uint8_t deviceId, int32_t targetPosition) 00419 { 00420 motorDir_t direction; 00421 int32_t steps; 00422 00423 /* Eventually deactivate motor */ 00424 if (devicePrm[deviceId].motionState != INACTIVE) 00425 { 00426 L6474_HardStop(deviceId); 00427 } 00428 00429 /* Get current position */ 00430 devicePrm[deviceId].currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); 00431 00432 /* Compute the number of steps to perform */ 00433 steps = targetPosition - devicePrm[deviceId].currentPosition; 00434 00435 if (steps >= 0) 00436 { 00437 devicePrm[deviceId].stepsToTake = steps; 00438 direction = FORWARD; 00439 00440 } 00441 else 00442 { 00443 devicePrm[deviceId].stepsToTake = -steps; 00444 direction = BACKWARD; 00445 } 00446 00447 if (steps != 0) 00448 { 00449 00450 devicePrm[deviceId].commandExecuted = MOVE_CMD; 00451 00452 /* Direction setup */ 00453 L6474_SetDirection(deviceId,direction); 00454 00455 L6474_ComputeSpeedProfile(deviceId, devicePrm[deviceId].stepsToTake); 00456 00457 /* Motor activation */ 00458 L6474_StartMovement(deviceId); 00459 } 00460 } 00461 00462 /******************************************************//** 00463 * @brief Immediatly stops the motor and disable the power bridge 00464 * @param[in] deviceId (from 0 to 2) 00465 * @retval None 00466 **********************************************************/ 00467 void L6474_HardStop(uint8_t deviceId) 00468 { 00469 /* Disable corresponding PWM */ 00470 BSP_MotorControlBoard_PwmStop(deviceId); 00471 00472 /* Disable power stage */ 00473 #ifndef L6474_CONF_BRAKE_WHEN_HALTED 00474 L6474_CmdDisable(deviceId); 00475 #else 00476 /* Set powerstage to reduced current, retaining position */ 00477 switch (deviceId) 00478 { 00479 case 0: 00480 L6474_CmdSetParam(deviceId, 00481 L6474_TVAL, 00482 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0/L6474_CONF_BRAKE_CURRENT_FACTOR)); 00483 break; 00484 case 1: 00485 L6474_CmdSetParam(deviceId, 00486 L6474_TVAL, 00487 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1/L6474_CONF_BRAKE_CURRENT_FACTOR)); 00488 break; 00489 case 2: 00490 L6474_CmdSetParam(deviceId, 00491 L6474_TVAL, 00492 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2/L6474_CONF_BRAKE_CURRENT_FACTOR)); 00493 break; 00494 default: ; 00495 } 00496 #endif 00497 /* Set inactive state */ 00498 devicePrm[deviceId].motionState = INACTIVE; 00499 devicePrm[deviceId].commandExecuted = NO_CMD; 00500 devicePrm[deviceId].stepsToTake = MAX_STEPS; 00501 } 00502 00503 /******************************************************//** 00504 * @brief Moves the motor of the specified number of steps 00505 * @param[in] deviceId (from 0 to 2) 00506 * @param[in] direction FORWARD or BACKWARD 00507 * @param[in] stepCount Number of steps to perform 00508 * @retval None 00509 **********************************************************/ 00510 void L6474_Move(uint8_t deviceId, motorDir_t direction, uint32_t stepCount) 00511 { 00512 /* Eventually deactivate motor */ 00513 if (devicePrm[deviceId].motionState != INACTIVE) 00514 { 00515 L6474_HardStop(deviceId); 00516 } 00517 00518 if (stepCount != 0) 00519 { 00520 devicePrm[deviceId].stepsToTake = stepCount; 00521 00522 devicePrm[deviceId].commandExecuted = MOVE_CMD; 00523 00524 devicePrm[deviceId].currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(deviceId,L6474_ABS_POS)); 00525 00526 /* Direction setup */ 00527 L6474_SetDirection(deviceId,direction); 00528 00529 L6474_ComputeSpeedProfile(deviceId, stepCount); 00530 00531 /* Motor activation */ 00532 L6474_StartMovement(deviceId); 00533 } 00534 } 00535 00536 /******************************************************//** 00537 * @brief Read id 00538 * @param None 00539 * @retval Id of the l6474 Driver Instance 00540 **********************************************************/ 00541 uint16_t L6474_ReadId(void) 00542 { 00543 return(l6474DriverInstance); 00544 } 00545 00546 /******************************************************//** 00547 * @brief Resets all L6474 devices 00548 * @param None 00549 * @retval None 00550 **********************************************************/ 00551 void L6474_ResetAllDevices(void) 00552 { 00553 uint8_t loop; 00554 00555 for (loop = 0; loop < numberOfDevices; loop++) 00556 { 00557 /* Stop movement and disable power stage*/ 00558 L6474_HardStop(loop); 00559 #ifdef L6474_CONF_BRAKE_WHEN_HALTED 00560 /* Disable corresponding PWM */ 00561 BSP_MotorControlBoard_PwmStop(loop); 00562 00563 /* Disable power stage */ 00564 L6474_CmdDisable(loop); 00565 #endif 00566 } 00567 L6474_Reset(); 00568 BSP_MotorControlBoard_Delay(1); // Reset pin must be forced low for at least 10us 00569 BSP_MotorControlBoard_ReleaseReset(); 00570 BSP_MotorControlBoard_Delay(1); 00571 } 00572 00573 /******************************************************//** 00574 * @brief Runs the motor. It will accelerate from the min 00575 * speed up to the max speed by using the device acceleration. 00576 * @param[in] deviceId (from 0 to 2) 00577 * @param[in] direction FORWARD or BACKWARD 00578 * @retval None 00579 **********************************************************/ 00580 void L6474_Run(uint8_t deviceId, motorDir_t direction) 00581 { 00582 /* Eventually deactivate motor */ 00583 if (devicePrm[deviceId].motionState != INACTIVE) 00584 { 00585 L6474_HardStop(deviceId); 00586 } 00587 00588 /* Direction setup */ 00589 L6474_SetDirection(deviceId,direction); 00590 00591 devicePrm[deviceId].commandExecuted = RUN_CMD; 00592 00593 /* Motor activation */ 00594 L6474_StartMovement(deviceId); 00595 } 00596 00597 /******************************************************//** 00598 * @brief Changes the acceleration of the specified device 00599 * @param[in] deviceId (from 0 to 2) 00600 * @param[in] newAcc New acceleration to apply in pps^2 00601 * @retval true if the command is successfully executed, else false 00602 * @note The command is not performed is the device is executing 00603 * a MOVE or GOTO command (but it can be used during a RUN command) 00604 **********************************************************/ 00605 bool L6474_SetAcceleration(uint8_t deviceId,uint16_t newAcc) 00606 { 00607 bool cmdExecuted = FALSE; 00608 if ((newAcc != 0)&& 00609 ((devicePrm[deviceId].motionState == INACTIVE)|| 00610 (devicePrm[deviceId].commandExecuted == RUN_CMD))) 00611 { 00612 devicePrm[deviceId].acceleration = newAcc; 00613 cmdExecuted = TRUE; 00614 } 00615 return cmdExecuted; 00616 } 00617 00618 /******************************************************//** 00619 * @brief Changes the deceleration of the specified device 00620 * @param[in] deviceId (from 0 to 2) 00621 * @param[in] newDec New deceleration to apply in pps^2 00622 * @retval true if the command is successfully executed, else false 00623 * @note The command is not performed is the device is executing 00624 * a MOVE or GOTO command (but it can be used during a RUN command) 00625 **********************************************************/ 00626 bool L6474_SetDeceleration(uint8_t deviceId, uint16_t newDec) 00627 { 00628 bool cmdExecuted = FALSE; 00629 if ((newDec != 0)&& 00630 ((devicePrm[deviceId].motionState == INACTIVE)|| 00631 (devicePrm[deviceId].commandExecuted == RUN_CMD))) 00632 { 00633 devicePrm[deviceId].deceleration = newDec; 00634 cmdExecuted = TRUE; 00635 } 00636 return cmdExecuted; 00637 } 00638 00639 /******************************************************//** 00640 * @brief Set current position to be the Home position (ABS pos set to 0) 00641 * @param[in] deviceId (from 0 to 2) 00642 * @retval None 00643 **********************************************************/ 00644 void L6474_SetHome(uint8_t deviceId) 00645 { 00646 L6474_CmdSetParam(deviceId, L6474_ABS_POS, 0); 00647 } 00648 00649 /******************************************************//** 00650 * @brief Sets current position to be the Mark position 00651 * @param[in] deviceId (from 0 to 2) 00652 * @retval None 00653 **********************************************************/ 00654 void L6474_SetMark(uint8_t deviceId) 00655 { 00656 uint32_t mark = L6474_CmdGetParam(deviceId,L6474_ABS_POS); 00657 L6474_CmdSetParam(deviceId,L6474_MARK, mark); 00658 } 00659 00660 /******************************************************//** 00661 * @brief Changes the max speed of the specified device 00662 * @param[in] deviceId (from 0 to 2) 00663 * @param[in] newMaxSpeed New max speed to apply in pps 00664 * @retval true if the command is successfully executed, else false 00665 * @note The command is not performed is the device is executing 00666 * a MOVE or GOTO command (but it can be used during a RUN command). 00667 **********************************************************/ 00668 bool L6474_SetMaxSpeed(uint8_t deviceId, uint16_t newMaxSpeed) 00669 { 00670 bool cmdExecuted = FALSE; 00671 if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&& 00672 (newMaxSpeed <= L6474_MAX_PWM_FREQ) && 00673 (devicePrm[deviceId].minSpeed <= newMaxSpeed) && 00674 ((devicePrm[deviceId].motionState == INACTIVE)|| 00675 (devicePrm[deviceId].commandExecuted == RUN_CMD))) 00676 { 00677 devicePrm[deviceId].maxSpeed = newMaxSpeed; 00678 cmdExecuted = TRUE; 00679 } 00680 return cmdExecuted; 00681 } 00682 00683 /******************************************************//** 00684 * @brief Changes the min speed of the specified device 00685 * @param[in] deviceId (from 0 to 2) 00686 * @param[in] newMinSpeed New min speed to apply in pps 00687 * @retval true if the command is successfully executed, else false 00688 * @note The command is not performed is the device is executing 00689 * a MOVE or GOTO command (but it can be used during a RUN command). 00690 **********************************************************/ 00691 bool L6474_SetMinSpeed(uint8_t deviceId, uint16_t newMinSpeed) 00692 { 00693 bool cmdExecuted = FALSE; 00694 if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&& 00695 (newMinSpeed <= L6474_MAX_PWM_FREQ) && 00696 (newMinSpeed <= devicePrm[deviceId].maxSpeed) && 00697 ((devicePrm[deviceId].motionState == INACTIVE)|| 00698 (devicePrm[deviceId].commandExecuted == RUN_CMD))) 00699 { 00700 devicePrm[deviceId].minSpeed = newMinSpeed; 00701 cmdExecuted = TRUE; 00702 } 00703 return cmdExecuted; 00704 } 00705 00706 /******************************************************//** 00707 * @brief Stops the motor by using the device deceleration 00708 * @param[in] deviceId (from 0 to 2) 00709 * @retval true if the command is successfully executed, else false 00710 * @note The command is not performed is the device is in INACTIVE state. 00711 **********************************************************/ 00712 bool L6474_SoftStop(uint8_t deviceId) 00713 { 00714 bool cmdExecuted = FALSE; 00715 if (devicePrm[deviceId].motionState != INACTIVE) 00716 { 00717 devicePrm[deviceId].commandExecuted = SOFT_STOP_CMD; 00718 cmdExecuted = TRUE; 00719 } 00720 return (cmdExecuted); 00721 } 00722 00723 /******************************************************//** 00724 * @brief Locks until the device state becomes Inactive 00725 * @param[in] deviceId (from 0 to 2) 00726 * @retval None 00727 **********************************************************/ 00728 void L6474_WaitWhileActive(uint8_t deviceId) 00729 { 00730 /* Wait while motor is running */ 00731 while (L6474_GetDeviceState(deviceId) != INACTIVE); 00732 } 00733 00734 /** 00735 * @} 00736 */ 00737 00738 /** @defgroup L6474_Control_Functions 00739 * @{ 00740 */ 00741 00742 /******************************************************//** 00743 * @brief Issue the Disable command to the L6474 of the specified device 00744 * @param[in] deviceId (from 0 to 2) 00745 * @retval None 00746 **********************************************************/ 00747 void L6474_CmdDisable(uint8_t deviceId) 00748 { 00749 L6474_SendCommand(deviceId, L6474_DISABLE); 00750 } 00751 00752 /******************************************************//** 00753 * @brief Issues the Enable command to the L6474 of the specified device 00754 * @param[in] deviceId (from 0 to 2) 00755 * @retval None 00756 **********************************************************/ 00757 void L6474_CmdEnable(uint8_t deviceId) 00758 { 00759 L6474_SendCommand(deviceId, L6474_ENABLE); 00760 } 00761 00762 /******************************************************//** 00763 * @brief Issues the GetParam command to the L6474 of the specified device 00764 * @param[in] deviceId (from 0 to 2) 00765 * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) 00766 * @retval Register value 00767 **********************************************************/ 00768 uint32_t L6474_CmdGetParam(uint8_t deviceId, uint32_t param) 00769 { 00770 uint32_t i; 00771 uint32_t spiRxData; 00772 uint8_t maxArgumentNbBytes = 0; 00773 uint8_t spiIndex = numberOfDevices - deviceId - 1; 00774 bool itDisable = FALSE; 00775 00776 do 00777 { 00778 spiPreemtionByIsr = FALSE; 00779 if (itDisable) 00780 { 00781 /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ 00782 BSP_MotorControlBoard_EnableIrq(); 00783 itDisable = FALSE; 00784 } 00785 00786 for (i = 0; i < numberOfDevices; i++) 00787 { 00788 spiTxBursts[0][i] = L6474_NOP; 00789 spiTxBursts[1][i] = L6474_NOP; 00790 spiTxBursts[2][i] = L6474_NOP; 00791 spiTxBursts[3][i] = L6474_NOP; 00792 spiRxBursts[1][i] = 0; 00793 spiRxBursts[2][i] = 0; 00794 spiRxBursts[3][i] = 0; 00795 } 00796 switch (param) 00797 { 00798 case L6474_ABS_POS: ; 00799 case L6474_MARK: 00800 spiTxBursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); 00801 maxArgumentNbBytes = 3; 00802 break; 00803 case L6474_EL_POS: ; 00804 case L6474_CONFIG: ; 00805 case L6474_STATUS: 00806 spiTxBursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); 00807 maxArgumentNbBytes = 2; 00808 break; 00809 default: 00810 spiTxBursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (param); 00811 maxArgumentNbBytes = 1; 00812 } 00813 00814 /* Disable interruption before checking */ 00815 /* pre-emption by ISR and SPI transfers*/ 00816 BSP_MotorControlBoard_DisableIrq(); 00817 itDisable = TRUE; 00818 } while (spiPreemtionByIsr); // check pre-emption by ISR 00819 00820 for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00821 i < L6474_CMD_ARG_MAX_NB_BYTES; 00822 i++) 00823 { 00824 L6474_WriteBytes(&spiTxBursts[i][0], 00825 &spiRxBursts[i][0]); 00826 } 00827 00828 spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)| 00829 (spiRxBursts[2][spiIndex] << 8) | 00830 (spiRxBursts[3][spiIndex]); 00831 00832 /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ 00833 BSP_MotorControlBoard_EnableIrq(); 00834 00835 return (spiRxData); 00836 } 00837 00838 /******************************************************//** 00839 * @brief Issues the GetStatus command to the L6474 of the specified device 00840 * @param[in] deviceId (from 0 to 2) 00841 * @retval Status Register value 00842 * @note Once the GetStatus command is performed, the flags of the status register 00843 * are reset. This is not the case when the status register is read with the 00844 * GetParam command (via the functions L6474ReadStatusRegister or L6474CmdGetParam). 00845 **********************************************************/ 00846 uint16_t L6474_CmdGetStatus(uint8_t deviceId) 00847 { 00848 uint32_t i; 00849 uint16_t status; 00850 uint8_t spiIndex = numberOfDevices - deviceId - 1; 00851 bool itDisable = FALSE; 00852 00853 do 00854 { 00855 spiPreemtionByIsr = FALSE; 00856 if (itDisable) 00857 { 00858 /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ 00859 BSP_MotorControlBoard_EnableIrq(); 00860 itDisable = FALSE; 00861 } 00862 00863 for (i = 0; i < numberOfDevices; i++) 00864 { 00865 spiTxBursts[0][i] = L6474_NOP; 00866 spiTxBursts[1][i] = L6474_NOP; 00867 spiTxBursts[2][i] = L6474_NOP; 00868 spiRxBursts[1][i] = 0; 00869 spiRxBursts[2][i] = 0; 00870 } 00871 spiTxBursts[0][spiIndex] = L6474_GET_STATUS; 00872 00873 /* Disable interruption before checking */ 00874 /* pre-emption by ISR and SPI transfers*/ 00875 BSP_MotorControlBoard_DisableIrq(); 00876 itDisable = TRUE; 00877 } while (spiPreemtionByIsr); // check pre-emption by ISR 00878 00879 for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++) 00880 { 00881 L6474_WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); 00882 } 00883 status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]); 00884 00885 /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ 00886 BSP_MotorControlBoard_EnableIrq(); 00887 00888 return (status); 00889 } 00890 00891 /******************************************************//** 00892 * @brief Issues the Nop command to the L6474 of the specified device 00893 * @param[in] deviceId (from 0 to 2) 00894 * @retval None 00895 **********************************************************/ 00896 void L6474_CmdNop(uint8_t deviceId) 00897 { 00898 L6474_SendCommand(deviceId, L6474_NOP); 00899 } 00900 00901 /******************************************************//** 00902 * @brief Issues the SetParam command to the L6474 of the specified device 00903 * @param[in] deviceId (from 0 to 2) 00904 * @param[in] param Register adress (L6474_ABS_POS, L6474_MARK,...) 00905 * @param[in] value Value to set in the register 00906 * @retval None 00907 **********************************************************/ 00908 void L6474_CmdSetParam(uint8_t deviceId, 00909 uint32_t param, 00910 uint32_t value) 00911 { 00912 uint32_t i; 00913 uint8_t maxArgumentNbBytes = 0; 00914 uint8_t spiIndex = numberOfDevices - deviceId - 1; 00915 bool itDisable = FALSE; 00916 do 00917 { 00918 spiPreemtionByIsr = FALSE; 00919 if (itDisable) 00920 { 00921 /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ 00922 BSP_MotorControlBoard_EnableIrq(); 00923 itDisable = FALSE; 00924 } 00925 for (i = 0; i < numberOfDevices; i++) 00926 { 00927 spiTxBursts[0][i] = L6474_NOP; 00928 spiTxBursts[1][i] = L6474_NOP; 00929 spiTxBursts[2][i] = L6474_NOP; 00930 spiTxBursts[3][i] = L6474_NOP; 00931 } 00932 switch (param) 00933 { 00934 case L6474_ABS_POS: ; 00935 case L6474_MARK: 00936 spiTxBursts[0][spiIndex] = param; 00937 spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16); 00938 spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); 00939 maxArgumentNbBytes = 3; 00940 break; 00941 case L6474_EL_POS: ; 00942 case L6474_CONFIG: 00943 spiTxBursts[1][spiIndex] = param; 00944 spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); 00945 maxArgumentNbBytes = 2; 00946 break; 00947 default: 00948 spiTxBursts[2][spiIndex] = param; 00949 maxArgumentNbBytes = 1; 00950 } 00951 spiTxBursts[3][spiIndex] = (uint8_t)(value); 00952 00953 /* Disable interruption before checking */ 00954 /* pre-emption by ISR and SPI transfers*/ 00955 BSP_MotorControlBoard_DisableIrq(); 00956 itDisable = TRUE; 00957 } while (spiPreemtionByIsr); // check pre-emption by ISR 00958 00959 /* SPI transfer */ 00960 for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00961 i < L6474_CMD_ARG_MAX_NB_BYTES; 00962 i++) 00963 { 00964 L6474_WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]); 00965 } 00966 /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ 00967 BSP_MotorControlBoard_EnableIrq(); 00968 } 00969 00970 /******************************************************//** 00971 * @brief Reads the Status Register value 00972 * @param[in] deviceId (from 0 to 2) 00973 * @retval Status register valued 00974 * @note The status register flags are not cleared 00975 * at the difference with L6474CmdGetStatus() 00976 **********************************************************/ 00977 uint16_t L6474_ReadStatusRegister(uint8_t deviceId) 00978 { 00979 return (L6474_CmdGetParam(deviceId,L6474_STATUS)); 00980 } 00981 00982 /******************************************************//** 00983 * @brief Releases the L6474 reset (pin set to High) of all devices 00984 * @param None 00985 * @retval None 00986 **********************************************************/ 00987 void L6474_ReleaseReset(void) 00988 { 00989 BSP_MotorControlBoard_ReleaseReset(); 00990 } 00991 00992 /******************************************************//** 00993 * @brief Resets the L6474 (reset pin set to low) of all devices 00994 * @param None 00995 * @retval None 00996 **********************************************************/ 00997 void L6474_Reset(void) 00998 { 00999 BSP_MotorControlBoard_Reset(); 01000 } 01001 01002 /******************************************************//** 01003 * @brief Set the stepping mode 01004 * @param[in] deviceId (from 0 to 2) 01005 * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t 01006 * @retval None 01007 **********************************************************/ 01008 void L6474_SelectStepMode(uint8_t deviceId, motorStepMode_t stepMod) 01009 { 01010 uint8_t stepModeRegister; 01011 L6474_STEP_SEL_t l6474StepMod; 01012 01013 switch (stepMod) 01014 { 01015 case STEP_MODE_FULL: 01016 l6474StepMod = L6474_STEP_SEL_1; 01017 break; 01018 case STEP_MODE_HALF: 01019 l6474StepMod = L6474_STEP_SEL_1_2; 01020 break; 01021 case STEP_MODE_1_4: 01022 l6474StepMod = L6474_STEP_SEL_1_4; 01023 break; 01024 case STEP_MODE_1_8: 01025 l6474StepMod = L6474_STEP_SEL_1_8; 01026 break; 01027 case STEP_MODE_1_16: 01028 default: 01029 l6474StepMod = L6474_STEP_SEL_1_16; 01030 break; 01031 } 01032 01033 /* Eventually deactivate motor */ 01034 if (devicePrm[deviceId].motionState != INACTIVE) 01035 { 01036 L6474_HardStop(deviceId); 01037 } 01038 01039 /* Read Step mode register and clear STEP_SEL field */ 01040 stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(deviceId,L6474_STEP_MODE)) ; 01041 01042 /* Apply new step mode */ 01043 L6474_CmdSetParam(deviceId, L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod); 01044 01045 /* Reset abs pos register */ 01046 L6474_SetHome(deviceId); 01047 } 01048 01049 /******************************************************//** 01050 * @brief Specifies the direction 01051 * @param[in] deviceId (from 0 to 2) 01052 * @param[in] dir FORWARD or BACKWARD 01053 * @note The direction change is only applied if the device 01054 * is in INACTIVE state 01055 * @retval None 01056 **********************************************************/ 01057 void L6474_SetDirection(uint8_t deviceId, motorDir_t dir) 01058 { 01059 if (devicePrm[deviceId].motionState == INACTIVE) 01060 { 01061 devicePrm[deviceId].direction = dir; 01062 BSP_MotorControlBoard_SetDirectionGpio(deviceId, dir); 01063 } 01064 } 01065 01066 /** 01067 * @} 01068 */ 01069 01070 /** @addtogroup L6474_Private_functions 01071 * @{ 01072 */ 01073 01074 /******************************************************//** 01075 * @brief Updates the current speed of the device 01076 * @param[in] deviceId (from 0 to 2) 01077 * @param[in] newSpeed in pps 01078 * @retval None 01079 **********************************************************/ 01080 void L6474_ApplySpeed(uint8_t deviceId, uint16_t newSpeed) 01081 { 01082 if (newSpeed < L6474_MIN_PWM_FREQ) 01083 { 01084 newSpeed = L6474_MIN_PWM_FREQ; 01085 } 01086 if (newSpeed > L6474_MAX_PWM_FREQ) 01087 { 01088 newSpeed = L6474_MAX_PWM_FREQ; 01089 } 01090 01091 devicePrm[deviceId].speed = newSpeed; 01092 01093 switch (deviceId) 01094 { 01095 case 0: 01096 BSP_MotorControlBoard_Pwm1SetFreq(newSpeed); 01097 break; 01098 case 1: 01099 BSP_MotorControlBoard_Pwm2SetFreq(newSpeed); 01100 break; 01101 case 2: 01102 BSP_MotorControlBoard_Pwm3SetFreq(newSpeed); 01103 break; 01104 default: 01105 break; //ignore error 01106 } 01107 } 01108 01109 /******************************************************//** 01110 * @brief Computes the speed profile according to the number of steps to move 01111 * @param[in] deviceId (from 0 to 2) 01112 * @param[in] nbSteps number of steps to perform 01113 * @retval None 01114 * @note Using the acceleration and deceleration of the device, 01115 * this function determines the duration in steps of the acceleration, 01116 * steady and deceleration phases. 01117 * If the total number of steps to perform is big enough, a trapezoidal move 01118 * is performed (i.e. there is a steady phase where the motor runs at the maximum 01119 * speed. 01120 * Else, a triangular move is performed (no steady phase: the maximum speed is never 01121 * reached. 01122 **********************************************************/ 01123 void L6474_ComputeSpeedProfile(uint8_t deviceId, uint32_t nbSteps) 01124 { 01125 uint32_t reqAccSteps; 01126 uint32_t reqDecSteps; 01127 01128 /* compute the number of steps to get the targeted speed */ 01129 uint16_t minSpeed = devicePrm[deviceId].minSpeed; 01130 reqAccSteps = (devicePrm[deviceId].maxSpeed - minSpeed); 01131 reqAccSteps *= (devicePrm[deviceId].maxSpeed + minSpeed); 01132 reqDecSteps = reqAccSteps; 01133 reqAccSteps /= (uint32_t)devicePrm[deviceId].acceleration; 01134 reqAccSteps /= 2; 01135 01136 /* compute the number of steps to stop */ 01137 reqDecSteps /= (uint32_t)devicePrm[deviceId].deceleration; 01138 reqDecSteps /= 2; 01139 01140 if(( reqAccSteps + reqDecSteps ) > nbSteps) 01141 { 01142 /* Triangular move */ 01143 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 01144 uint32_t dec = devicePrm[deviceId].deceleration; 01145 uint32_t acc = devicePrm[deviceId].acceleration; 01146 01147 reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec); 01148 if (reqDecSteps > 1) 01149 { 01150 reqAccSteps = reqDecSteps - 1; 01151 if(reqAccSteps == 0) 01152 { 01153 reqAccSteps = 1; 01154 } 01155 } 01156 else 01157 { 01158 reqAccSteps = 0; 01159 } 01160 devicePrm[deviceId].endAccPos = reqAccSteps; 01161 devicePrm[deviceId].startDecPos = reqDecSteps; 01162 } 01163 else 01164 { 01165 /* Trapezoidal move */ 01166 /* accelerating phase to endAccPos */ 01167 /* steady phase from endAccPos to startDecPos */ 01168 /* decelerating from startDecPos to stepsToTake*/ 01169 devicePrm[deviceId].endAccPos = reqAccSteps; 01170 devicePrm[deviceId].startDecPos = nbSteps - reqDecSteps - 1; 01171 } 01172 } 01173 01174 /******************************************************//** 01175 * @brief Converts the ABS_POSITION register value to a 32b signed integer 01176 * @param[in] abs_position_reg value of the ABS_POSITION register 01177 * @retval operation_result 32b signed integer corresponding to the absolute position 01178 **********************************************************/ 01179 int32_t L6474_ConvertPosition(uint32_t abs_position_reg) 01180 { 01181 int32_t operation_result; 01182 01183 if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) 01184 { 01185 /* Negative register value */ 01186 abs_position_reg = ~abs_position_reg; 01187 abs_position_reg += 1; 01188 01189 operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK); 01190 operation_result = -operation_result; 01191 } 01192 else 01193 { 01194 operation_result = (int32_t) abs_position_reg; 01195 } 01196 return operation_result; 01197 } 01198 01199 /******************************************************//** 01200 * @brief Error handler which calls the user callback (if defined) 01201 * @param[in] error Number of the error 01202 * @retval None 01203 **********************************************************/ 01204 void L6474_ErrorHandler(uint16_t error) 01205 { 01206 if (errorHandlerCallback != 0) 01207 { 01208 (void) errorHandlerCallback(error); 01209 } 01210 else 01211 { 01212 while(1) 01213 { 01214 /* Infinite loop */ 01215 } 01216 } 01217 } 01218 01219 /******************************************************//** 01220 * @brief Handlers of the flag interrupt which calls the user callback (if defined) 01221 * @param None 01222 * @retval None 01223 **********************************************************/ 01224 void L6474_FlagInterruptHandler(void) 01225 { 01226 if (flagInterruptCallback != 0) 01227 { 01228 /* Set isr flag */ 01229 isrFlag = TRUE; 01230 01231 flagInterruptCallback(); 01232 01233 /* Reset isr flag */ 01234 isrFlag = FALSE; 01235 } 01236 } 01237 01238 /******************************************************//** 01239 * @brief Sends a command without arguments to the L6474 via the SPI 01240 * @param[in] deviceId (from 0 to 2) 01241 * @param[in] param Command to send 01242 * @retval None 01243 **********************************************************/ 01244 void L6474_SendCommand(uint8_t deviceId, uint8_t param) 01245 { 01246 uint32_t i; 01247 uint8_t spiIndex = numberOfDevices - deviceId - 1; 01248 bool itDisable = FALSE; 01249 01250 do 01251 { 01252 spiPreemtionByIsr = FALSE; 01253 if (itDisable) 01254 { 01255 /* re-enable BSP_MotorControlBoard_EnableIrq if disable in previous iteration */ 01256 BSP_MotorControlBoard_EnableIrq(); 01257 itDisable = FALSE; 01258 } 01259 01260 for (i = 0; i < numberOfDevices; i++) 01261 { 01262 spiTxBursts[3][i] = L6474_NOP; 01263 } 01264 spiTxBursts[3][spiIndex] = param; 01265 01266 /* Disable interruption before checking */ 01267 /* pre-emption by ISR and SPI transfers*/ 01268 BSP_MotorControlBoard_DisableIrq(); 01269 itDisable = TRUE; 01270 } while (spiPreemtionByIsr); // check pre-emption by ISR 01271 01272 L6474_WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); 01273 01274 /* re-enable BSP_MotorControlBoard_EnableIrq after SPI transfers*/ 01275 BSP_MotorControlBoard_EnableIrq(); 01276 } 01277 01278 /******************************************************//** 01279 * @brief Sets the registers of the L6474 to their predefined values 01280 * from l6474_target_config.h 01281 * @param[in] deviceId (from 0 to 2) 01282 * @retval None 01283 **********************************************************/ 01284 void L6474_SetRegisterToPredefinedValues(uint8_t deviceId) 01285 { 01286 L6474_CmdSetParam(deviceId, 01287 L6474_ABS_POS, 01288 0); 01289 L6474_CmdSetParam(deviceId, 01290 L6474_EL_POS, 01291 0); 01292 L6474_CmdSetParam(deviceId, 01293 L6474_MARK, 01294 0); 01295 switch (deviceId) 01296 { 01297 case 0: 01298 L6474_CmdSetParam(deviceId, 01299 L6474_TVAL, 01300 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); 01301 L6474_CmdSetParam(deviceId, 01302 L6474_T_FAST, 01303 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 | 01304 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0); 01305 L6474_CmdSetParam(deviceId, 01306 L6474_TON_MIN, 01307 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0) 01308 ); 01309 L6474_CmdSetParam(deviceId, 01310 L6474_TOFF_MIN, 01311 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0)); 01312 L6474_CmdSetParam(deviceId, 01313 L6474_OCD_TH, 01314 L6474_CONF_PARAM_OCD_TH_DEVICE_0); 01315 L6474_CmdSetParam(deviceId, 01316 L6474_STEP_MODE, 01317 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 | 01318 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0); 01319 L6474_CmdSetParam(deviceId, 01320 L6474_ALARM_EN, 01321 L6474_CONF_PARAM_ALARM_EN_DEVICE_0); 01322 L6474_CmdSetParam(deviceId, 01323 L6474_CONFIG, 01324 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | 01325 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 | 01326 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 | 01327 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 | 01328 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0); 01329 break; 01330 case 1: 01331 L6474_CmdSetParam(deviceId, 01332 L6474_TVAL, 01333 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); 01334 L6474_CmdSetParam(deviceId, 01335 L6474_T_FAST, 01336 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 | 01337 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1); 01338 L6474_CmdSetParam(deviceId, 01339 L6474_TON_MIN, 01340 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1)); 01341 L6474_CmdSetParam(deviceId, 01342 L6474_TOFF_MIN, 01343 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1)); 01344 L6474_CmdSetParam(deviceId, 01345 L6474_OCD_TH, 01346 L6474_CONF_PARAM_OCD_TH_DEVICE_1); 01347 L6474_CmdSetParam(deviceId, 01348 L6474_STEP_MODE, 01349 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 | 01350 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1); 01351 L6474_CmdSetParam(deviceId, 01352 L6474_ALARM_EN, 01353 L6474_CONF_PARAM_ALARM_EN_DEVICE_1); 01354 L6474_CmdSetParam(deviceId, 01355 L6474_CONFIG, 01356 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | 01357 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 | 01358 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 | 01359 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 | 01360 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1); 01361 break; 01362 case 2: 01363 L6474_CmdSetParam(deviceId, 01364 L6474_TVAL, 01365 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); 01366 L6474_CmdSetParam(deviceId, 01367 L6474_T_FAST, 01368 (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 | 01369 (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2); 01370 L6474_CmdSetParam(deviceId, 01371 L6474_TON_MIN, 01372 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2)); 01373 L6474_CmdSetParam(deviceId, 01374 L6474_TOFF_MIN, 01375 L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2)); 01376 L6474_CmdSetParam(deviceId, 01377 L6474_OCD_TH, 01378 L6474_CONF_PARAM_OCD_TH_DEVICE_2); 01379 L6474_CmdSetParam(deviceId, 01380 L6474_STEP_MODE, 01381 (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 | 01382 (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2); 01383 L6474_CmdSetParam(deviceId, 01384 L6474_ALARM_EN, 01385 L6474_CONF_PARAM_ALARM_EN_DEVICE_2); 01386 L6474_CmdSetParam(deviceId, 01387 L6474_CONFIG, 01388 (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | 01389 (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 | 01390 (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 | 01391 (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 | 01392 (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2); 01393 break; 01394 default: ; 01395 } 01396 } 01397 01398 /******************************************************//** 01399 * @brief Sets the parameters of the device to predefined values 01400 * from l6474_target_config.h 01401 * @param None 01402 * @retval None 01403 **********************************************************/ 01404 void L6474_SetDeviceParamsToPredefinedValues(void) 01405 { 01406 uint32_t i; 01407 01408 devicePrm[0].acceleration = L6474_CONF_PARAM_ACC_DEVICE_0; 01409 devicePrm[0].deceleration = L6474_CONF_PARAM_DEC_DEVICE_0; 01410 devicePrm[0].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0; 01411 devicePrm[0].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0; 01412 01413 devicePrm[1].acceleration = L6474_CONF_PARAM_ACC_DEVICE_1; 01414 devicePrm[1].deceleration = L6474_CONF_PARAM_DEC_DEVICE_1; 01415 devicePrm[1].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1; 01416 devicePrm[1].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1; 01417 01418 devicePrm[2].acceleration = L6474_CONF_PARAM_ACC_DEVICE_2; 01419 devicePrm[2].deceleration = L6474_CONF_PARAM_DEC_DEVICE_2; 01420 devicePrm[2].maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2; 01421 devicePrm[2].minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2; 01422 01423 for (i = 0; i < MAX_NUMBER_OF_DEVICES; i++) 01424 { 01425 devicePrm[i].accu = 0; 01426 devicePrm[i].currentPosition = 0; 01427 devicePrm[i].endAccPos = 0; 01428 devicePrm[i].relativePos = 0; 01429 devicePrm[i].startDecPos = 0; 01430 devicePrm[i].stepsToTake = 0; 01431 devicePrm[i].speed = 0; 01432 devicePrm[i].commandExecuted = NO_CMD; 01433 devicePrm[i].direction = FORWARD; 01434 devicePrm[i].motionState = INACTIVE; 01435 } 01436 01437 for (i = 0; i < numberOfDevices; i++) 01438 { 01439 L6474_SetRegisterToPredefinedValues(i); 01440 } 01441 } 01442 01443 /******************************************************//** 01444 * @brief Initialises the bridge parameters to start the movement 01445 * and enable the power bridge 01446 * @param[in] deviceId (from 0 to 2) 01447 * @retval None 01448 **********************************************************/ 01449 void L6474_StartMovement(uint8_t deviceId) 01450 { 01451 #ifdef L6474_CONF_BRAKE_WHEN_HALTED 01452 /* Restore powerstage to full current capability */ 01453 switch (deviceId) 01454 { 01455 case 0: 01456 L6474_CmdSetParam(deviceId, 01457 L6474_TVAL, 01458 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0)); 01459 break; 01460 case 1: 01461 L6474_CmdSetParam(deviceId, 01462 L6474_TVAL, 01463 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1)); 01464 break; 01465 case 2: 01466 L6474_CmdSetParam(deviceId, 01467 L6474_TVAL, 01468 L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2)); 01469 break; 01470 default: ; 01471 } 01472 #endif 01473 01474 /* Enable L6474 powerstage */ 01475 L6474_CmdEnable(deviceId); 01476 01477 if (devicePrm[deviceId].endAccPos != 0) 01478 { 01479 devicePrm[deviceId].motionState = ACCELERATING; 01480 } 01481 else 01482 { 01483 devicePrm[deviceId].motionState = DECELERATING; 01484 } 01485 devicePrm[deviceId].accu = 0; 01486 devicePrm[deviceId].relativePos = 0; 01487 L6474_ApplySpeed(deviceId, devicePrm[deviceId].minSpeed); 01488 } 01489 01490 /******************************************************//** 01491 * @brief Handles the device state machine at each ste 01492 * @param[in] deviceId (from 0 to 2) 01493 * @retval None 01494 * @note Must only be called by the timer ISR 01495 **********************************************************/ 01496 void L6474_StepClockHandler(uint8_t deviceId) 01497 { 01498 /* Set isr flag */ 01499 isrFlag = TRUE; 01500 01501 /* Incrementation of the relative position */ 01502 devicePrm[deviceId].relativePos++; 01503 01504 switch (devicePrm[deviceId].motionState) 01505 { 01506 case ACCELERATING: 01507 { 01508 uint32_t relPos = devicePrm[deviceId].relativePos; 01509 uint32_t endAccPos = devicePrm[deviceId].endAccPos; 01510 uint16_t speed = devicePrm[deviceId].speed; 01511 uint32_t acc = ((uint32_t)devicePrm[deviceId].acceleration << 16); 01512 01513 if ((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)|| 01514 ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& 01515 (relPos == devicePrm[deviceId].startDecPos))) 01516 { 01517 devicePrm[deviceId].motionState = DECELERATING; 01518 devicePrm[deviceId].accu = 0; 01519 } 01520 else if ((speed >= devicePrm[deviceId].maxSpeed)|| 01521 ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& 01522 (relPos == endAccPos))) 01523 { 01524 devicePrm[deviceId].motionState = STEADY; 01525 } 01526 else 01527 { 01528 bool speedUpdated = FALSE; 01529 /* Go on accelerating */ 01530 if (speed == 0) speed =1; 01531 devicePrm[deviceId].accu += acc / speed; 01532 while (devicePrm[deviceId].accu >= (0X10000L)) 01533 { 01534 devicePrm[deviceId].accu -= (0X10000L); 01535 speed +=1; 01536 speedUpdated = TRUE; 01537 } 01538 01539 if (speedUpdated) 01540 { 01541 if (speed > devicePrm[deviceId].maxSpeed) 01542 { 01543 speed = devicePrm[deviceId].maxSpeed; 01544 } 01545 devicePrm[deviceId].speed = speed; 01546 L6474_ApplySpeed(deviceId, devicePrm[deviceId].speed); 01547 } 01548 } 01549 break; 01550 } 01551 case STEADY: 01552 { 01553 uint16_t maxSpeed = devicePrm[deviceId].maxSpeed; 01554 uint32_t relativePos = devicePrm[deviceId].relativePos; 01555 if ((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)|| 01556 ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& 01557 (relativePos >= (devicePrm[deviceId].startDecPos))) || 01558 ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& 01559 (devicePrm[deviceId].speed > maxSpeed))) 01560 { 01561 devicePrm[deviceId].motionState = DECELERATING; 01562 devicePrm[deviceId].accu = 0; 01563 } 01564 else if ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& 01565 (devicePrm[deviceId].speed < maxSpeed)) 01566 { 01567 devicePrm[deviceId].motionState = ACCELERATING; 01568 devicePrm[deviceId].accu = 0; 01569 } 01570 break; 01571 } 01572 case DECELERATING: 01573 { 01574 uint32_t relativePos = devicePrm[deviceId].relativePos; 01575 uint16_t speed = devicePrm[deviceId].speed; 01576 uint32_t deceleration = ((uint32_t)devicePrm[deviceId].deceleration << 16); 01577 if (((devicePrm[deviceId].commandExecuted == SOFT_STOP_CMD)&&(speed <= devicePrm[deviceId].minSpeed))|| 01578 ((devicePrm[deviceId].commandExecuted != RUN_CMD)&& 01579 (relativePos >= devicePrm[deviceId].stepsToTake))) 01580 { 01581 /* Motion process complete */ 01582 L6474_HardStop(deviceId); 01583 } 01584 else if ((devicePrm[deviceId].commandExecuted == RUN_CMD)&& 01585 (speed <= devicePrm[deviceId].maxSpeed)) 01586 { 01587 devicePrm[deviceId].motionState = STEADY; 01588 } 01589 else 01590 { 01591 /* Go on decelerating */ 01592 if (speed > devicePrm[deviceId].minSpeed) 01593 { 01594 bool speedUpdated = FALSE; 01595 if (speed == 0) speed =1; 01596 devicePrm[deviceId].accu += deceleration / speed; 01597 while (devicePrm[deviceId].accu >= (0X10000L)) 01598 { 01599 devicePrm[deviceId].accu -= (0X10000L); 01600 if (speed > 1) 01601 { 01602 speed -=1; 01603 } 01604 speedUpdated = TRUE; 01605 } 01606 01607 if (speedUpdated) 01608 { 01609 if (speed < devicePrm[deviceId].minSpeed) 01610 { 01611 speed = devicePrm[deviceId].minSpeed; 01612 } 01613 devicePrm[deviceId].speed = speed; 01614 L6474_ApplySpeed(deviceId, devicePrm[deviceId].speed); 01615 } 01616 } 01617 } 01618 break; 01619 } 01620 default: 01621 { 01622 break; 01623 } 01624 } 01625 /* Set isr flag */ 01626 isrFlag = FALSE; 01627 } 01628 01629 /******************************************************//** 01630 * @brief Converts mA in compatible values for TVAL register 01631 * @param[in] Tval 01632 * @retval TVAL values 01633 **********************************************************/ 01634 inline uint8_t L6474_Tval_Current_to_Par(double Tval) 01635 { 01636 return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); 01637 } 01638 01639 /******************************************************//** 01640 * @brief Convert time in us in compatible values 01641 * for TON_MIN register 01642 * @param[in] Tmin 01643 * @retval TON_MIN values 01644 **********************************************************/ 01645 inline uint8_t L6474_Tmin_Time_to_Par(double Tmin) 01646 { 01647 return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); 01648 } 01649 01650 /******************************************************//** 01651 * @brief Write and receive a byte via SPI 01652 * @param[in] pByteToTransmit pointer to the byte to transmit 01653 * @param[in] pReceivedByte pointer to the received byte 01654 * @retval None 01655 **********************************************************/ 01656 void L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) 01657 { 01658 if (BSP_MotorControlBoard_SpiWriteBytes(pByteToTransmit, pReceivedByte, numberOfDevices) != 0) 01659 { 01660 L6474_ErrorHandler(L6474_ERROR_1); 01661 } 01662 01663 if (isrFlag) 01664 { 01665 spiPreemtionByIsr = TRUE; 01666 } 01667 } 01668 01669 /** 01670 * @} 01671 */ 01672 01673 /** 01674 * @} 01675 */ 01676 01677 /** 01678 * @} 01679 */ 01680 01681 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Tue Jul 12 2022 22:53:31 by
1.7.2