main.c

Atmel算法(pid)-步进电机驱动源码 HOWTO,不好用你找我,绝对ok!

  //Set output duty cycle to zero for now.
  BlockCommutationSetDuty(0);

  //Wait for next PWM cycle to ensure that all outputs are updated.
  TimersWaitForNextPWMCycle();

  fastFlags.driveWaveform = WAVEFORM_BLOCK_COMMUTATION;

  //Change PWM pins to output again to allow PWM control.
  EnablePWMOutputs();
}


/*! \brief Configures timers for braking and starts braking.
 *
 *  This function configures the timers for braking and starts braking. Please
 *  note that braking when turning can produce too much heat for the drivers to
 *  handle. Use with care!
 */
#if (TURN_MODE == TURN_MODE_BRAKE)
#pragma inline=forced
static void TimersSetModeBrake(void)
{
  //Set PWM pins to input (Hi-Z) while changing modes.
  DisablePWMOutputs();

  //Sets both outputs of all 3 timers in "clear on up-counting, set on down-counting" mode.
  TCCR0A = (1 << COM0A1) | (0 << COM0A0) | (1 << COM0B1) | (0 << COM0B0) | (1 << WGM00);
  TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << COM1B1) | (0 << COM1B0) | (1 << WGM11);
  TCCR2A = (1 << COM2A1) | (0 << COM2A0) | (1 << COM2B1) | (0 << COM2B0) | (1 << WGM20);

  // High side
  OCR0A = OCR1A = OCR2A = 0x00;

  // Low side
  OCR0B = OCR1B = OCR2B = 0xff;

  //Wait for next PWM cycle to ensure that all outputs are updated.
  TimersWaitForNextPWMCycle();

  fastFlags.driveWaveform = WAVEFORM_BRAKING;

  EnablePWMOutputs();
}
#endif


/*! \brief Waits for the start of the next PWM cycle.
 *
 *  Waits for the start of the next PWM cycle.
 *  Can be used to make sure that a shoot-through
 *  does not occur in the transition between two output waveform generation modes.
 */
#pragma inline=forced
static void TimersWaitForNextPWMCycle(void)
{
  //Clear Timer1 Capture event flag.
  TIFR1 = (1 << ICF1);

  //Wait for new Timer1 Capture event flag.
  while ( !(TIFR1 & (1 << ICF1)) )
  {

  }
}


/*! \brief Sets duty cycle for block commutation
 *
 *  This function sets duty cycle when block commutation is used.
 *  Never call this function when sine wave generation is used.
 *  \note The duty cycle is not in percent, but a value from
 *  0-255. Duty cycle in percent can be calculated as
 *  duty * 100 / 255.
 *
 *  \param duty New duty cycle. Range is 0-255.
 */
#pragma inline=forced
static void BlockCommutationSetDuty(const uint8_t duty)
{
  // Set all compare registers to new duty cycle value.
  OCR0A = OCR0B = OCR1AL = OCR1BL = OCR2A = OCR2B = duty;
}


/*! \brief Returns the desired direction.
 *
 *  This function returns the current desired direction.
 *  \note The direction input is not read at this time. A
 *  separate pin change interrupt is responsible for reading
 *  the input.
 *
 *  \retval DIRECTION_FORWARD Forward direction is requested.
 *  \retval DIRECTION_REVERSE Reverse direction is requested.
 */
#pragma inline=forced
static uint8_t GetDesiredDirection(void)
{
  return (uint8_t)fastFlags.desiredDirection;
}


/*! \brief Returns the actual direction of the motor.
 *
 *  This function returns the actual direction of the motor.
 *
 *  \note Two hall changes in the same direction is needed
 *  before this function returns the correct direction of
 *  rotation.
 *
 *  \return The desired direction.
 *
 *  \retval DIRECTION_FORWARD Motor is running forward
 *  \retval DIRECTION_REVERSE Motor is running in reverse
 *  \retval DIRECTION_UNKNOWN The current direction of the motor
 *  can not be determined (may be stopped or turning), or the
 *  hall sensors output incorrect values.
 */
#pragma inline=forced
static uint8_t GetActualDirection(void)
{
  return fastFlags.actualDirection;
}


/*! \brief Performs block commutation according to running direction and hall sensor input
 *
 *  This function performs a block commutation according to the
 *  specified running direction and hall sensor input.
 *
 *  \note Do not use this function while the timers are configured
 *  for sine wave driving.
 *
 *  \param direction Direction of rotation.
 *  \param hall Hall sensor value at the same form as returned by GetHall().
 */
#pragma inline=forced
static void BlockCommutate(const uint8_t direction, uint8_t hall)
{
  uint8_t const __flash *pattern;

  if (direction == DIRECTION_FORWARD)
  {
    pattern = blockCommutationTableForward;
  }
  else
  {
    pattern = blockCommutationTableReverse;
  }
  pattern += (hall * 2);

  DisablePWMOutputs();
  DDRB |= *pattern++;
  DDRD |= *pattern;
}


/*! \brief Reads the hall sensor inputs.
 *
 *  This function reads the hall sensor inputs and converts it to
 *  a number from 1 to 6 by combining the hall sensors as bits: H3|H2|H1.
 *
 *  \note It is possible to change the physical placement of hall sensor
 *  inputs, but the recommended configuration is the one used in this
 *  example, since it requires very little code to decode the hall values.
 *
 *  \return The decoded hall sensor value.
 *
 *  \retval 0 Illegal hall state. Possible hardware error.
 *  \retval 1-6 Legal hall sensor values.
 *  \retval 7 Illegal hall state. Possible hardware error.
 */
#pragma inline=forced
static uint8_t GetHall(void)
{
  uint8_t hall;

  hall = PINC & ((1 << H3_PIN) | (1 << H2_PIN) | (1 << H1_PIN));
  hall >>= H1_PIN;
  return hall;
}


/*! \brief Updates global desired direction flag.
 *
 *  Running this function triggers a reading of the direction
 *  input pin. The desiredDirection flag is set accordingly.
 */
#pragma inline=forced
static void DesiredDirectionUpdate(void)
{
  if ( (PIND & (1 << DIRECTION_COMMAND_PIN)) != 0 )
  {
    fastFlags.desiredDirection = DIRECTION_REVERSE;
  }
  else
  {
    fastFlags.desiredDirection = DIRECTION_FORWARD;
  }
}


/*! \brief Updates global actual direction flag based on the two latest hall values.
 *
 *  Calling this function with the last two hall sensor values as
 *  parameters triggers an update of the global actualDirection flag.
 *
 *  \param lastHall The last hall value.
 *  \param newHall The current hall value.
 */
#pragma inline=forced
static void ActualDirectionUpdate(uint8_t lastHall, const uint8_t newHall)
{
  //Make sure that lastHall is within bounds of table. If not, set to 0, which is
  //also an illegal hall value, but legal table index.
  if (lastHall > 6)
  {
    lastHall = 0;
  }
  if (expectedHallSequenceForward[lastHall] == newHall)
  {
    fastFlags.actualDirection = DIRECTION_FORWARD;
  }
  else if (expectedHallSequenceReverse[lastHall] == newHall)
  {
   fastFlags.actualDirection = DIRECTION_REVERSE;
  }
  else
  {
    PMSMflags_t tempFlags = fastFlags;
    tempFlags.actualDirection = DIRECTION_UNKNOWN;
    tempFlags.motorSynchronized = FALSE;
    fastFlags = tempFlags;
  }
}


/*! \brief Updates the reverse rotation signal output pin value.
 *
 *  Running this function compares the actual and desired direction
 *  flags to decide if the motor is running in the opposite direction
 *  of what is requested. The Reverse rotation pin will be set to high
 *  when the motor is running in the opposite direction and low when
 *  the motor is running in the correct direction.
 *
 *  \note This function does not update actual and desired direction
 *  flags. The result of this function will therefore represent the
 *  time at which these variables were updated.
 */
#pragma inline=forced
static void ReverseRotationSignalUpdate(void)
{
#if (REVERSE_ROTATION_SIGNAL_ENABLE)
  if (GetActualDirection() == GetDesiredDirection())
  {
    PORTD &= ~(1 << REV_ROTATION_PIN);
  }
  else
  {
    PORTD |= (1 << REV_ROTATION_PIN);
  }
#endif
}


/*! \brief Returns the high and low values with deadband for a given compare value.
 *
 * This function takes as argument a desired compare value and inserts a symmetric
 * deadband. The compare values for high and low side with deadband are returned
 * through the two supplied pointers.
 * The constant \ref DEAD_TIME_HALF is used as deadband, and the resulting deadtime
 * will be DEAD_TIME_HALF clock cycles times 2.
 *
 * \param compareValue desired compare value
 * \param compareHighPtr Pointer used to return high side compare value with dead band.
 * \param compareLowPtr  Pointer used to return low side compare value with dead band.
 */
#pragma inline=forced
static void InsertDeadband(const uint8_t compareValue, uint8_t * compareHighPtr, uint8_t * compareLowPtr)
{
  if (compareValue <= DEAD_TIME_HALF)
  {
    *compareHighPtr = 0x00;
    *compareLowPtr = compareValue;
  }
  else if (compareValue >= (0xff - DEAD_TIME_HALF))
  {
    *compareHighPtr = 0xff - (2 * DEAD_TIME_HALF);
    *compareLowPtr = 0xff;
  }
  else
  {
    *compareHighPtr = compareValue - DEAD_TIME_HALF;
    *compareLowPtr = compareValue + DEAD_TIME_HALF;
  }
}


/*! \brief Calculates the increment for sine table iteration.
 *
 *  This function calculates the increment to be used for sine
 *  table iteration, based on the number of 'ticks' between two
 *  consecutive hall changes. Since a divide is needed, which is
 *  not supported in hardware on the AVR, the divide is done through
 *  a lookup table for values below 256 (when the motor is running
 *  fastest).
 *
 *  \param ticks The number of ticks between two consecutive hall changes.
 *
 *  \return The increment in 8.8 format to be used for sine table iteration.
 */
#pragma inline = forced
static uint16_t SineTableIncrementCalculate(const uint16_t ticks)
{
  if (ticks < 256)
  {
    return divisionTable[(uint8_t)ticks];
  }
  return (uint16_t)(((SINE_TABLE_LENGTH / 6) << 8) / ticks);
}


/*! \brief Adjusts the sine table index according to the current increment.
 *
 *  This function increases the sine table index with the given increment
 *  The index is then adjusted to be within the table length.
 *
 *  \param increment The increment (in 8.8 format) added to the sine table index.
 */
#pragma inline=forced
static void AdjustSineTableIndex(const uint16_t increment)
{
  sineTableIndex += increment;

  // If the table index is out of bounds, wrap the index around the table end
  // to continue from the beginning. Also wrap the next sector start index.
  if ((sineTableIndex >> 8) >= SINE_TABLE_LENGTH)
  {
    sineTableIndex -= (SINE_TABLE_LENGTH << 8);
    sineTableNextSectorStart -= SINE_TABLE_LENGTH;
  }

  //Make copy of sineNextSectorStart to specify order of volatile access.
  uint8_t nextSectorStart = sineTableNextSectorStart;
  if ((sineTableIndex >> 8) > nextSectorStart)
  {
    sineTableIndex = (nextSectorStart << 8);
  }
}


/*! \brief Sets the lead angle.
 *
 *  This function sets the advance commutation angle to be used during sine
 *  wave operation. An increase of one equals 1.875 degrees.
 *
 *  \note There is no checking of the advanceCommutation input parameter, but this
 *  should not be set to a value above 40 to avoid overflow in the sine table
 *  index.
 *
 *  \param advanceCommutation The advance commutation (1 equals 1.875 degrees)
 */
#pragma inline=forced
static void SetAdvanceCommutation(const uint8_t advanceCommutation)
{
  advanceCommutationSteps = advanceCommutation;
}


/*! \brief Updates the tacho output pin signal
 *
 *  This function uses the current hall sensor value to determine the tacho
 *  output signal. The "algorithm" used is based on the fact that this signal
 *  should have a 0 value when the combined hall sensor inputs are a power of 2.
 *
 *  The expression (hall & (hall - 1) computes to 0 for all values of 'hall' that
 *  is a power of 2. This can be used to produce the correct output signal.
 *
 *  \param hall The current hall sensor value.
 */