Speed profile working
easyspin.cpp
- Committer:
- julientiron
- Date:
- 2015-08-26
- Revision:
- 1:9efe863db15e
- Parent:
- 0:cba942f8172a
File content as of revision 1:9efe863db15e:
/******************************************************//** * @file easyspin.cpp * @version V1.0 * @date June 29, 2015 * @brief easyspin library for mbed * * This file is free software; you can redistribute it and/or modify * it under the terms of either the GNU General Public License version 2 * or the GNU Lesser General Public License version 2.1, both as * published by the Free Software Foundation. **********************************************************/ #include "easyspin.h" #include "mbed.h" const uint16_t Easyspin::prescalerArrayTimer0_1[PRESCALER_ARRAY_TIMER0_1_SIZE] = { 0, 1, 8, 64, 256, 1024}; const uint16_t Easyspin::prescalerArrayTimer2[PRESCALER_ARRAY_TIMER2_SIZE] = {0, 1, 8, 32, 64, 128, 256, 1024}; volatile void (*Easyspin::flagInterruptCallback)(void); volatile uint8_t Easyspin::numberOfShields; uint8_t Easyspin::spiTxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; uint8_t Easyspin::spiRxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; volatile bool Easyspin::spiPreemtionByIsr = false; volatile bool Easyspin::isrFlag = false; volatile class Easyspin* Easyspin::instancePtr = NULL; /******************************************************//** * @brief Constructor * @param None * @retval None **********************************************************/ Easyspin::Easyspin():reset(Easyspin_Reset_Pin), dir1(Easyspin_DIR_1_Pin), dir2(Easyspin_DIR_2_Pin), dir3(Easyspin_DIR_3_Pin), CS(Easyspin_CS_Pin), flag(Easyspin_FLAG_Pin), pwm1(Easyspin_PWM_1_Pin), pwm2(Easyspin_PWM_2_Pin), pwm3(Easyspin_PWM_3_Pin), spi(Easyspin_MOSI_Pin, Easyspin_MISO_Pin, Easyspin_SCK_Pin) { uint8_t i; for (i = 0; i < MAX_NUMBER_OF_SHIELDS; i++) { shieldPrm[i].motionState = INACTIVE; shieldPrm[i].commandExecuted = NO_CMD; shieldPrm[i].stepsToTake = MAX_STEPS; } instancePtr = this; } /******************************************************//** * @brief Attaches a user callback to the flag Interrupt * The call back will be then called each time the status * flag pin will be pulled down due to the occurrence of * a programmed alarms ( OCD, thermal pre-warning or * shutdown, UVLO, wrong command, non-performable command) * @param[in] callback Name of the callback to attach * to the Flag Interrupt * @retval None **********************************************************/ void Easyspin::AttachFlagInterrupt(void (*callback)(void)) { flagInterruptCallback = (volatile void (*)())callback; } /******************************************************//** * @brief Starts the Easyspin library * @param[in] nbShields Number of Easyspin shields to use (from 1 to 3) * @retval None **********************************************************/ void Easyspin::Begin(uint8_t nbShields) { numberOfShields = nbShields; // start the SPI library: //SPI.begin(); //SPI.setBitOrder(MSBFIRST); spi.format(8, 3); //SPI.setClockDivider(SPI_CLOCK_DIV4); // flag pin //pinMode(Easyspin_FLAG_Pin, INPUT_PULLUP); flag.mode(PullUp); flag.fall(&FlagInterruptHandler); //reset pin //pinMode(Easyspin_Reset_Pin, OUTPUT); switch (nbShields) { case 3: //pinMode(Easyspin_DIR_3_Pin, OUTPUT); //pinMode(Easyspin_PWM_3_Pin, OUTPUT); PwmInit(2); case 2: //pinMode(Easyspin_DIR_2_Pin, OUTPUT); //pinMode(Easyspin_PWM_2_Pin, OUTPUT); PwmInit(1); case 1: //pinMode(Easyspin_DIR_1_Pin, OUTPUT); //pinMode(Easyspin_PWM_1_Pin, OUTPUT); PwmInit(0); default: ; } /* Standby-reset deactivation */ ReleaseReset(); /* Set all registers and context variables to the predefined values from Easyspin_target_config.h */ SetShieldParamsToPredefinedValues(); /* Disable Easyspin powerstage */ for (uint32_t i = 0; i < nbShields; i++) { CmdDisable(i); /* Get Status to clear flags after start up */ CmdGetStatus(i); } } /******************************************************//** * @brief Returns the acceleration of the specified shield * @param[in] shieldId (from 0 to 2) * @retval Acceleration in pps^2 **********************************************************/ uint16_t Easyspin::GetAcceleration(uint8_t shieldId) { return (shieldPrm[shieldId].acceleration); } /******************************************************//** * @brief Returns the current speed of the specified shield * @param[in] shieldId (from 0 to 2) * @retval Speed in pps **********************************************************/ uint16_t Easyspin::GetCurrentSpeed(uint8_t shieldId) { return shieldPrm[shieldId].speed; } /******************************************************//** * @brief Returns the deceleration of the specified shield * @param[in] shieldId (from 0 to 2) * @retval Deceleration in pps^2 **********************************************************/ uint16_t Easyspin::GetDeceleration(uint8_t shieldId) { return (shieldPrm[shieldId].deceleration); } /******************************************************//** * @brief Returns the FW version of the library * @param None * @retval Easyspin_FW_VERSION **********************************************************/ uint8_t Easyspin::GetFwVersion(void) { return (Easyspin_FW_VERSION); } /******************************************************//** * @brief Returns the mark position of the specified shield * @param[in] shieldId (from 0 to 2) * @retval Mark register value converted in a 32b signed integer **********************************************************/ int32_t Easyspin::GetMark(uint8_t shieldId) { return ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); } /******************************************************//** * @brief Returns the max speed of the specified shield * @param[in] shieldId (from 0 to 2) * @retval maxSpeed in pps **********************************************************/ uint16_t Easyspin::GetMaxSpeed(uint8_t shieldId) { return (shieldPrm[shieldId].maxSpeed); } /******************************************************//** * @brief Returns the min speed of the specified shield * @param[in] shieldId (from 0 to 2) * @retval minSpeed in pps **********************************************************/ uint16_t Easyspin::GetMinSpeed(uint8_t shieldId) { return (shieldPrm[shieldId].minSpeed); } /******************************************************//** * @brief Returns the ABS_POSITION of the specified shield * @param[in] shieldId (from 0 to 2) * @retval ABS_POSITION register value converted in a 32b signed integer **********************************************************/ int32_t Easyspin::GetPosition(uint8_t shieldId) { return ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); } /******************************************************//** * @brief Returns the shield state * @param[in] shieldId (from 0 to 2) * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) **********************************************************/ shieldState_t Easyspin::GetShieldState(uint8_t shieldId) { return shieldPrm[shieldId].motionState; } /******************************************************//** * @brief Requests the motor to move to the home position (ABS_POSITION = 0) * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::GoHome(uint8_t shieldId) { GoTo(shieldId, 0); } /******************************************************//** * @brief Requests the motor to move to the mark position * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::GoMark(uint8_t shieldId) { uint32_t mark; mark = ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); GoTo(shieldId,mark); } /******************************************************//** * @brief Requests the motor to move to the specified position * @param[in] shieldId (from 0 to 2) * @param[in] targetPosition absolute position in steps * @retval None **********************************************************/ void Easyspin::GoTo(uint8_t shieldId, int32_t targetPosition) { dir_t direction; int32_t steps; /* Eventually deactivate motor */ if (shieldPrm[shieldId].motionState != INACTIVE) { HardStop(shieldId); } /* Get current position */ shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); /* Compute the number of steps to perform */ steps = targetPosition - shieldPrm[shieldId].currentPosition; if (steps >= 0) { shieldPrm[shieldId].stepsToTake = steps; direction = FORWARD; } else { shieldPrm[shieldId].stepsToTake = -steps; direction = BACKWARD; } if (steps != 0) { shieldPrm[shieldId].commandExecuted = MOVE_CMD; /* Direction setup */ SetDirection(shieldId,direction); ComputeSpeedProfile(shieldId, shieldPrm[shieldId].stepsToTake); /* Motor activation */ StartMovement(shieldId); } } /******************************************************//** * @brief Immediatly stops the motor and disable the power bridge * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::HardStop(uint8_t shieldId) { /* Disable corresponding PWM */ PwmStop(shieldId); /* Disable power stage */ CmdDisable(shieldId); /* Set inactive state */ shieldPrm[shieldId].motionState = INACTIVE; shieldPrm[shieldId].commandExecuted = NO_CMD; shieldPrm[shieldId].stepsToTake = MAX_STEPS; #ifdef _DEBUG_Easyspin Serial.println("Inactive\n"); #endif } /******************************************************//** * @brief Moves the motor of the specified number of steps * @param[in] shieldId (from 0 to 2) * @param[in] direction FORWARD or BACKWARD * @param[in] stepCount Number of steps to perform * @retval None **********************************************************/ void Easyspin::Move(uint8_t shieldId, dir_t direction, uint32_t stepCount) { /* Eventually deactivate motor */ if (shieldPrm[shieldId].motionState != INACTIVE) { HardStop(shieldId); } if (stepCount != 0) { shieldPrm[shieldId].stepsToTake = stepCount; shieldPrm[shieldId].commandExecuted = MOVE_CMD; shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); /* Direction setup */ SetDirection(shieldId,direction); ComputeSpeedProfile(shieldId, stepCount); /* Motor activation */ StartMovement(shieldId); } } /******************************************************//** * @brief Turn left or right with the specified angle * @param[in] rotation RIGHT or LEFT * @param[in] angle in degrees * @retval None **********************************************************/ void Easyspin::Turn(rot_t rotation, uint16_t angle) { uint16_t nbStep = (10934*angle)/360; if(rotation==RIGHT) { Move(0, FORWARD, nbStep); Move(1, FORWARD, nbStep); } else { Move(0, BACKWARD, nbStep); Move(1, BACKWARD, nbStep); } } /******************************************************//** * @brief Move forward or backward with the specified distance * @param[in] direction FORWARD or BACKWARD * @param[in] distance in cm * @retval None **********************************************************/ void Easyspin::Move_cm(dir_t direction, uint16_t distance) { uint16_t nbStep = (10000 * distance)/61; if(direction==FORWARD) { Move(0, FORWARD, nbStep); Move(1, BACKWARD, nbStep); } else { Move(0, BACKWARD, nbStep); Move(1, FORWARD, nbStep); } } /******************************************************//** * @brief Resets all Easyspin shields * @param None * @retval None **********************************************************/ void Easyspin::ResetAllShields(void) { uint8_t loop; for (loop = 0; loop < numberOfShields; loop++) { /* Stop movement and disable power stage*/ HardStop(loop); } Reset(); wait_us(20); // Reset pin must be forced low for at least 10us ReleaseReset(); } /******************************************************//** * @brief Runs the motor. It will accelerate from the min * speed up to the max speed by using the shield acceleration. * @param[in] shieldId (from 0 to 2) * @param[in] direction FORWARD or BACKWARD * @retval None **********************************************************/ void Easyspin::Run(uint8_t shieldId, dir_t direction) { /* Eventually deactivate motor */ if (shieldPrm[shieldId].motionState != INACTIVE) { HardStop(shieldId); } /* Direction setup */ SetDirection(shieldId,direction); shieldPrm[shieldId].commandExecuted = RUN_CMD; /* Motor activation */ StartMovement(shieldId); } /******************************************************//** * @brief Changes the acceleration of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] newAcc New acceleration to apply in pps^2 * @retval true if the command is successfully executed, else false * @note The command is not performed is the shield is executing * a MOVE or GOTO command (but it can be used during a RUN command) **********************************************************/ bool Easyspin::SetAcceleration(uint8_t shieldId,uint16_t newAcc) { bool cmdExecuted = false; if ((newAcc != 0)&& ((shieldPrm[shieldId].motionState == INACTIVE)|| (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { shieldPrm[shieldId].acceleration = newAcc; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Changes the deceleration of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] newDec New deceleration to apply in pps^2 * @retval true if the command is successfully executed, else false * @note The command is not performed is the shield is executing * a MOVE or GOTO command (but it can be used during a RUN command) **********************************************************/ bool Easyspin::SetDeceleration(uint8_t shieldId, uint16_t newDec) { bool cmdExecuted = false; if ((newDec != 0)&& ((shieldPrm[shieldId].motionState == INACTIVE)|| (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { shieldPrm[shieldId].deceleration = newDec; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Set current position to be the Home position (ABS pos set to 0) * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::SetHome(uint8_t shieldId) { CmdSetParam(shieldId, Easyspin_ABS_POS, 0); } /******************************************************//** * @brief Sets current position to be the Mark position * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::SetMark(uint8_t shieldId) { uint32_t mark = CmdGetParam(shieldId,Easyspin_ABS_POS); CmdSetParam(shieldId,Easyspin_MARK, mark); } /******************************************************//** * @brief Changes the max speed of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] newMaxSpeed New max speed to apply in pps * @retval true if the command is successfully executed, else false * @note The command is not performed is the shield is executing * a MOVE or GOTO command (but it can be used during a RUN command). **********************************************************/ bool Easyspin::SetMaxSpeed(uint8_t shieldId, uint16_t newMaxSpeed) { bool cmdExecuted = false; if ((newMaxSpeed > Easyspin_MIN_PWM_FREQ)&& (newMaxSpeed <= Easyspin_MAX_PWM_FREQ) && (shieldPrm[shieldId].minSpeed <= newMaxSpeed) && ((shieldPrm[shieldId].motionState == INACTIVE)|| (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { shieldPrm[shieldId].maxSpeed = newMaxSpeed; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Changes the min speed of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] newMinSpeed New min speed to apply in pps * @retval true if the command is successfully executed, else false * @note The command is not performed is the shield is executing * a MOVE or GOTO command (but it can be used during a RUN command). **********************************************************/ bool Easyspin::SetMinSpeed(uint8_t shieldId, uint16_t newMinSpeed) { bool cmdExecuted = false; if ((newMinSpeed >= Easyspin_MIN_PWM_FREQ)&& (newMinSpeed < Easyspin_MAX_PWM_FREQ) && (newMinSpeed <= shieldPrm[shieldId].maxSpeed) && ((shieldPrm[shieldId].motionState == INACTIVE)|| (shieldPrm[shieldId].commandExecuted == RUN_CMD))) { shieldPrm[shieldId].minSpeed = newMinSpeed; cmdExecuted = true; } return cmdExecuted; } /******************************************************//** * @brief Stops the motor by using the shield deceleration * @param[in] shieldId (from 0 to 2) * @retval true if the command is successfully executed, else false * @note The command is not performed is the shield is in INACTIVE state. **********************************************************/ bool Easyspin::SoftStop(uint8_t shieldId) { bool cmdExecuted = false; if (shieldPrm[shieldId].motionState != INACTIVE) { shieldPrm[shieldId].commandExecuted = SOFT_STOP_CMD; cmdExecuted = true; } return (cmdExecuted); } /******************************************************//** * @brief Locks until the shield state becomes Inactive * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::WaitWhileActive(uint8_t shieldId) { /* Wait while motor is running */ while (GetShieldState(shieldId) != INACTIVE); } /******************************************************//** * @brief Issue the Disable command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::CmdDisable(uint8_t shieldId) { SendCommand(shieldId, Easyspin_DISABLE); } /******************************************************//** * @brief Issues the Enable command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::CmdEnable(uint8_t shieldId) { SendCommand(shieldId, Easyspin_ENABLE); } /******************************************************//** * @brief Issues the GetParam command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) * @retval Register value **********************************************************/ uint32_t Easyspin::CmdGetParam(uint8_t shieldId, Easyspin_Registers_t param) { uint32_t i; uint32_t spiRxData; uint8_t maxArgumentNbBytes = 0; uint8_t spiIndex = numberOfShields - shieldId - 1; bool itDisable = false; do { spiPreemtionByIsr = false; if (itDisable) { /* re-enable interrupts if disable in previous iteration */ flag.enable_irq(); itDisable = false; } for (i = 0; i < numberOfShields; i++) { spiTxBursts[0][i] = Easyspin_NOP; spiTxBursts[1][i] = Easyspin_NOP; spiTxBursts[2][i] = Easyspin_NOP; spiTxBursts[3][i] = Easyspin_NOP; spiRxBursts[1][i] = 0; spiRxBursts[2][i] = 0; spiRxBursts[3][i] = 0; } switch (param) { case Easyspin_ABS_POS: ; case Easyspin_MARK: spiTxBursts[0][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); maxArgumentNbBytes = 3; break; case Easyspin_EL_POS: ; case Easyspin_CONFIG: ; case Easyspin_STATUS: spiTxBursts[1][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); maxArgumentNbBytes = 2; break; default: spiTxBursts[2][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); maxArgumentNbBytes = 1; } /* Disable interruption before checking */ /* pre-emption by ISR and SPI transfers*/ flag.disable_irq(); itDisable = true; } while (spiPreemtionByIsr); // check pre-emption by ISR for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; i < Easyspin_CMD_ARG_MAX_NB_BYTES; i++) { WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); } spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)| (spiRxBursts[2][spiIndex] << 8) | (spiRxBursts[3][spiIndex]); /* re-enable interrupts after SPI transfers*/ flag.enable_irq(); return (spiRxData); } /******************************************************//** * @brief Issues the GetStatus command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @retval Status Register value * @note Once the GetStatus command is performed, the flags of the status register * are reset. This is not the case when the status register is read with the * GetParam command (via the functions ReadStatusRegister or CmdGetParam). **********************************************************/ uint16_t Easyspin::CmdGetStatus(uint8_t shieldId) { uint32_t i; uint16_t status; uint8_t spiIndex = numberOfShields - shieldId - 1; bool itDisable = false; do { spiPreemtionByIsr = false; if (itDisable) { /* re-enable interrupts if disable in previous iteration */ flag.enable_irq(); itDisable = false; } for (i = 0; i < numberOfShields; i++) { spiTxBursts[0][i] = Easyspin_NOP; spiTxBursts[1][i] = Easyspin_NOP; spiTxBursts[2][i] = Easyspin_NOP; spiRxBursts[1][i] = 0; spiRxBursts[2][i] = 0; } spiTxBursts[0][spiIndex] = Easyspin_GET_STATUS; /* Disable interruption before checking */ /* pre-emption by ISR and SPI transfers*/ flag.disable_irq(); itDisable = true; } while (spiPreemtionByIsr); // check pre-emption by ISR for (i = 0; i < Easyspin_CMD_ARG_NB_BYTES_GET_STATUS + Easyspin_RSP_NB_BYTES_GET_STATUS; i++) { WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); } status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]); /* re-enable interrupts after SPI transfers*/ flag.enable_irq(); return (status); } /******************************************************//** * @brief Issues the Nop command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::CmdNop(uint8_t shieldId) { SendCommand(shieldId, Easyspin_NOP); } /******************************************************//** * @brief Issues the SetParam command to the Easyspin of the specified shield * @param[in] shieldId (from 0 to 2) * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) * @param[in] value Value to set in the register * @retval None **********************************************************/ void Easyspin::CmdSetParam(uint8_t shieldId, Easyspin_Registers_t param, uint32_t value) { uint32_t i; uint8_t maxArgumentNbBytes = 0; uint8_t spiIndex = numberOfShields - shieldId - 1; bool itDisable = false; do { spiPreemtionByIsr = false; if (itDisable) { /* re-enable interrupts if disable in previous iteration */ flag.enable_irq();//interrupts(); itDisable = false; } for (i = 0; i < numberOfShields; i++) { spiTxBursts[0][i] = Easyspin_NOP; spiTxBursts[1][i] = Easyspin_NOP; spiTxBursts[2][i] = Easyspin_NOP; spiTxBursts[3][i] = Easyspin_NOP; } switch (param) { case Easyspin_ABS_POS: ; case Easyspin_MARK: spiTxBursts[0][spiIndex] = param; spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16); spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); maxArgumentNbBytes = 3; break; case Easyspin_EL_POS: ; case Easyspin_CONFIG: spiTxBursts[1][spiIndex] = param; spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); maxArgumentNbBytes = 2; break; default: spiTxBursts[2][spiIndex] = param; maxArgumentNbBytes = 1; } spiTxBursts[3][spiIndex] = (uint8_t)(value); /* Disable interruption before checking */ /* pre-emption by ISR and SPI transfers*/ flag.disable_irq(); itDisable = true; } while (spiPreemtionByIsr); // check pre-emption by ISR /* SPI transfer */ for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; i < Easyspin_CMD_ARG_MAX_NB_BYTES; i++) { WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]); } /* re-enable interrupts after SPI transfers*/ flag.enable_irq(); } /******************************************************//** * @brief Reads the Status Register value * @param[in] shieldId (from 0 to 2) * @retval Status register valued * @note The status register flags are not cleared * at the difference with CmdGetStatus() **********************************************************/ uint16_t Easyspin::ReadStatusRegister(uint8_t shieldId) { return (CmdGetParam(shieldId,Easyspin_STATUS)); } /******************************************************//** * @brief Releases the Easyspin reset (pin set to High) of all shields * @param None * @retval None **********************************************************/ void Easyspin::ReleaseReset(void) { reset = 1; } /******************************************************//** * @brief Resets the Easyspin (reset pin set to low) of all shields * @param None * @retval None **********************************************************/ void Easyspin::Reset(void) { reset = 0; } /******************************************************//** * @brief Set the stepping mode * @param[in] shieldId (from 0 to 2) * @param[in] stepMod from full step to 1/16 microstep as specified in enum Easyspin_STEP_SEL_t * @retval None **********************************************************/ void Easyspin::SelectStepMode(uint8_t shieldId, Easyspin_STEP_SEL_t stepMod) { uint8_t stepModeRegister; /* Eventually deactivate motor */ if (shieldPrm[shieldId].motionState != INACTIVE) { HardStop(shieldId); } /* Read Step mode register and clear STEP_SEL field */ stepModeRegister = (uint8_t)(0xF8 & CmdGetParam(shieldId,Easyspin_STEP_MODE)) ; /* Apply new step mode */ CmdSetParam(shieldId, Easyspin_STEP_MODE, stepModeRegister | (uint8_t)stepMod); /* Reset abs pos register */ SetHome(shieldId); } /******************************************************//** * @brief Specifies the direction * @param[in] shieldId (from 0 to 2) * @param[in] dir FORWARD or BACKWARD * @note The direction change is only applied if the shield * is in INACTIVE state * @retval None **********************************************************/ void Easyspin::SetDirection(uint8_t shieldId, dir_t dir) { if (shieldPrm[shieldId].motionState == INACTIVE) { shieldPrm[shieldId].direction = dir; switch (shieldId) { case 2: dir3 = dir; break; case 1: dir2 = dir; break; case 0: dir1 = dir; break; default: ; } } } /******************************************************//** * @brief Gets the pointer to the Easyspin instance * @param None * @retval Pointer to the instance of Easyspin **********************************************************/ class Easyspin* Easyspin::GetInstancePtr(void) { return (class Easyspin*)instancePtr; } /******************************************************//** * @brief Handles the shield state machine at each ste * @param[in] shieldId (from 0 to 2) * @retval None * @note Must only be called by the timer ISR **********************************************************/ void Easyspin::StepClockHandler(uint8_t shieldId) { /* Set isr flag */ isrFlag = true; /* Incrementation of the relative position */ shieldPrm[shieldId].relativePos++; /* Periodically check that estimated position is correct */ if ((shieldPrm[shieldId].commandExecuted != RUN_CMD) && ((shieldPrm[shieldId].relativePos % 10) == 00)) { uint32_t AbsPos= ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); /* Correct estimated position if needed */ if (AbsPos != 0) { if ((shieldPrm[shieldId].direction == FORWARD) && (AbsPos != shieldPrm[shieldId].currentPosition + shieldPrm[shieldId].relativePos)) { #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "F EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); Serial.println(EasyspinStrOut); #endif shieldPrm[shieldId].relativePos = AbsPos - shieldPrm[shieldId].currentPosition; } else if ((shieldPrm[shieldId].direction == BACKWARD) && (AbsPos != shieldPrm[shieldId].currentPosition - shieldPrm[shieldId].relativePos)) { #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "B EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); Serial.println(EasyspinStrOut); #endif shieldPrm[shieldId].relativePos = shieldPrm[shieldId].currentPosition - AbsPos; } } } switch (shieldPrm[shieldId].motionState) { case ACCELERATING: { if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].startDecPos))) { shieldPrm[shieldId].motionState = DECELERATING; shieldPrm[shieldId].accu = 0; #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Dec: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); Serial.println(EasyspinStrOut); #endif } else if ((shieldPrm[shieldId].speed >= shieldPrm[shieldId].maxSpeed)|| ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].endAccPos))) { shieldPrm[shieldId].motionState = STEADY; #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); Serial.println(EasyspinStrOut); #endif } else { bool speedUpdated = false; /* Go on accelerating */ shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].acceleration << 16) / shieldPrm[shieldId].speed; while (shieldPrm[shieldId].accu >= (0X10000L)) { shieldPrm[shieldId].accu -= (0X10000L); shieldPrm[shieldId].speed +=1; speedUpdated = true; } if (speedUpdated) { ApplySpeed(shieldId, shieldPrm[shieldId].speed); } } break; } case STEADY: { if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& (shieldPrm[shieldId].relativePos >= (shieldPrm[shieldId].startDecPos))) || ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& (shieldPrm[shieldId].speed > shieldPrm[shieldId].maxSpeed))) { shieldPrm[shieldId].motionState = DECELERATING; shieldPrm[shieldId].accu = 0; } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& (shieldPrm[shieldId].speed < shieldPrm[shieldId].maxSpeed)) { shieldPrm[shieldId].motionState = ACCELERATING; shieldPrm[shieldId].accu = 0; } break; } case DECELERATING: { if (((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)&&(shieldPrm[shieldId].speed <= shieldPrm[shieldId].minSpeed))|| ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& (shieldPrm[shieldId].relativePos >= shieldPrm[shieldId].stepsToTake))) { /* Motion process complete */ HardStop(shieldId); #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Stop: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos ); Serial.println(EasyspinStrOut); #endif } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& (shieldPrm[shieldId].speed <= shieldPrm[shieldId].maxSpeed)) { shieldPrm[shieldId].motionState = STEADY; #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); Serial.println(EasyspinStrOut); #endif } else { /* Go on decelerating */ if (shieldPrm[shieldId].speed > shieldPrm[shieldId].minSpeed) { bool speedUpdated = false; shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].deceleration << 16) / shieldPrm[shieldId].speed; while (shieldPrm[shieldId].accu >= (0X10000L)) { shieldPrm[shieldId].accu -= (0X10000L); shieldPrm[shieldId].speed -=1; speedUpdated = true; } if (speedUpdated) { ApplySpeed(shieldId, shieldPrm[shieldId].speed); } } } break; } default: { break; } } /* Set isr flag */ isrFlag = false; } /******************************************************//** * @brief Updates the current speed of the shield * @param[in] shieldId (from 0 to 2) * @param[in] newSpeed in pps * @retval None **********************************************************/ void Easyspin::ApplySpeed(uint8_t shieldId, uint16_t newSpeed) { if (newSpeed < Easyspin_MIN_PWM_FREQ) { newSpeed = Easyspin_MIN_PWM_FREQ; } if (newSpeed > Easyspin_MAX_PWM_FREQ) { newSpeed = Easyspin_MAX_PWM_FREQ; } shieldPrm[shieldId].speed = newSpeed; switch (shieldId) { case 0: Pwm1SetFreq(newSpeed); break; case 1: Pwm2SetFreq(newSpeed); break; case 2: Pwm3SetFreq(newSpeed); break; default: break; //ignore error } } /******************************************************//** * @brief Computes the speed profile according to the number of steps to move * @param[in] shieldId (from 0 to 2) * @param[in] nbSteps number of steps to perform * @retval None * @note Using the acceleration and deceleration of the shield, * this function determines the duration in steps of the acceleration, * steady and deceleration phases. * If the total number of steps to perform is big enough, a trapezoidal move * is performed (i.e. there is a steady phase where the motor runs at the maximum * speed. * Else, a triangular move is performed (no steady phase: the maximum speed is never * reached. **********************************************************/ void Easyspin::ComputeSpeedProfile(uint8_t shieldId, uint32_t nbSteps) { uint32_t reqAccSteps; uint32_t reqDecSteps; /* compute the number of steps to get the targeted speed */ reqAccSteps = (shieldPrm[shieldId].maxSpeed - shieldPrm[shieldId].minSpeed); reqAccSteps *= (shieldPrm[shieldId].maxSpeed + shieldPrm[shieldId].minSpeed); reqDecSteps = reqAccSteps; reqAccSteps /= (uint32_t)shieldPrm[shieldId].acceleration; reqAccSteps /= 2; /* compute the number of steps to stop */ reqDecSteps /= (uint32_t)shieldPrm[shieldId].deceleration; reqDecSteps /= 2; if(( reqAccSteps + reqDecSteps ) > nbSteps) { /* Triangular move */ /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ reqDecSteps = ((uint32_t) shieldPrm[shieldId].deceleration * nbSteps) / (shieldPrm[shieldId].acceleration + shieldPrm[shieldId].deceleration); if (reqDecSteps > 1) { reqAccSteps = reqDecSteps - 1; if(reqAccSteps == 0) { reqAccSteps = 1; } } else { reqAccSteps = 0; } shieldPrm[shieldId].endAccPos = reqAccSteps; shieldPrm[shieldId].startDecPos = reqDecSteps; } else { /* Trapezoidal move */ /* accelerating phase to endAccPos */ /* steady phase from endAccPos to startDecPos */ /* decelerating from startDecPos to stepsToTake*/ shieldPrm[shieldId].endAccPos = reqAccSteps; shieldPrm[shieldId].startDecPos = nbSteps - reqDecSteps - 1; } } /******************************************************//** * @brief Converts the ABS_POSITION register value to a 32b signed integer * @param[in] abs_position_reg value of the ABS_POSITION register * @retval operation_result 32b signed integer corresponding to the absolute position **********************************************************/ int32_t Easyspin::ConvertPosition(uint32_t abs_position_reg) { int32_t operation_result; if (abs_position_reg & Easyspin_ABS_POS_SIGN_BIT_MASK) { /* Negative register value */ abs_position_reg = ~abs_position_reg; abs_position_reg += 1; operation_result = (int32_t) (abs_position_reg & Easyspin_ABS_POS_VALUE_MASK); operation_result = -operation_result; } else { operation_result = (int32_t) abs_position_reg; } return operation_result; } /******************************************************//** * @brief Handlers of the flag interrupt which calls the user callback (if defined) * @param None * @retval None **********************************************************/ void Easyspin::FlagInterruptHandler(void) { if (flagInterruptCallback != NULL) { /* Set isr flag */ isrFlag = true; flagInterruptCallback(); /* Reset isr flag */ isrFlag = false; } } /******************************************************//** * @brief Sends a command without arguments to the Easyspin via the SPI * @param[in] shieldId (from 0 to 2) * @param[in] param Command to send * @retval None **********************************************************/ void Easyspin::SendCommand(uint8_t shieldId, uint8_t param) { uint8_t spiIndex = numberOfShields - shieldId - 1; bool itDisable = false; do { spiPreemtionByIsr = false; if (itDisable) { /* re-enable interrupts if disable in previous iteration */ flag.enable_irq(); itDisable = false; } for (uint32_t i = 0; i < numberOfShields; i++) { spiTxBursts[3][i] = Easyspin_NOP; } spiTxBursts[3][spiIndex] = param; /* Disable interruption before checking */ /* pre-emption by ISR and SPI transfers*/ flag.disable_irq(); itDisable = true; } while (spiPreemtionByIsr); // check pre-emption by ISR WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); /* re-enable interrupts after SPI transfers*/ flag.enable_irq(); } /******************************************************//** * @brief Sets the registers of the Easyspin to their predefined values * from Easyspin_target_config.h * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::SetRegisterToPredefinedValues(uint8_t shieldId) { CmdSetParam(shieldId, Easyspin_ABS_POS, 0); CmdSetParam(shieldId, Easyspin_EL_POS, 0); CmdSetParam(shieldId, Easyspin_MARK, 0); switch (shieldId) { case 0: CmdSetParam(shieldId, Easyspin_TVAL, Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_0)); CmdSetParam(shieldId, Easyspin_T_FAST, (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_0 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_0); CmdSetParam(shieldId, Easyspin_TON_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_0)); CmdSetParam(shieldId, Easyspin_TOFF_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_0)); CmdSetParam(shieldId, Easyspin_OCD_TH, Easyspin_CONF_PARAM_OCD_TH_SHIELD_0); CmdSetParam(shieldId, Easyspin_STEP_MODE, (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_0 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_0); CmdSetParam(shieldId, Easyspin_ALARM_EN, Easyspin_CONF_PARAM_ALARM_EN_SHIELD_0); CmdSetParam(shieldId, Easyspin_CONFIG, (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_0 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_0 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_0 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_0 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_0); break; case 1: CmdSetParam(shieldId, Easyspin_TVAL, Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_1)); CmdSetParam(shieldId, Easyspin_T_FAST, (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_1 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_1); CmdSetParam(shieldId, Easyspin_TON_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_1)); CmdSetParam(shieldId, Easyspin_TOFF_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_1)); CmdSetParam(shieldId, Easyspin_OCD_TH, Easyspin_CONF_PARAM_OCD_TH_SHIELD_1); CmdSetParam(shieldId, Easyspin_STEP_MODE, (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_1 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_1); CmdSetParam(shieldId, Easyspin_ALARM_EN, Easyspin_CONF_PARAM_ALARM_EN_SHIELD_1); CmdSetParam(shieldId, Easyspin_CONFIG, (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_1 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_1 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_1 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_1 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_1); break; case 2: CmdSetParam(shieldId, Easyspin_TVAL, Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_2)); CmdSetParam(shieldId, Easyspin_T_FAST, (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_2 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_2); CmdSetParam(shieldId, Easyspin_TON_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_2)); CmdSetParam(shieldId, Easyspin_TOFF_MIN, Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_2)); CmdSetParam(shieldId, Easyspin_OCD_TH, Easyspin_CONF_PARAM_OCD_TH_SHIELD_2); CmdSetParam(shieldId, Easyspin_STEP_MODE, (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_2 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_2); CmdSetParam(shieldId, Easyspin_ALARM_EN, Easyspin_CONF_PARAM_ALARM_EN_SHIELD_2); CmdSetParam(shieldId, Easyspin_CONFIG, (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_2 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_2 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_2 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_2 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_2); break; default: ; } } /******************************************************//** * @brief Sets the registers of the Easyspin to their predefined values * from Easyspin_target_config.h * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) { CS = 0; for (uint32_t i = 0; i < numberOfShields; i++) { *pReceivedByte = spi.write(*pByteToTransmit); pByteToTransmit++; pReceivedByte++; } CS = 1; if (isrFlag) { spiPreemtionByIsr = true; } } /******************************************************//** * @brief Initialises the PWM uses by the specified shield * @param[in] shieldId (from 0 to 2) * @retval None * @note Shield 0 uses PW1 based on timer 1 * Shield 1 uses PWM 2 based on timer 2 * Shield 2 uses PWM3 based timer 0 **********************************************************/ void Easyspin::PwmInit(uint8_t shieldId) { switch (shieldId) { case 0: /* PWM1 uses timer 1 */ /* Initialise timer by setting waveform generation mode to PWM phase and Frequency correct: mode = 8 (WGM10 = 0, WGM11 = 0, WGM12 = 0, WGM13 = 1) */ /* Stop timer1 by clearing CSS bits and set WGM13 and WGM12 */ //TCCR1B = 0x10; /* Set WGM10 and WGM11 */ //TCCR1A = 0x00; /* Disable Timer1 interrupt */ //TIMSK1 = 0; //pwm1.period_us(400); //pwm1.pulsewidth_us(200); break; case 1: /* PWM2 uses timer 2 */ /* Initialise timer by setting waveform generation mode to PWM phase correct: mode = 5 (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ /* Stop timer2 by clearing CSS bits and set WGM22 */ //TCCR2B = 0x08; /* Set WGM20 and WGM21 */ //TCCR2A = 0x01; /* Disable Timer2 interrupt */ //TIMSK2 = 0; //pwm2.period_us(500); //pwm2.pulsewidth_us(100); break; case 2: /* PWM3 uses timer 0 */ /* !!!!! Caution: Calling this configuration will break */ /* all default Arduino's timing functions as delay(),millis()... */ /* Initialise timer by setting waveform generation mode to PWM phase correct: mode = 5 (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ /* Stop timer0 by clearing CSS bits and set WGM22 */ //TCCR0B = 0x08; /* Set WGM00 and WGM01 */ //TCCR0A = 0x01; /* Disable Timer0 interrupt */ //TIMSK0 = 0; //pwm3.period_ms(10); //pwm3.pulsewidth_ms(1); break; default: break;//ignore error } } /******************************************************//** * @brief Ticker1 * @param[in] * @retval **********************************************************/ void Easyspin::tick1() { class Easyspin* instancePtr = Easyspin::GetInstancePtr(); pwm1 = !pwm1; if (instancePtr != NULL) { if (instancePtr->GetShieldState(0) != INACTIVE) { instancePtr->StepClockHandler(0); } } } /******************************************************//** * @brief Ticker2 * @param[in] * @retval **********************************************************/ void Easyspin::tick2() { class Easyspin* instancePtr = Easyspin::GetInstancePtr(); pwm2 = !pwm2; if (instancePtr != NULL) { if (instancePtr->GetShieldState(1) != INACTIVE) { instancePtr->StepClockHandler(1); } } } /******************************************************//** * @brief Sets the frequency of PWM1 used by shield 0 * @param[in] newFreq in Hz * @retval None * @note The frequency is directly the current speed of the shield **********************************************************/ void Easyspin::Pwm1SetFreq(uint16_t newFreq) { uint16_t us; us = (1000000 / (newFreq * 2)); ticker1.attach_us(this, &Easyspin::tick1, us); } /******************************************************//** * @brief Sets the frequency of PWM2 used by shield 1 * @param[in] newFreq in Hz * @retval None * @note The frequency is directly the current speed of the shield **********************************************************/ void Easyspin::Pwm2SetFreq(uint16_t newFreq) { uint16_t us; us = (1000000 / (newFreq * 2)); ticker2.attach_us(this, &Easyspin::tick2, us); } /******************************************************//** * @brief Sets the frequency of PWM3 used by shield 2 * @param[in] newFreq in Hz * @retval None * @note The frequency is directly the current speed of the shield **********************************************************/ void Easyspin::Pwm3SetFreq(uint16_t newFreq) { } /******************************************************//** * @brief Stops the PWM uses by the specified shield * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::PwmStop(uint8_t shieldId) { switch (shieldId) { case 0: ticker1.detach(); break; case 1: ticker2.detach(); break; case 2: ticker3.detach(); break; default: break;//ignore error } } /******************************************************//** * @brief Sets the parameters of the shield to predefined values * from Easyspin_target_config.h * @param None * @retval None **********************************************************/ void Easyspin::SetShieldParamsToPredefinedValues(void) { shieldPrm[0].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_0; shieldPrm[0].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_0; shieldPrm[0].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_0; shieldPrm[0].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_0; shieldPrm[1].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_1; shieldPrm[1].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_1; shieldPrm[1].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_1; shieldPrm[1].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_1; shieldPrm[2].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_2; shieldPrm[2].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_2; shieldPrm[2].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_2; shieldPrm[2].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_2; for (uint8_t i = 0; i < numberOfShields; i++) { SetRegisterToPredefinedValues(i); } } /******************************************************//** * @brief Initialises the bridge parameters to start the movement * and enable the power bridge * @param[in] shieldId (from 0 to 2) * @retval None **********************************************************/ void Easyspin::StartMovement(uint8_t shieldId) { /* Enable Easyspin powerstage */ CmdEnable(shieldId); if (shieldPrm[shieldId].endAccPos != 0) { shieldPrm[shieldId].motionState = ACCELERATING;; } else { shieldPrm[shieldId].motionState = DECELERATING; } shieldPrm[shieldId].accu = 0; shieldPrm[shieldId].relativePos = 0; ApplySpeed(shieldId, shieldPrm[shieldId].minSpeed); #ifdef _DEBUG_Easyspin snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Stop->Acc: speed: %u relPos: %ld\n", shieldPrm[shieldId].minSpeed, shieldPrm[shieldId].relativePos) ; Serial.println(EasyspinStrOut); #endif } /******************************************************//** * @brief Converts mA in compatible values for TVAL register * @param[in] Tval * @retval TVAL values **********************************************************/ inline uint8_t Easyspin::Tval_Current_to_Par(double Tval) { return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); } /******************************************************//** * @brief Convert time in us in compatible values * for TON_MIN register * @param[in] Tmin * @retval TON_MIN values **********************************************************/ inline uint8_t Easyspin::Tmin_Time_to_Par(double Tmin) { return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); }