main.c

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

#pragma inline=forced
static void TachoOutputUpdate(const uint8_t hall)
{
#if (TACHO_OUTPUT_ENABLED)
  if ( (hall & (uint8_t)(hall - 1)) != 0 )
  {
    PORTD &= ~(1 << TACHO_OUTPUT_PIN);
  }
  else
  {
    PORTD |= (1 << TACHO_OUTPUT_PIN);
  }
#endif
}


/*! \brief Enables PWM output pins
 *
 *  This function enables PWM outputs by setting the port direction for
 *  all PWM pins as output. The PWM configuration itself is not altered
 *  in any way by running this function.
 */
#pragma inline=forced
static void EnablePWMOutputs(void)
{
  DDRB |= PWM_PATTERN_PORTB;
  DDRD |= PWM_PATTERN_PORTD;
}


/*! \brief Disables PWM output pins
 *
 *  This function disables PWM outputs by setting the port dierction for
 *  all PWM pins as inputs, thus overriding the PWM. The PWM configuration
 *  itself is not altered in any way by running this function.
 */
#pragma inline=forced
static void DisablePWMOutputs(void)
{
  DDRB &= ~PWM_PATTERN_PORTB;
  DDRD &= ~PWM_PATTERN_PORTD;
}


/*! \brief Updates the 'tick' counter and checks for stopped motor.
 *
 *  This function should be run at every PWM timer overflow to ensure
 *  that all 'ticks' are counted. It increases the 'tick' counter
 *  until it reaches the maximum tick limit that corresponds to what
 *  is considered a stopped or stalled motor. In that case, the global
 *  motor stopped flag is set.
 */
#pragma inline=forced
static void CommutationTicksUpdate(void)
{
  if (commutationTicks < COMMUTATION_TICKS_STOPPED)
  {
    commutationTicks++;
  }
  else
  {
    fastFlags.motorStopped = TRUE;
    fastFlags.motorSynchronized = FALSE;
    if (fastFlags.driveWaveform != WAVEFORM_BLOCK_COMMUTATION)
    {
      TimersSetModeBlockCommutation();
      BlockCommutate(GetDesiredDirection(), GetHall());

#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
      PID_Reset_Integrator(&pidParameters);
#endif
    }
  }
}


/*! \brief Updates the global motor synchronized flag.
 *
 *  This function updates the global motor synchronized flag. The motor control
 *  is considered to be synchronized with the motor when it is not stopped and
 *  the driver has detected a direction of rotation that corresponds to the
 *  desired direction a predefined number of times.
 */
#pragma inline=forced
static void MotorSynchronizedUpdate(void)
{
  static uint8_t synchCount = 0;

  PMSMflags_t tempFlags;

  tempFlags = fastFlags;

  if ((tempFlags.desiredDirection == tempFlags.actualDirection) &&
      (tempFlags.motorStopped == FALSE) && (tempFlags.motorSynchronized == FALSE))
  {
    synchCount++;
    if (synchCount >= SYNCHRONIZATION_COUNT)
    {
      fastFlags.motorSynchronized = TRUE;
    }
  }
  else
  {
    fastFlags.motorSynchronized = FALSE;
    synchCount = 0;
  }
}


/*! \brief Returns the motor synchronized flag.
 *
 *  This function returns the motor synchronized flag.
 *
 *  \return The motor synchronized flag.
 *
 *  \retval TRUE  Motor control is synchronized with motor.
 *  \retval FALSE Motor control is not yet synchronized with motor.
 */
#pragma inline=forced
static uint8_t IsMotorSynchronized(void)
{
  return (uint8_t)(fastFlags.motorSynchronized);
}


/*! \brief Emergency shut-off interrupt service routine.
 *
 *  This ISR is triggered if the emergency shutdown input changes state.
 *
 *  \note In the current implementation, the motor driver outputs are
 *  simply disabled and the program goes into an eternal loop on any
 *  change at this input. Custom code should be made here that handles
 *  such situation in a more intelligent way, adapted to the motor and
 *  driver stage.
 *
 *  \todo Change how emergency shut-off is handled.
 */
#pragma vector=PCINT0_vect
__interrupt void EmergencyInterruptISR(void)
{
  DisablePWMOutputs();
  for (;;)
  {

  }
}


/*! \brief Hall sensor change interrupt service routine.
 *
 *  This interrupt service routine is called every time any of the hall
 *  sensors change. The sine table index is updated to reflect the
 *  position indicated by the hall sensors.
 *
 *  A new increment is calculated, based on the time since last hall
 *  sensor change.
 *
 *  The Tacho output signal is updated.
 *
 *  The actual direction is updated.
 *
 *  The reverse rotation output signal is updated.
 *
 *  The motor stopped flag is set to false, since the motor is obviously not
 *  stopped when there is a hall change.
 */
#pragma vector=PCINT1_vect
__interrupt void HallChangeISR(void)
{
  static uint8_t lastHall = 0xff;
  uint8_t hall;

  hall = GetHall();

  MotorSynchronizedUpdate();
  uint8_t synch = IsMotorSynchronized();
  if ((fastFlags.driveWaveform != WAVEFORM_SINUSOIDAL) && (synch))
  {
    TimersSetModeSinusoidal();
  }

  //If sinusoidal driving is used, synchronize sine wave generation to the
  //current hall sensor value. Advance commutation is also
  //added in the process.
  if (fastFlags.driveWaveform == WAVEFORM_SINUSOIDAL)
  {
    uint16_t tempIndex;
    if (GetDesiredDirection() == DIRECTION_FORWARD)
    {
      tempIndex = (CSOffsetsForward[hall] + advanceCommutationSteps) << 8;
    }
    else
    {
      tempIndex = (CSOffsetsReverse[hall] + advanceCommutationSteps) << 8;
    }
    sineTableIndex = tempIndex;

    //Adjust next sector start index. It might be set to a value larger than
    //SINE_TABLE_LENGTH at this point. This is adjusted in AdjustSineTableIndex
    //and should not be done here, as it will cause problems when advance
    //commutation is used.
    sineTableNextSectorStart = (tempIndex >> 8) + TABLE_ELEMENTS_PER_COMMUTATION_SECTOR;
  }

  //If block commutation is used. Commutate according to hall signal.
  else if (fastFlags.driveWaveform == WAVEFORM_BLOCK_COMMUTATION)
  {
    BlockCommutate(GetDesiredDirection(), hall);
  }

  //Update internal and external signals that depend on hall sensor value.
  TachoOutputUpdate(hall);
  ActualDirectionUpdate(lastHall, hall);
  ReverseRotationSignalUpdate();

  lastHall = hall;

  //Calculate new increment for sine wave generation and reset commutation
  //timer.
  sineTableIncrement = SineTableIncrementCalculate(commutationTicks);
  commutationTicks = 0;

  //Since the hall sensors are changing, the motor can not be stopped.
  fastFlags.motorStopped = FALSE;
}


/*! \brief Direction input change interrupt service routine.
 *
 *  This ISR is called every time the direction input pin changes
 *  state. The desired direction flag is updated accordingly. The
 *  motor control then goes into a state where it needs a stopped
 *  motor or a synchronization before any driving of the motor is
 *  performed.
 */
#pragma vector=PCINT2_vect
__interrupt void DirectionInputChangeISR(void)
{
  //Update desired direction flag.
  DesiredDirectionUpdate();

#if (TURN_MODE == TURN_MODE_COAST)
  //Disable driver signals to let motor coast. The motor will automatically
  //start once it is synchronized or stopped.
  DisablePWMOutputs();
  fastFlags.motorSynchronized = FALSE;
  fastFlags.motorStopped = FALSE;
  fastFlags.driveWaveform = WAVEFORM_UNDEFINED;
#endif

#if (TURN_MODE == TURN_MODE_BRAKE)
  //Set motor in brake mode. The motor will automatically start once it is
  //synchronized or stopped.
  fastFlags.motorSynchronized = FALSE;
  fastFlags.motorStopped = FALSE;
  TimersSetModeBrake(); // Automatically sets driveWaveform.
#endif
}


/*! \brief Timer1 Capture Evente interrupt service routine.
 *
 * This interrupt service routine is run everytime the up-down counting timer0
 * reaches TOP (0xff). New sinusoidal output values are calculated and the
 * timers are updated to reflect the new values.
 */
#pragma vector=TIMER1_CAPT_vect
__interrupt void Timer1CaptureISR(void)
{
  if (fastFlags.driveWaveform == WAVEFORM_SINUSOIDAL)
  {
    uint8_t tempU, tempV, tempW;
    {
      uint8_t const __flash * sineTablePtr = sineTable;

      AdjustSineTableIndex(sineTableIncrement);

      //Add sine table offset to pointer. Must be multiplied by 3, since one
      //value for each phase is stored in the table.
      sineTablePtr += (sineTableIndex >> 8) * 3;

      tempU = *sineTablePtr++;
      if (GetDesiredDirection() == DIRECTION_FORWARD)
      {
        tempV = *sineTablePtr++;
        tempW = *sineTablePtr;
      }
      else
      {
        tempW = *sineTablePtr++;
        tempV = *sineTablePtr;
      }
    }

    //Scale sine modulation values to the current amplitude.
    tempU = ((uint16_t)(amplitude * tempU) >> 8);
    tempV = ((uint16_t)(amplitude * tempV) >> 8);
    tempW = ((uint16_t)(amplitude * tempW) >> 8);

    {
      uint8_t compareHigh, compareLow;


      InsertDeadband(tempU, &compareHigh, &compareLow);
      OCR0A = compareHigh;
      OCR0B = compareLow;

      InsertDeadband(tempV, &compareHigh, &compareLow);
      OCR1AL = compareHigh;
      OCR1BL = compareLow;

      InsertDeadband(tempW, &compareHigh, &compareLow);
      OCR2A = compareHigh;
      OCR2B = compareLow;
    }
  }
  else if (fastFlags.driveWaveform == WAVEFORM_BLOCK_COMMUTATION)
  {
    uint16_t blockCommutationDuty = amplitude * BLOCK_COMMUTATION_DUTY_MULTIPLIER;

    if (blockCommutationDuty > 255)
    {
      blockCommutationDuty = 255;
    }

    BlockCommutationSetDuty((uint8_t)blockCommutationDuty);
  }

  CommutationTicksUpdate();

  {
    //Run the speed regulation loop with constant intervals.
    static uint8_t speedRegTicks = 0;
    speedRegTicks++;
    if (speedRegTicks >= SPEED_CONTROLLER_TIME_BASE)
    {
      SpeedControllerRun = TRUE;
      speedRegTicks -= SPEED_CONTROLLER_TIME_BASE;
    }
  }
}


/*! \brief AD conversion complete interrupt service routine
 *
 *  This interrupt service routine is automatically executed every time
 *  an AD conversion is finished and the converted result is available
 *  in the ADC data register.
 *
 *  The switch/case construct makes sure the converted value is stored in the
 *  variable corresponding to the selected channel and changes the channel for
 *  the next ADC measurement.
 *
 *  More ADC measurements can be added to the cycle by extending the switch/
 *  case construct.
 *
 *  Only the 8 most significant bits of the ADC result are used.
 */
#pragma vector=ADC_vect
__interrupt void ADCCompleteISR(void)
{
  switch (ADMUX)
  {
  case ADMUX_SPEED_REF:
    speedInput = ADCH;
    ADMUX = ADMUX_CURRENT;
    break;
  case ADMUX_CURRENT:
    current = ADCH;
    ADMUX = ADMUX_SPEED_REF;
    break;
  default:
    //This is probably an error and should be handled.
    break;
  }

  //Clear Timer/counter0 overflow flag.
  TIFR0 = (1 << TOV0);
}