Robotique FIP
/
Easyspin_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Easyspin_lib
by 
Julien Tiron
Embed:
(wiki syntax)
Show/hide line numbers
easyspin.cpp
Go to the documentation of this file.
00001 /******************************************************//** 00002 * @file easyspin.cpp 00003 * @version V1.0 00004 * @date June 29, 2015 00005 * @brief easyspin library for mbed 00006 * 00007 * This file is free software; you can redistribute it and/or modify 00008 * it under the terms of either the GNU General Public License version 2 00009 * or the GNU Lesser General Public License version 2.1, both as 00010 * published by the Free Software Foundation. 00011 **********************************************************/ 00012 00013 #include "easyspin.h" 00014 #include "mbed.h" 00015 00016 const uint16_t Easyspin::prescalerArrayTimer0_1[PRESCALER_ARRAY_TIMER0_1_SIZE] = { 0, 1, 8, 64, 256, 1024}; 00017 const uint16_t Easyspin::prescalerArrayTimer2[PRESCALER_ARRAY_TIMER2_SIZE] = {0, 1, 8, 32, 64, 128, 256, 1024}; 00018 volatile void (*Easyspin::flagInterruptCallback)(void); 00019 volatile uint8_t Easyspin::numberOfShields; 00020 uint8_t Easyspin::spiTxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; 00021 uint8_t Easyspin::spiRxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; 00022 volatile bool Easyspin::spiPreemtionByIsr = false; 00023 volatile bool Easyspin::isrFlag = false; 00024 volatile class Easyspin* Easyspin::instancePtr = NULL; 00025 00026 /******************************************************//** 00027 * @brief Constructor 00028 * @param None 00029 * @retval None 00030 **********************************************************/ 00031 Easyspin::Easyspin():reset(Easyspin_Reset_Pin), dir1(Easyspin_DIR_1_Pin), dir2(Easyspin_DIR_2_Pin), 00032 dir3(Easyspin_DIR_3_Pin), CS(Easyspin_CS_Pin), flag(Easyspin_FLAG_Pin), pwm1(Easyspin_PWM_1_Pin), 00033 pwm2(Easyspin_PWM_2_Pin), pwm3(Easyspin_PWM_3_Pin), spi(Easyspin_MOSI_Pin, Easyspin_MISO_Pin, Easyspin_SCK_Pin) 00034 { 00035 uint8_t i; 00036 for (i = 0; i < MAX_NUMBER_OF_SHIELDS; i++) { 00037 shieldPrm[i].motionState = INACTIVE; 00038 shieldPrm[i].commandExecuted = NO_CMD; 00039 shieldPrm[i].stepsToTake = MAX_STEPS; 00040 } 00041 instancePtr = this; 00042 } 00043 00044 /******************************************************//** 00045 * @brief Attaches a user callback to the flag Interrupt 00046 * The call back will be then called each time the status 00047 * flag pin will be pulled down due to the occurrence of 00048 * a programmed alarms ( OCD, thermal pre-warning or 00049 * shutdown, UVLO, wrong command, non-performable command) 00050 * @param[in] callback Name of the callback to attach 00051 * to the Flag Interrupt 00052 * @retval None 00053 **********************************************************/ 00054 void Easyspin::AttachFlagInterrupt(void (*callback)(void)) 00055 { 00056 flagInterruptCallback = (volatile void (*)())callback; 00057 } 00058 00059 /******************************************************//** 00060 * @brief Starts the Easyspin library 00061 * @param[in] nbShields Number of Easyspin shields to use (from 1 to 3) 00062 * @retval None 00063 **********************************************************/ 00064 void Easyspin::Begin(uint8_t nbShields) 00065 { 00066 numberOfShields = nbShields; 00067 00068 // start the SPI library: 00069 //SPI.begin(); 00070 //SPI.setBitOrder(MSBFIRST); 00071 spi.format(8, 3); 00072 //SPI.setClockDivider(SPI_CLOCK_DIV4); 00073 00074 // flag pin 00075 //pinMode(Easyspin_FLAG_Pin, INPUT_PULLUP); 00076 flag.mode(PullUp); 00077 flag.fall(&FlagInterruptHandler); 00078 00079 //reset pin 00080 //pinMode(Easyspin_Reset_Pin, OUTPUT); 00081 00082 switch (nbShields) { 00083 case 3: 00084 //pinMode(Easyspin_DIR_3_Pin, OUTPUT); 00085 //pinMode(Easyspin_PWM_3_Pin, OUTPUT); 00086 PwmInit(2); 00087 case 2: 00088 //pinMode(Easyspin_DIR_2_Pin, OUTPUT); 00089 //pinMode(Easyspin_PWM_2_Pin, OUTPUT); 00090 PwmInit(1); 00091 case 1: 00092 //pinMode(Easyspin_DIR_1_Pin, OUTPUT); 00093 //pinMode(Easyspin_PWM_1_Pin, OUTPUT); 00094 PwmInit(0); 00095 default: 00096 ; 00097 } 00098 00099 /* Standby-reset deactivation */ 00100 ReleaseReset(); 00101 00102 /* Set all registers and context variables to the predefined values from Easyspin_target_config.h */ 00103 SetShieldParamsToPredefinedValues(); 00104 00105 /* Disable Easyspin powerstage */ 00106 for (uint32_t i = 0; i < nbShields; i++) { 00107 CmdDisable(i); 00108 /* Get Status to clear flags after start up */ 00109 CmdGetStatus(i); 00110 } 00111 } 00112 00113 /******************************************************//** 00114 * @brief Returns the acceleration of the specified shield 00115 * @param[in] shieldId (from 0 to 2) 00116 * @retval Acceleration in pps^2 00117 **********************************************************/ 00118 uint16_t Easyspin::GetAcceleration(uint8_t shieldId) 00119 { 00120 return (shieldPrm[shieldId].acceleration); 00121 } 00122 00123 /******************************************************//** 00124 * @brief Returns the current speed of the specified shield 00125 * @param[in] shieldId (from 0 to 2) 00126 * @retval Speed in pps 00127 **********************************************************/ 00128 uint16_t Easyspin::GetCurrentSpeed(uint8_t shieldId) 00129 { 00130 return shieldPrm[shieldId].speed; 00131 } 00132 00133 /******************************************************//** 00134 * @brief Returns the deceleration of the specified shield 00135 * @param[in] shieldId (from 0 to 2) 00136 * @retval Deceleration in pps^2 00137 **********************************************************/ 00138 uint16_t Easyspin::GetDeceleration(uint8_t shieldId) 00139 { 00140 return (shieldPrm[shieldId].deceleration); 00141 } 00142 00143 /******************************************************//** 00144 * @brief Returns the FW version of the library 00145 * @param None 00146 * @retval Easyspin_FW_VERSION 00147 **********************************************************/ 00148 uint8_t Easyspin::GetFwVersion(void) 00149 { 00150 return (Easyspin_FW_VERSION); 00151 } 00152 00153 /******************************************************//** 00154 * @brief Returns the mark position of the specified shield 00155 * @param[in] shieldId (from 0 to 2) 00156 * @retval Mark register value converted in a 32b signed integer 00157 **********************************************************/ 00158 int32_t Easyspin::GetMark(uint8_t shieldId) 00159 { 00160 return ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); 00161 } 00162 00163 /******************************************************//** 00164 * @brief Returns the max speed of the specified shield 00165 * @param[in] shieldId (from 0 to 2) 00166 * @retval maxSpeed in pps 00167 **********************************************************/ 00168 uint16_t Easyspin::GetMaxSpeed(uint8_t shieldId) 00169 { 00170 return (shieldPrm[shieldId].maxSpeed); 00171 } 00172 00173 /******************************************************//** 00174 * @brief Returns the min speed of the specified shield 00175 * @param[in] shieldId (from 0 to 2) 00176 * @retval minSpeed in pps 00177 **********************************************************/ 00178 uint16_t Easyspin::GetMinSpeed(uint8_t shieldId) 00179 { 00180 return (shieldPrm[shieldId].minSpeed); 00181 } 00182 00183 /******************************************************//** 00184 * @brief Returns the ABS_POSITION of the specified shield 00185 * @param[in] shieldId (from 0 to 2) 00186 * @retval ABS_POSITION register value converted in a 32b signed integer 00187 **********************************************************/ 00188 int32_t Easyspin::GetPosition(uint8_t shieldId) 00189 { 00190 return ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); 00191 } 00192 00193 /******************************************************//** 00194 * @brief Returns the shield state 00195 * @param[in] shieldId (from 0 to 2) 00196 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) 00197 **********************************************************/ 00198 shieldState_t Easyspin::GetShieldState(uint8_t shieldId) 00199 { 00200 return shieldPrm[shieldId].motionState; 00201 } 00202 00203 /******************************************************//** 00204 * @brief Requests the motor to move to the home position (ABS_POSITION = 0) 00205 * @param[in] shieldId (from 0 to 2) 00206 * @retval None 00207 **********************************************************/ 00208 void Easyspin::GoHome(uint8_t shieldId) 00209 { 00210 GoTo(shieldId, 0); 00211 } 00212 00213 /******************************************************//** 00214 * @brief Requests the motor to move to the mark position 00215 * @param[in] shieldId (from 0 to 2) 00216 * @retval None 00217 **********************************************************/ 00218 void Easyspin::GoMark(uint8_t shieldId) 00219 { 00220 uint32_t mark; 00221 00222 mark = ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); 00223 GoTo(shieldId,mark); 00224 } 00225 00226 /******************************************************//** 00227 * @brief Requests the motor to move to the specified position 00228 * @param[in] shieldId (from 0 to 2) 00229 * @param[in] targetPosition absolute position in steps 00230 * @retval None 00231 **********************************************************/ 00232 void Easyspin::GoTo(uint8_t shieldId, int32_t targetPosition) 00233 { 00234 dir_t direction; 00235 int32_t steps; 00236 00237 /* Eventually deactivate motor */ 00238 if (shieldPrm[shieldId].motionState != INACTIVE) { 00239 HardStop(shieldId); 00240 } 00241 00242 /* Get current position */ 00243 shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); 00244 00245 /* Compute the number of steps to perform */ 00246 steps = targetPosition - shieldPrm[shieldId].currentPosition; 00247 00248 if (steps >= 0) { 00249 shieldPrm[shieldId].stepsToTake = steps; 00250 direction = FORWARD; 00251 00252 } else { 00253 shieldPrm[shieldId].stepsToTake = -steps; 00254 direction = BACKWARD; 00255 } 00256 00257 if (steps != 0) { 00258 00259 shieldPrm[shieldId].commandExecuted = MOVE_CMD; 00260 00261 /* Direction setup */ 00262 SetDirection(shieldId,direction); 00263 00264 ComputeSpeedProfile(shieldId, shieldPrm[shieldId].stepsToTake); 00265 00266 /* Motor activation */ 00267 StartMovement(shieldId); 00268 } 00269 } 00270 00271 /******************************************************//** 00272 * @brief Immediatly stops the motor and disable the power bridge 00273 * @param[in] shieldId (from 0 to 2) 00274 * @retval None 00275 **********************************************************/ 00276 void Easyspin::HardStop(uint8_t shieldId) 00277 { 00278 /* Disable corresponding PWM */ 00279 PwmStop(shieldId); 00280 00281 /* Disable power stage */ 00282 CmdDisable(shieldId); 00283 00284 /* Set inactive state */ 00285 shieldPrm[shieldId].motionState = INACTIVE; 00286 shieldPrm[shieldId].commandExecuted = NO_CMD; 00287 shieldPrm[shieldId].stepsToTake = MAX_STEPS; 00288 00289 #ifdef _DEBUG_Easyspin 00290 Serial.println("Inactive\n"); 00291 #endif 00292 } 00293 00294 /******************************************************//** 00295 * @brief Moves the motor of the specified number of steps 00296 * @param[in] shieldId (from 0 to 2) 00297 * @param[in] direction FORWARD or BACKWARD 00298 * @param[in] stepCount Number of steps to perform 00299 * @retval None 00300 **********************************************************/ 00301 void Easyspin::Move(uint8_t shieldId, dir_t direction, uint32_t stepCount) 00302 { 00303 /* Eventually deactivate motor */ 00304 if (shieldPrm[shieldId].motionState != INACTIVE) { 00305 HardStop(shieldId); 00306 } 00307 00308 if (stepCount != 0) { 00309 shieldPrm[shieldId].stepsToTake = stepCount; 00310 00311 shieldPrm[shieldId].commandExecuted = MOVE_CMD; 00312 00313 shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); 00314 00315 /* Direction setup */ 00316 SetDirection(shieldId,direction); 00317 00318 ComputeSpeedProfile(shieldId, stepCount); 00319 00320 /* Motor activation */ 00321 StartMovement(shieldId); 00322 } 00323 } 00324 00325 00326 /******************************************************//** 00327 * @brief Turn left or right with the specified angle 00328 * @param[in] rotation RIGHT or LEFT 00329 * @param[in] angle in degrees 00330 * @retval None 00331 **********************************************************/ 00332 void Easyspin::Turn(rot_t rotation, uint16_t angle) 00333 { 00334 uint16_t nbStep = (10934*angle)/360; 00335 if(rotation==RIGHT) { 00336 Move(0, FORWARD, nbStep); 00337 Move(1, FORWARD, nbStep); 00338 } else { 00339 Move(0, BACKWARD, nbStep); 00340 Move(1, BACKWARD, nbStep); 00341 } 00342 } 00343 00344 /******************************************************//** 00345 * @brief Move forward or backward with the specified distance 00346 * @param[in] direction FORWARD or BACKWARD 00347 * @param[in] distance in cm 00348 * @retval None 00349 **********************************************************/ 00350 void Easyspin::Move_cm(dir_t direction, uint16_t distance) 00351 { 00352 uint16_t nbStep = (10000 * distance)/61; 00353 if(direction==FORWARD) { 00354 Move(0, FORWARD, nbStep); 00355 Move(1, BACKWARD, nbStep); 00356 } else { 00357 Move(0, BACKWARD, nbStep); 00358 Move(1, FORWARD, nbStep); 00359 } 00360 } 00361 00362 /******************************************************//** 00363 * @brief Resets all Easyspin shields 00364 * @param None 00365 * @retval None 00366 **********************************************************/ 00367 void Easyspin::ResetAllShields(void) 00368 { 00369 uint8_t loop; 00370 00371 for (loop = 0; loop < numberOfShields; loop++) { 00372 /* Stop movement and disable power stage*/ 00373 HardStop(loop); 00374 } 00375 Reset(); 00376 wait_us(20); // Reset pin must be forced low for at least 10us 00377 ReleaseReset(); 00378 } 00379 00380 /******************************************************//** 00381 * @brief Runs the motor. It will accelerate from the min 00382 * speed up to the max speed by using the shield acceleration. 00383 * @param[in] shieldId (from 0 to 2) 00384 * @param[in] direction FORWARD or BACKWARD 00385 * @retval None 00386 **********************************************************/ 00387 void Easyspin::Run(uint8_t shieldId, dir_t direction) 00388 { 00389 /* Eventually deactivate motor */ 00390 if (shieldPrm[shieldId].motionState != INACTIVE) { 00391 HardStop(shieldId); 00392 } 00393 00394 /* Direction setup */ 00395 SetDirection(shieldId,direction); 00396 00397 shieldPrm[shieldId].commandExecuted = RUN_CMD; 00398 00399 /* Motor activation */ 00400 StartMovement(shieldId); 00401 } 00402 00403 /******************************************************//** 00404 * @brief Changes the acceleration of the specified shield 00405 * @param[in] shieldId (from 0 to 2) 00406 * @param[in] newAcc New acceleration to apply in pps^2 00407 * @retval true if the command is successfully executed, else false 00408 * @note The command is not performed is the shield is executing 00409 * a MOVE or GOTO command (but it can be used during a RUN command) 00410 **********************************************************/ 00411 bool Easyspin::SetAcceleration(uint8_t shieldId,uint16_t newAcc) 00412 { 00413 bool cmdExecuted = false; 00414 if ((newAcc != 0)&& 00415 ((shieldPrm[shieldId].motionState == INACTIVE)|| 00416 (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { 00417 shieldPrm[shieldId].acceleration = newAcc; 00418 cmdExecuted = true; 00419 } 00420 return cmdExecuted; 00421 } 00422 00423 /******************************************************//** 00424 * @brief Changes the deceleration of the specified shield 00425 * @param[in] shieldId (from 0 to 2) 00426 * @param[in] newDec New deceleration to apply in pps^2 00427 * @retval true if the command is successfully executed, else false 00428 * @note The command is not performed is the shield is executing 00429 * a MOVE or GOTO command (but it can be used during a RUN command) 00430 **********************************************************/ 00431 bool Easyspin::SetDeceleration(uint8_t shieldId, uint16_t newDec) 00432 { 00433 bool cmdExecuted = false; 00434 if ((newDec != 0)&& 00435 ((shieldPrm[shieldId].motionState == INACTIVE)|| 00436 (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { 00437 shieldPrm[shieldId].deceleration = newDec; 00438 cmdExecuted = true; 00439 } 00440 return cmdExecuted; 00441 } 00442 00443 /******************************************************//** 00444 * @brief Set current position to be the Home position (ABS pos set to 0) 00445 * @param[in] shieldId (from 0 to 2) 00446 * @retval None 00447 **********************************************************/ 00448 void Easyspin::SetHome(uint8_t shieldId) 00449 { 00450 CmdSetParam(shieldId, Easyspin_ABS_POS, 0); 00451 } 00452 00453 /******************************************************//** 00454 * @brief Sets current position to be the Mark position 00455 * @param[in] shieldId (from 0 to 2) 00456 * @retval None 00457 **********************************************************/ 00458 void Easyspin::SetMark(uint8_t shieldId) 00459 { 00460 uint32_t mark = CmdGetParam(shieldId,Easyspin_ABS_POS); 00461 CmdSetParam(shieldId,Easyspin_MARK, mark); 00462 } 00463 00464 /******************************************************//** 00465 * @brief Changes the max speed of the specified shield 00466 * @param[in] shieldId (from 0 to 2) 00467 * @param[in] newMaxSpeed New max speed to apply in pps 00468 * @retval true if the command is successfully executed, else false 00469 * @note The command is not performed is the shield is executing 00470 * a MOVE or GOTO command (but it can be used during a RUN command). 00471 **********************************************************/ 00472 bool Easyspin::SetMaxSpeed(uint8_t shieldId, uint16_t newMaxSpeed) 00473 { 00474 bool cmdExecuted = false; 00475 if ((newMaxSpeed > Easyspin_MIN_PWM_FREQ)&& 00476 (newMaxSpeed <= Easyspin_MAX_PWM_FREQ) && 00477 (shieldPrm[shieldId].minSpeed <= newMaxSpeed) && 00478 ((shieldPrm[shieldId].motionState == INACTIVE)|| 00479 (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { 00480 shieldPrm[shieldId].maxSpeed = newMaxSpeed; 00481 cmdExecuted = true; 00482 } 00483 return cmdExecuted; 00484 } 00485 00486 /******************************************************//** 00487 * @brief Changes the min speed of the specified shield 00488 * @param[in] shieldId (from 0 to 2) 00489 * @param[in] newMinSpeed New min speed to apply in pps 00490 * @retval true if the command is successfully executed, else false 00491 * @note The command is not performed is the shield is executing 00492 * a MOVE or GOTO command (but it can be used during a RUN command). 00493 **********************************************************/ 00494 bool Easyspin::SetMinSpeed(uint8_t shieldId, uint16_t newMinSpeed) 00495 { 00496 bool cmdExecuted = false; 00497 if ((newMinSpeed >= Easyspin_MIN_PWM_FREQ)&& 00498 (newMinSpeed < Easyspin_MAX_PWM_FREQ) && 00499 (newMinSpeed <= shieldPrm[shieldId].maxSpeed) && 00500 ((shieldPrm[shieldId].motionState == INACTIVE)|| 00501 (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { 00502 shieldPrm[shieldId].minSpeed = newMinSpeed; 00503 cmdExecuted = true; 00504 } 00505 return cmdExecuted; 00506 } 00507 00508 /******************************************************//** 00509 * @brief Stops the motor by using the shield deceleration 00510 * @param[in] shieldId (from 0 to 2) 00511 * @retval true if the command is successfully executed, else false 00512 * @note The command is not performed is the shield is in INACTIVE state. 00513 **********************************************************/ 00514 bool Easyspin::SoftStop(uint8_t shieldId) 00515 { 00516 bool cmdExecuted = false; 00517 if (shieldPrm[shieldId].motionState != INACTIVE) { 00518 shieldPrm[shieldId].commandExecuted = SOFT_STOP_CMD; 00519 cmdExecuted = true; 00520 } 00521 return (cmdExecuted); 00522 } 00523 00524 /******************************************************//** 00525 * @brief Locks until the shield state becomes Inactive 00526 * @param[in] shieldId (from 0 to 2) 00527 * @retval None 00528 **********************************************************/ 00529 void Easyspin::WaitWhileActive(uint8_t shieldId) 00530 { 00531 /* Wait while motor is running */ 00532 while (GetShieldState(shieldId) != INACTIVE); 00533 } 00534 00535 /******************************************************//** 00536 * @brief Issue the Disable command to the Easyspin of the specified shield 00537 * @param[in] shieldId (from 0 to 2) 00538 * @retval None 00539 **********************************************************/ 00540 void Easyspin::CmdDisable(uint8_t shieldId) 00541 { 00542 SendCommand(shieldId, Easyspin_DISABLE); 00543 } 00544 00545 /******************************************************//** 00546 * @brief Issues the Enable command to the Easyspin of the specified shield 00547 * @param[in] shieldId (from 0 to 2) 00548 * @retval None 00549 **********************************************************/ 00550 void Easyspin::CmdEnable(uint8_t shieldId) 00551 { 00552 SendCommand(shieldId, Easyspin_ENABLE); 00553 } 00554 00555 /******************************************************//** 00556 * @brief Issues the GetParam command to the Easyspin of the specified shield 00557 * @param[in] shieldId (from 0 to 2) 00558 * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) 00559 * @retval Register value 00560 **********************************************************/ 00561 uint32_t Easyspin::CmdGetParam(uint8_t shieldId, Easyspin_Registers_t param) 00562 { 00563 uint32_t i; 00564 uint32_t spiRxData; 00565 uint8_t maxArgumentNbBytes = 0; 00566 uint8_t spiIndex = numberOfShields - shieldId - 1; 00567 bool itDisable = false; 00568 00569 do { 00570 spiPreemtionByIsr = false; 00571 if (itDisable) { 00572 /* re-enable interrupts if disable in previous iteration */ 00573 flag.enable_irq(); 00574 itDisable = false; 00575 } 00576 00577 for (i = 0; i < numberOfShields; i++) { 00578 spiTxBursts[0][i] = Easyspin_NOP; 00579 spiTxBursts[1][i] = Easyspin_NOP; 00580 spiTxBursts[2][i] = Easyspin_NOP; 00581 spiTxBursts[3][i] = Easyspin_NOP; 00582 spiRxBursts[1][i] = 0; 00583 spiRxBursts[2][i] = 0; 00584 spiRxBursts[3][i] = 0; 00585 } 00586 switch (param) { 00587 case Easyspin_ABS_POS: 00588 ; 00589 case Easyspin_MARK: 00590 spiTxBursts[0][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); 00591 maxArgumentNbBytes = 3; 00592 break; 00593 case Easyspin_EL_POS: 00594 ; 00595 case Easyspin_CONFIG: 00596 ; 00597 case Easyspin_STATUS: 00598 spiTxBursts[1][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); 00599 maxArgumentNbBytes = 2; 00600 break; 00601 default: 00602 spiTxBursts[2][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); 00603 maxArgumentNbBytes = 1; 00604 } 00605 00606 /* Disable interruption before checking */ 00607 /* pre-emption by ISR and SPI transfers*/ 00608 flag.disable_irq(); 00609 itDisable = true; 00610 } while (spiPreemtionByIsr); // check pre-emption by ISR 00611 00612 for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00613 i < Easyspin_CMD_ARG_MAX_NB_BYTES; 00614 i++) { 00615 WriteBytes(&spiTxBursts[i][0], 00616 &spiRxBursts[i][0]); 00617 } 00618 00619 spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)| 00620 (spiRxBursts[2][spiIndex] << 8) | 00621 (spiRxBursts[3][spiIndex]); 00622 00623 /* re-enable interrupts after SPI transfers*/ 00624 flag.enable_irq(); 00625 00626 return (spiRxData); 00627 } 00628 00629 /******************************************************//** 00630 * @brief Issues the GetStatus command to the Easyspin of the specified shield 00631 * @param[in] shieldId (from 0 to 2) 00632 * @retval Status Register value 00633 * @note Once the GetStatus command is performed, the flags of the status register 00634 * are reset. This is not the case when the status register is read with the 00635 * GetParam command (via the functions ReadStatusRegister or CmdGetParam). 00636 **********************************************************/ 00637 uint16_t Easyspin::CmdGetStatus(uint8_t shieldId) 00638 { 00639 uint32_t i; 00640 uint16_t status; 00641 uint8_t spiIndex = numberOfShields - shieldId - 1; 00642 bool itDisable = false; 00643 00644 do { 00645 spiPreemtionByIsr = false; 00646 if (itDisable) { 00647 /* re-enable interrupts if disable in previous iteration */ 00648 flag.enable_irq(); 00649 itDisable = false; 00650 } 00651 00652 for (i = 0; i < numberOfShields; i++) { 00653 spiTxBursts[0][i] = Easyspin_NOP; 00654 spiTxBursts[1][i] = Easyspin_NOP; 00655 spiTxBursts[2][i] = Easyspin_NOP; 00656 spiRxBursts[1][i] = 0; 00657 spiRxBursts[2][i] = 0; 00658 } 00659 spiTxBursts[0][spiIndex] = Easyspin_GET_STATUS; 00660 00661 /* Disable interruption before checking */ 00662 /* pre-emption by ISR and SPI transfers*/ 00663 flag.disable_irq(); 00664 itDisable = true; 00665 } while (spiPreemtionByIsr); // check pre-emption by ISR 00666 00667 for (i = 0; i < Easyspin_CMD_ARG_NB_BYTES_GET_STATUS + Easyspin_RSP_NB_BYTES_GET_STATUS; i++) { 00668 WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); 00669 } 00670 status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]); 00671 00672 /* re-enable interrupts after SPI transfers*/ 00673 flag.enable_irq(); 00674 00675 return (status); 00676 } 00677 00678 /******************************************************//** 00679 * @brief Issues the Nop command to the Easyspin of the specified shield 00680 * @param[in] shieldId (from 0 to 2) 00681 * @retval None 00682 **********************************************************/ 00683 void Easyspin::CmdNop(uint8_t shieldId) 00684 { 00685 SendCommand(shieldId, Easyspin_NOP); 00686 } 00687 00688 /******************************************************//** 00689 * @brief Issues the SetParam command to the Easyspin of the specified shield 00690 * @param[in] shieldId (from 0 to 2) 00691 * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) 00692 * @param[in] value Value to set in the register 00693 * @retval None 00694 **********************************************************/ 00695 void Easyspin::CmdSetParam(uint8_t shieldId, 00696 Easyspin_Registers_t param, 00697 uint32_t value) 00698 { 00699 uint32_t i; 00700 uint8_t maxArgumentNbBytes = 0; 00701 uint8_t spiIndex = numberOfShields - shieldId - 1; 00702 bool itDisable = false; 00703 do { 00704 spiPreemtionByIsr = false; 00705 if (itDisable) { 00706 /* re-enable interrupts if disable in previous iteration */ 00707 flag.enable_irq();//interrupts(); 00708 itDisable = false; 00709 } 00710 for (i = 0; i < numberOfShields; i++) { 00711 spiTxBursts[0][i] = Easyspin_NOP; 00712 spiTxBursts[1][i] = Easyspin_NOP; 00713 spiTxBursts[2][i] = Easyspin_NOP; 00714 spiTxBursts[3][i] = Easyspin_NOP; 00715 } 00716 switch (param) { 00717 case Easyspin_ABS_POS: 00718 ; 00719 case Easyspin_MARK: 00720 spiTxBursts[0][spiIndex] = param; 00721 spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16); 00722 spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); 00723 maxArgumentNbBytes = 3; 00724 break; 00725 case Easyspin_EL_POS: 00726 ; 00727 case Easyspin_CONFIG: 00728 spiTxBursts[1][spiIndex] = param; 00729 spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); 00730 maxArgumentNbBytes = 2; 00731 break; 00732 default: 00733 spiTxBursts[2][spiIndex] = param; 00734 maxArgumentNbBytes = 1; 00735 } 00736 spiTxBursts[3][spiIndex] = (uint8_t)(value); 00737 00738 /* Disable interruption before checking */ 00739 /* pre-emption by ISR and SPI transfers*/ 00740 flag.disable_irq(); 00741 itDisable = true; 00742 } while (spiPreemtionByIsr); // check pre-emption by ISR 00743 00744 /* SPI transfer */ 00745 for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; 00746 i < Easyspin_CMD_ARG_MAX_NB_BYTES; 00747 i++) { 00748 WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]); 00749 } 00750 /* re-enable interrupts after SPI transfers*/ 00751 flag.enable_irq(); 00752 } 00753 00754 /******************************************************//** 00755 * @brief Reads the Status Register value 00756 * @param[in] shieldId (from 0 to 2) 00757 * @retval Status register valued 00758 * @note The status register flags are not cleared 00759 * at the difference with CmdGetStatus() 00760 **********************************************************/ 00761 uint16_t Easyspin::ReadStatusRegister(uint8_t shieldId) 00762 { 00763 return (CmdGetParam(shieldId,Easyspin_STATUS)); 00764 } 00765 00766 /******************************************************//** 00767 * @brief Releases the Easyspin reset (pin set to High) of all shields 00768 * @param None 00769 * @retval None 00770 **********************************************************/ 00771 void Easyspin::ReleaseReset(void) 00772 { 00773 reset = 1; 00774 } 00775 00776 /******************************************************//** 00777 * @brief Resets the Easyspin (reset pin set to low) of all shields 00778 * @param None 00779 * @retval None 00780 **********************************************************/ 00781 void Easyspin::Reset(void) 00782 { 00783 reset = 0; 00784 } 00785 00786 /******************************************************//** 00787 * @brief Set the stepping mode 00788 * @param[in] shieldId (from 0 to 2) 00789 * @param[in] stepMod from full step to 1/16 microstep as specified in enum Easyspin_STEP_SEL_t 00790 * @retval None 00791 **********************************************************/ 00792 void Easyspin::SelectStepMode(uint8_t shieldId, Easyspin_STEP_SEL_t stepMod) 00793 { 00794 uint8_t stepModeRegister; 00795 00796 /* Eventually deactivate motor */ 00797 if (shieldPrm[shieldId].motionState != INACTIVE) { 00798 HardStop(shieldId); 00799 } 00800 00801 /* Read Step mode register and clear STEP_SEL field */ 00802 stepModeRegister = (uint8_t)(0xF8 & CmdGetParam(shieldId,Easyspin_STEP_MODE)) ; 00803 00804 /* Apply new step mode */ 00805 CmdSetParam(shieldId, Easyspin_STEP_MODE, stepModeRegister | (uint8_t)stepMod); 00806 00807 /* Reset abs pos register */ 00808 SetHome(shieldId); 00809 } 00810 00811 /******************************************************//** 00812 * @brief Specifies the direction 00813 * @param[in] shieldId (from 0 to 2) 00814 * @param[in] dir FORWARD or BACKWARD 00815 * @note The direction change is only applied if the shield 00816 * is in INACTIVE state 00817 * @retval None 00818 **********************************************************/ 00819 void Easyspin::SetDirection(uint8_t shieldId, dir_t dir) 00820 { 00821 if (shieldPrm[shieldId].motionState == INACTIVE) { 00822 shieldPrm[shieldId].direction = dir; 00823 00824 switch (shieldId) { 00825 case 2: 00826 dir3 = dir; 00827 break; 00828 case 1: 00829 dir2 = dir; 00830 break; 00831 case 0: 00832 dir1 = dir; 00833 break; 00834 default: 00835 ; 00836 } 00837 } 00838 } 00839 00840 /******************************************************//** 00841 * @brief Gets the pointer to the Easyspin instance 00842 * @param None 00843 * @retval Pointer to the instance of Easyspin 00844 **********************************************************/ 00845 class Easyspin* Easyspin::GetInstancePtr(void) 00846 { 00847 return (class Easyspin*)instancePtr; 00848 } 00849 00850 /******************************************************//** 00851 * @brief Handles the shield state machine at each ste 00852 * @param[in] shieldId (from 0 to 2) 00853 * @retval None 00854 * @note Must only be called by the timer ISR 00855 **********************************************************/ 00856 void Easyspin::StepClockHandler(uint8_t shieldId) 00857 { 00858 /* Set isr flag */ 00859 isrFlag = true; 00860 00861 /* Incrementation of the relative position */ 00862 shieldPrm[shieldId].relativePos++; 00863 00864 /* Periodically check that estimated position is correct */ 00865 if ((shieldPrm[shieldId].commandExecuted != RUN_CMD) && 00866 ((shieldPrm[shieldId].relativePos % 10) == 00)) { 00867 uint32_t AbsPos= ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); 00868 00869 /* Correct estimated position if needed */ 00870 if (AbsPos != 0) { 00871 if ((shieldPrm[shieldId].direction == FORWARD) && 00872 (AbsPos != shieldPrm[shieldId].currentPosition + shieldPrm[shieldId].relativePos)) { 00873 #ifdef _DEBUG_Easyspin 00874 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "F EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); 00875 Serial.println(EasyspinStrOut); 00876 #endif 00877 shieldPrm[shieldId].relativePos = AbsPos - shieldPrm[shieldId].currentPosition; 00878 00879 } else if ((shieldPrm[shieldId].direction == BACKWARD) && 00880 (AbsPos != shieldPrm[shieldId].currentPosition - shieldPrm[shieldId].relativePos)) { 00881 #ifdef _DEBUG_Easyspin 00882 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "B EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); 00883 Serial.println(EasyspinStrOut); 00884 #endif 00885 shieldPrm[shieldId].relativePos = shieldPrm[shieldId].currentPosition - AbsPos; 00886 } 00887 } 00888 } 00889 00890 switch (shieldPrm[shieldId].motionState) { 00891 case ACCELERATING: { 00892 if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| 00893 ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& 00894 (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].startDecPos))) { 00895 shieldPrm[shieldId].motionState = DECELERATING; 00896 shieldPrm[shieldId].accu = 0; 00897 #ifdef _DEBUG_Easyspin 00898 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Dec: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); 00899 Serial.println(EasyspinStrOut); 00900 #endif 00901 } else if ((shieldPrm[shieldId].speed >= shieldPrm[shieldId].maxSpeed)|| 00902 ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& 00903 (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].endAccPos))) { 00904 shieldPrm[shieldId].motionState = STEADY; 00905 #ifdef _DEBUG_Easyspin 00906 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); 00907 Serial.println(EasyspinStrOut); 00908 #endif 00909 } else { 00910 bool speedUpdated = false; 00911 /* Go on accelerating */ 00912 shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].acceleration << 16) / shieldPrm[shieldId].speed; 00913 while (shieldPrm[shieldId].accu >= (0X10000L)) { 00914 shieldPrm[shieldId].accu -= (0X10000L); 00915 shieldPrm[shieldId].speed +=1; 00916 speedUpdated = true; 00917 } 00918 00919 if (speedUpdated) { 00920 ApplySpeed(shieldId, shieldPrm[shieldId].speed); 00921 } 00922 } 00923 break; 00924 } 00925 case STEADY: { 00926 if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| 00927 ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& 00928 (shieldPrm[shieldId].relativePos >= (shieldPrm[shieldId].startDecPos))) || 00929 ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& 00930 (shieldPrm[shieldId].speed > shieldPrm[shieldId].maxSpeed))) { 00931 shieldPrm[shieldId].motionState = DECELERATING; 00932 shieldPrm[shieldId].accu = 0; 00933 } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& 00934 (shieldPrm[shieldId].speed < shieldPrm[shieldId].maxSpeed)) { 00935 shieldPrm[shieldId].motionState = ACCELERATING; 00936 shieldPrm[shieldId].accu = 0; 00937 } 00938 break; 00939 } 00940 case DECELERATING: { 00941 if (((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)&&(shieldPrm[shieldId].speed <= shieldPrm[shieldId].minSpeed))|| 00942 ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& 00943 (shieldPrm[shieldId].relativePos >= shieldPrm[shieldId].stepsToTake))) { 00944 /* Motion process complete */ 00945 HardStop(shieldId); 00946 #ifdef _DEBUG_Easyspin 00947 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Stop: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos ); 00948 Serial.println(EasyspinStrOut); 00949 #endif 00950 } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& 00951 (shieldPrm[shieldId].speed <= shieldPrm[shieldId].maxSpeed)) { 00952 shieldPrm[shieldId].motionState = STEADY; 00953 #ifdef _DEBUG_Easyspin 00954 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); 00955 Serial.println(EasyspinStrOut); 00956 #endif 00957 } else { 00958 /* Go on decelerating */ 00959 if (shieldPrm[shieldId].speed > shieldPrm[shieldId].minSpeed) { 00960 bool speedUpdated = false; 00961 shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].deceleration << 16) / shieldPrm[shieldId].speed; 00962 while (shieldPrm[shieldId].accu >= (0X10000L)) { 00963 shieldPrm[shieldId].accu -= (0X10000L); 00964 shieldPrm[shieldId].speed -=1; 00965 speedUpdated = true; 00966 } 00967 if (speedUpdated) { 00968 ApplySpeed(shieldId, shieldPrm[shieldId].speed); 00969 } 00970 } 00971 } 00972 break; 00973 } 00974 default: { 00975 break; 00976 } 00977 } 00978 /* Set isr flag */ 00979 isrFlag = false; 00980 } 00981 00982 /******************************************************//** 00983 * @brief Updates the current speed of the shield 00984 * @param[in] shieldId (from 0 to 2) 00985 * @param[in] newSpeed in pps 00986 * @retval None 00987 **********************************************************/ 00988 void Easyspin::ApplySpeed(uint8_t shieldId, uint16_t newSpeed) 00989 { 00990 if (newSpeed < Easyspin_MIN_PWM_FREQ) { 00991 newSpeed = Easyspin_MIN_PWM_FREQ; 00992 } 00993 if (newSpeed > Easyspin_MAX_PWM_FREQ) { 00994 newSpeed = Easyspin_MAX_PWM_FREQ; 00995 } 00996 00997 shieldPrm[shieldId].speed = newSpeed; 00998 00999 switch (shieldId) { 01000 case 0: 01001 Pwm1SetFreq(newSpeed); 01002 break; 01003 case 1: 01004 Pwm2SetFreq(newSpeed); 01005 break; 01006 case 2: 01007 Pwm3SetFreq(newSpeed); 01008 break; 01009 default: 01010 break; //ignore error 01011 } 01012 } 01013 01014 /******************************************************//** 01015 * @brief Computes the speed profile according to the number of steps to move 01016 * @param[in] shieldId (from 0 to 2) 01017 * @param[in] nbSteps number of steps to perform 01018 * @retval None 01019 * @note Using the acceleration and deceleration of the shield, 01020 * this function determines the duration in steps of the acceleration, 01021 * steady and deceleration phases. 01022 * If the total number of steps to perform is big enough, a trapezoidal move 01023 * is performed (i.e. there is a steady phase where the motor runs at the maximum 01024 * speed. 01025 * Else, a triangular move is performed (no steady phase: the maximum speed is never 01026 * reached. 01027 **********************************************************/ 01028 void Easyspin::ComputeSpeedProfile(uint8_t shieldId, uint32_t nbSteps) 01029 { 01030 uint32_t reqAccSteps; 01031 uint32_t reqDecSteps; 01032 01033 /* compute the number of steps to get the targeted speed */ 01034 reqAccSteps = (shieldPrm[shieldId].maxSpeed - shieldPrm[shieldId].minSpeed); 01035 reqAccSteps *= (shieldPrm[shieldId].maxSpeed + shieldPrm[shieldId].minSpeed); 01036 reqDecSteps = reqAccSteps; 01037 reqAccSteps /= (uint32_t)shieldPrm[shieldId].acceleration; 01038 reqAccSteps /= 2; 01039 01040 /* compute the number of steps to stop */ 01041 reqDecSteps /= (uint32_t)shieldPrm[shieldId].deceleration; 01042 reqDecSteps /= 2; 01043 01044 if(( reqAccSteps + reqDecSteps ) > nbSteps) { 01045 /* Triangular move */ 01046 /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ 01047 01048 reqDecSteps = ((uint32_t) shieldPrm[shieldId].deceleration * nbSteps) / (shieldPrm[shieldId].acceleration + shieldPrm[shieldId].deceleration); 01049 if (reqDecSteps > 1) { 01050 reqAccSteps = reqDecSteps - 1; 01051 if(reqAccSteps == 0) { 01052 reqAccSteps = 1; 01053 } 01054 } else { 01055 reqAccSteps = 0; 01056 } 01057 shieldPrm[shieldId].endAccPos = reqAccSteps; 01058 shieldPrm[shieldId].startDecPos = reqDecSteps; 01059 } else { 01060 /* Trapezoidal move */ 01061 /* accelerating phase to endAccPos */ 01062 /* steady phase from endAccPos to startDecPos */ 01063 /* decelerating from startDecPos to stepsToTake*/ 01064 shieldPrm[shieldId].endAccPos = reqAccSteps; 01065 shieldPrm[shieldId].startDecPos = nbSteps - reqDecSteps - 1; 01066 } 01067 } 01068 01069 /******************************************************//** 01070 * @brief Converts the ABS_POSITION register value to a 32b signed integer 01071 * @param[in] abs_position_reg value of the ABS_POSITION register 01072 * @retval operation_result 32b signed integer corresponding to the absolute position 01073 **********************************************************/ 01074 int32_t Easyspin::ConvertPosition(uint32_t abs_position_reg) 01075 { 01076 int32_t operation_result; 01077 01078 if (abs_position_reg & Easyspin_ABS_POS_SIGN_BIT_MASK) { 01079 /* Negative register value */ 01080 abs_position_reg = ~abs_position_reg; 01081 abs_position_reg += 1; 01082 01083 operation_result = (int32_t) (abs_position_reg & Easyspin_ABS_POS_VALUE_MASK); 01084 operation_result = -operation_result; 01085 } else { 01086 operation_result = (int32_t) abs_position_reg; 01087 } 01088 return operation_result; 01089 } 01090 01091 /******************************************************//** 01092 * @brief Handlers of the flag interrupt which calls the user callback (if defined) 01093 * @param None 01094 * @retval None 01095 **********************************************************/ 01096 void Easyspin::FlagInterruptHandler(void) 01097 { 01098 if (flagInterruptCallback != NULL) { 01099 /* Set isr flag */ 01100 isrFlag = true; 01101 01102 flagInterruptCallback(); 01103 01104 /* Reset isr flag */ 01105 isrFlag = false; 01106 } 01107 } 01108 01109 /******************************************************//** 01110 * @brief Sends a command without arguments to the Easyspin via the SPI 01111 * @param[in] shieldId (from 0 to 2) 01112 * @param[in] param Command to send 01113 * @retval None 01114 **********************************************************/ 01115 void Easyspin::SendCommand(uint8_t shieldId, uint8_t param) 01116 { 01117 uint8_t spiIndex = numberOfShields - shieldId - 1; 01118 bool itDisable = false; 01119 01120 do { 01121 spiPreemtionByIsr = false; 01122 if (itDisable) { 01123 /* re-enable interrupts if disable in previous iteration */ 01124 flag.enable_irq(); 01125 itDisable = false; 01126 } 01127 01128 for (uint32_t i = 0; i < numberOfShields; i++) { 01129 spiTxBursts[3][i] = Easyspin_NOP; 01130 } 01131 spiTxBursts[3][spiIndex] = param; 01132 01133 /* Disable interruption before checking */ 01134 /* pre-emption by ISR and SPI transfers*/ 01135 flag.disable_irq(); 01136 itDisable = true; 01137 } while (spiPreemtionByIsr); // check pre-emption by ISR 01138 01139 WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); 01140 01141 /* re-enable interrupts after SPI transfers*/ 01142 flag.enable_irq(); 01143 } 01144 01145 /******************************************************//** 01146 * @brief Sets the registers of the Easyspin to their predefined values 01147 * from Easyspin_target_config.h 01148 * @param[in] shieldId (from 0 to 2) 01149 * @retval None 01150 **********************************************************/ 01151 void Easyspin::SetRegisterToPredefinedValues(uint8_t shieldId) 01152 { 01153 CmdSetParam(shieldId, 01154 Easyspin_ABS_POS, 01155 0); 01156 CmdSetParam(shieldId, 01157 Easyspin_EL_POS, 01158 0); 01159 CmdSetParam(shieldId, 01160 Easyspin_MARK, 01161 0); 01162 switch (shieldId) { 01163 case 0: 01164 CmdSetParam(shieldId, 01165 Easyspin_TVAL, 01166 Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_0)); 01167 CmdSetParam(shieldId, 01168 Easyspin_T_FAST, 01169 (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_0 | 01170 (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_0); 01171 CmdSetParam(shieldId, 01172 Easyspin_TON_MIN, 01173 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_0)); 01174 CmdSetParam(shieldId, 01175 Easyspin_TOFF_MIN, 01176 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_0)); 01177 CmdSetParam(shieldId, 01178 Easyspin_OCD_TH, 01179 Easyspin_CONF_PARAM_OCD_TH_SHIELD_0); 01180 CmdSetParam(shieldId, 01181 Easyspin_STEP_MODE, 01182 (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_0 | 01183 (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_0); 01184 CmdSetParam(shieldId, 01185 Easyspin_ALARM_EN, 01186 Easyspin_CONF_PARAM_ALARM_EN_SHIELD_0); 01187 CmdSetParam(shieldId, 01188 Easyspin_CONFIG, 01189 (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_0 | 01190 (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_0 | 01191 (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_0 | 01192 (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_0 | 01193 (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_0); 01194 break; 01195 case 1: 01196 CmdSetParam(shieldId, 01197 Easyspin_TVAL, 01198 Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_1)); 01199 CmdSetParam(shieldId, 01200 Easyspin_T_FAST, 01201 (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_1 | 01202 (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_1); 01203 CmdSetParam(shieldId, 01204 Easyspin_TON_MIN, 01205 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_1)); 01206 CmdSetParam(shieldId, 01207 Easyspin_TOFF_MIN, 01208 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_1)); 01209 CmdSetParam(shieldId, 01210 Easyspin_OCD_TH, 01211 Easyspin_CONF_PARAM_OCD_TH_SHIELD_1); 01212 CmdSetParam(shieldId, 01213 Easyspin_STEP_MODE, 01214 (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_1 | 01215 (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_1); 01216 CmdSetParam(shieldId, 01217 Easyspin_ALARM_EN, 01218 Easyspin_CONF_PARAM_ALARM_EN_SHIELD_1); 01219 CmdSetParam(shieldId, 01220 Easyspin_CONFIG, 01221 (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_1 | 01222 (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_1 | 01223 (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_1 | 01224 (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_1 | 01225 (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_1); 01226 break; 01227 case 2: 01228 CmdSetParam(shieldId, 01229 Easyspin_TVAL, 01230 Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_2)); 01231 CmdSetParam(shieldId, 01232 Easyspin_T_FAST, 01233 (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_2 | 01234 (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_2); 01235 CmdSetParam(shieldId, 01236 Easyspin_TON_MIN, 01237 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_2)); 01238 CmdSetParam(shieldId, 01239 Easyspin_TOFF_MIN, 01240 Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_2)); 01241 CmdSetParam(shieldId, 01242 Easyspin_OCD_TH, 01243 Easyspin_CONF_PARAM_OCD_TH_SHIELD_2); 01244 CmdSetParam(shieldId, 01245 Easyspin_STEP_MODE, 01246 (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_2 | 01247 (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_2); 01248 CmdSetParam(shieldId, 01249 Easyspin_ALARM_EN, 01250 Easyspin_CONF_PARAM_ALARM_EN_SHIELD_2); 01251 CmdSetParam(shieldId, 01252 Easyspin_CONFIG, 01253 (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_2 | 01254 (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_2 | 01255 (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_2 | 01256 (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_2 | 01257 (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_2); 01258 break; 01259 default: 01260 ; 01261 } 01262 } 01263 01264 /******************************************************//** 01265 * @brief Sets the registers of the Easyspin to their predefined values 01266 * from Easyspin_target_config.h 01267 * @param[in] shieldId (from 0 to 2) 01268 * @retval None 01269 **********************************************************/ 01270 void Easyspin::WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) 01271 { 01272 CS = 0; 01273 for (uint32_t i = 0; i < numberOfShields; i++) { 01274 *pReceivedByte = spi.write(*pByteToTransmit); 01275 pByteToTransmit++; 01276 pReceivedByte++; 01277 } 01278 CS = 1; 01279 if (isrFlag) { 01280 spiPreemtionByIsr = true; 01281 } 01282 } 01283 01284 /******************************************************//** 01285 * @brief Initialises the PWM uses by the specified shield 01286 * @param[in] shieldId (from 0 to 2) 01287 * @retval None 01288 * @note Shield 0 uses PW1 based on timer 1 01289 * Shield 1 uses PWM 2 based on timer 2 01290 * Shield 2 uses PWM3 based timer 0 01291 **********************************************************/ 01292 void Easyspin::PwmInit(uint8_t shieldId) 01293 { 01294 switch (shieldId) { 01295 case 0: 01296 /* PWM1 uses timer 1 */ 01297 /* Initialise timer by setting waveform generation mode 01298 to PWM phase and Frequency correct: mode = 8 01299 (WGM10 = 0, WGM11 = 0, WGM12 = 0, WGM13 = 1) */ 01300 01301 /* Stop timer1 by clearing CSS bits and set WGM13 and WGM12 */ 01302 //TCCR1B = 0x10; 01303 01304 /* Set WGM10 and WGM11 */ 01305 //TCCR1A = 0x00; 01306 01307 /* Disable Timer1 interrupt */ 01308 //TIMSK1 = 0; 01309 //pwm1.period_us(400); 01310 //pwm1.pulsewidth_us(200); 01311 01312 break; 01313 case 1: 01314 /* PWM2 uses timer 2 */ 01315 /* Initialise timer by setting waveform generation mode 01316 to PWM phase correct: mode = 5 01317 (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ 01318 01319 /* Stop timer2 by clearing CSS bits and set WGM22 */ 01320 //TCCR2B = 0x08; 01321 01322 /* Set WGM20 and WGM21 */ 01323 //TCCR2A = 0x01; 01324 01325 /* Disable Timer2 interrupt */ 01326 //TIMSK2 = 0; 01327 //pwm2.period_us(500); 01328 //pwm2.pulsewidth_us(100); 01329 break; 01330 01331 01332 case 2: 01333 /* PWM3 uses timer 0 */ 01334 /* !!!!! Caution: Calling this configuration will break */ 01335 /* all default Arduino's timing functions as delay(),millis()... */ 01336 01337 /* Initialise timer by setting waveform generation mode 01338 to PWM phase correct: mode = 5 01339 (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ 01340 01341 /* Stop timer0 by clearing CSS bits and set WGM22 */ 01342 //TCCR0B = 0x08; 01343 01344 /* Set WGM00 and WGM01 */ 01345 //TCCR0A = 0x01; 01346 01347 /* Disable Timer0 interrupt */ 01348 //TIMSK0 = 0; 01349 //pwm3.period_ms(10); 01350 //pwm3.pulsewidth_ms(1); 01351 break; 01352 default: 01353 break;//ignore error 01354 } 01355 } 01356 01357 /******************************************************//** 01358 * @brief Ticker1 01359 * @param[in] 01360 * @retval 01361 **********************************************************/ 01362 void Easyspin::tick1() 01363 { 01364 class Easyspin* instancePtr = Easyspin::GetInstancePtr(); 01365 pwm1 = !pwm1; 01366 if (instancePtr != NULL) { 01367 if (instancePtr->GetShieldState(0) != INACTIVE) { 01368 instancePtr->StepClockHandler(0); 01369 } 01370 } 01371 } 01372 01373 /******************************************************//** 01374 * @brief Ticker2 01375 * @param[in] 01376 * @retval 01377 **********************************************************/ 01378 void Easyspin::tick2() 01379 { 01380 class Easyspin* instancePtr = Easyspin::GetInstancePtr(); 01381 pwm2 = !pwm2; 01382 if (instancePtr != NULL) { 01383 if (instancePtr->GetShieldState(1) != INACTIVE) { 01384 instancePtr->StepClockHandler(1); 01385 } 01386 } 01387 } 01388 01389 /******************************************************//** 01390 * @brief Sets the frequency of PWM1 used by shield 0 01391 * @param[in] newFreq in Hz 01392 * @retval None 01393 * @note The frequency is directly the current speed of the shield 01394 **********************************************************/ 01395 void Easyspin::Pwm1SetFreq(uint16_t newFreq) 01396 { 01397 uint16_t us; 01398 us = (1000000 / (newFreq * 2)); 01399 ticker1.attach_us(this, &Easyspin::tick1, us); 01400 } 01401 01402 /******************************************************//** 01403 * @brief Sets the frequency of PWM2 used by shield 1 01404 * @param[in] newFreq in Hz 01405 * @retval None 01406 * @note The frequency is directly the current speed of the shield 01407 **********************************************************/ 01408 void Easyspin::Pwm2SetFreq(uint16_t newFreq) 01409 { 01410 uint16_t us; 01411 us = (1000000 / (newFreq * 2)); 01412 ticker2.attach_us(this, &Easyspin::tick2, us); 01413 } 01414 01415 /******************************************************//** 01416 * @brief Sets the frequency of PWM3 used by shield 2 01417 * @param[in] newFreq in Hz 01418 * @retval None 01419 * @note The frequency is directly the current speed of the shield 01420 **********************************************************/ 01421 void Easyspin::Pwm3SetFreq(uint16_t newFreq) 01422 { 01423 01424 } 01425 01426 /******************************************************//** 01427 * @brief Stops the PWM uses by the specified shield 01428 * @param[in] shieldId (from 0 to 2) 01429 * @retval None 01430 **********************************************************/ 01431 void Easyspin::PwmStop(uint8_t shieldId) 01432 { 01433 switch (shieldId) { 01434 case 0: 01435 ticker1.detach(); 01436 break; 01437 case 1: 01438 ticker2.detach(); 01439 break; 01440 case 2: 01441 ticker3.detach(); 01442 break; 01443 default: 01444 break;//ignore error 01445 } 01446 } 01447 01448 /******************************************************//** 01449 * @brief Sets the parameters of the shield to predefined values 01450 * from Easyspin_target_config.h 01451 * @param None 01452 * @retval None 01453 **********************************************************/ 01454 void Easyspin::SetShieldParamsToPredefinedValues(void) 01455 { 01456 shieldPrm[0].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_0; 01457 shieldPrm[0].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_0; 01458 shieldPrm[0].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_0; 01459 shieldPrm[0].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_0; 01460 01461 shieldPrm[1].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_1; 01462 shieldPrm[1].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_1; 01463 shieldPrm[1].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_1; 01464 shieldPrm[1].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_1; 01465 01466 shieldPrm[2].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_2; 01467 shieldPrm[2].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_2; 01468 shieldPrm[2].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_2; 01469 shieldPrm[2].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_2; 01470 01471 for (uint8_t i = 0; i < numberOfShields; i++) { 01472 SetRegisterToPredefinedValues(i); 01473 } 01474 } 01475 01476 /******************************************************//** 01477 * @brief Initialises the bridge parameters to start the movement 01478 * and enable the power bridge 01479 * @param[in] shieldId (from 0 to 2) 01480 * @retval None 01481 **********************************************************/ 01482 void Easyspin::StartMovement(uint8_t shieldId) 01483 { 01484 /* Enable Easyspin powerstage */ 01485 CmdEnable(shieldId); 01486 01487 if (shieldPrm[shieldId].endAccPos != 0) { 01488 shieldPrm[shieldId].motionState = ACCELERATING;; 01489 } else { 01490 shieldPrm[shieldId].motionState = DECELERATING; 01491 } 01492 01493 shieldPrm[shieldId].accu = 0; 01494 shieldPrm[shieldId].relativePos = 0; 01495 ApplySpeed(shieldId, shieldPrm[shieldId].minSpeed); 01496 #ifdef _DEBUG_Easyspin 01497 snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Stop->Acc: speed: %u relPos: %ld\n", shieldPrm[shieldId].minSpeed, shieldPrm[shieldId].relativePos) ; 01498 Serial.println(EasyspinStrOut); 01499 #endif 01500 } 01501 01502 /******************************************************//** 01503 * @brief Converts mA in compatible values for TVAL register 01504 * @param[in] Tval 01505 * @retval TVAL values 01506 **********************************************************/ 01507 inline uint8_t Easyspin::Tval_Current_to_Par(double Tval) 01508 { 01509 return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); 01510 } 01511 01512 /******************************************************//** 01513 * @brief Convert time in us in compatible values 01514 * for TON_MIN register 01515 * @param[in] Tmin 01516 * @retval TON_MIN values 01517 **********************************************************/ 01518 inline uint8_t Easyspin::Tmin_Time_to_Par(double Tmin) 01519 { 01520 return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); 01521 }
Generated on Tue Jul 12 2022 21:33:48 by
1.7.2