Robotique FIP
Speed profile working
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