File content as of revision 0:cba942f8172a:

/******************************************************//**
 * @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"

#ifdef _DEBUG_Easyspin
/// Log buffer
char EasyspinStrOut[DEBUG_BUFFER_SIZE];
#endif

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.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 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();
    WaitUs(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  Waits for the specify delay in milliseconds
 * @param[in] msDelay delay in milliseconds
 * @retval None
 * @note Should only be used for 3 shields configuration.
 * Else, prefer the standard Arduino function delay().
 **********************************************************/
void Easyspin::WaitMs(uint16_t msDelay)
{
  uint16_t i;
  for (i = 0; i < msDelay ; i++)
  {
    WaitUs(1000);
  }
}

/******************************************************//**
 * @brief  Waits for the specify delay in microseconds
 * @param[in] usDelay delay in microseconds
 * @retval None
 * @note Should be only used for 3 shields configuration.
 * Else, prefer the standard Arduino function delayMicroseconds().
 * Besides, this function is a copy of delayMicroseconds inside
 * the Easyspin library to avoid dependencies conflicts 
 * (a redefinition of ISR(TIMER0_OVF_vect)). 
 **********************************************************/
void Easyspin::WaitUs(uint16_t usDelay)
{
    // calling avrlib's delay_us() function with low values (e.g. 1 or
    // 2 microseconds) gives delays longer than desired.
    //delay_us(us);
#if F_CPU >= 20000000L
    // for the 20 MHz clock on rare Arduino boards

    // for a one-microsecond delay, simply wait 2 cycle and return. The overhead
    // of the function call yields a delay of exactly a one microsecond.
    __asm__ __volatile__ (
        "nop" "\n\t"
        "nop"); //just waiting 2 cycle
    if (--usDelay == 0)
        return;

    // the following loop takes a 1/5 of a microsecond (4 cycles)
    // per iteration, so execute it five times for each microsecond of
    // delay requested.
    usDelay = (usDelay<<2) + usDelay; // x5 us

    // account for the time taken in the preceeding commands.
    usDelay -= 2;

#elif F_CPU >= 16000000L
    // for the 16 MHz clock on most Arduino boards

    // for a one-microsecond delay, simply return.  the overhead
    // of the function call yields a delay of approximately 1 1/8 us.
    if (--usDelay == 0)
        return;

    // the following loop takes a quarter of a microsecond (4 cycles)
    // per iteration, so execute it four times for each microsecond of
    // delay requested.
    usDelay <<= 2;

    // account for the time taken in the preceeding commands.
    usDelay -= 2;
#else
    // for the 8 MHz internal clock on the ATmega168

    // for a one- or two-microsecond delay, simply return.  the overhead of
    // the function calls takes more than two microseconds.  can't just
    // subtract two, since us is unsigned; we'd overflow.
    if (--usDelay == 0)
        return;
    if (--usDelay == 0)
        return;

    // the following loop takes half of a microsecond (4 cycles)
    // per iteration, so execute it twice for each microsecond of
    // delay requested.
    usDelay <<= 1;
    
    // partially compensate for the time taken by the preceeding commands.
    // we can't subtract any more than this or we'd overflow w/ small delays.
    usDelay--;
#endif
}  
                  
/******************************************************//**
 * @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  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)
{
    pwm1.period_us(newFreq);
    pwm1.pulsewidth_us(newFreq/2);
}

/******************************************************//**
 * @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)
{
    pwm2.period_us(newFreq);
    pwm2.pulsewidth_us(newFreq/2);
}

/******************************************************//**
 * @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)
{
    pwm3.period_us(newFreq);
    pwm3.pulsewidth_us(newFreq/2);
}

/******************************************************//**
 * @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:
    pwm1.period_us(0);
    pwm1.pulsewidth_us(0);
      break;
    case  1:
    pwm2.period_us(0);
    pwm2.pulsewidth_us(0);
      break;
    case 2:
      pwm3.write(0.0f);
      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));
}



/******************************************************//**
 * @brief  Debug   function to get the amount of free ram
 * @param  None
 * @retval number of bytes of free ram
 **********************************************************/
#ifdef _DEBUG_Easyspin
uint16_t GetFreeRam (void)
{
  extern uint16_t __heap_start, *__brkval;
  uint16_t v;
  return (uint16_t) &v - (__brkval == 0 ? (uint16_t) &__heap_start : (uint16_t) __brkval);
}
#endif