main.c

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

/* This file has been prepared for Doxygen automatic documentation generation.*/
/*! \file *********************************************************************
 *
 * \brief
 *      Motor control implementation.
 *
 *      This file contains the full implementation of the motor control,
 *      except the PID-controller.
 *
 * \par Application note:
 *      AVR447: Sinusoidal driving of three-phase permanent motor using
 *      ATmega48/88/168
 *
 * \par Documentation
 *      For comprehensive code documentation, supported compilers, compiler
 *      settings and supported devices see readme.html
 *
 * \author
 *      Atmel Corporation: http://www.atmel.com \n
 *      Support email: [e-mail]
 *
 * $Name: RELEASE_1_0 $
 * $Revision: 1.8 $
 * $RCSfile: main.c,v $
 * $Date: 2006/03/27 07:20:51 $  \n
 ******************************************************************************/

#include <ioavr.h>
#include <inavr.h>
#include "stdint.h"
#include "PMSM.h"
#include "PMSMtables.h"

#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
#include "pid.h"
#endif

/*! \brief Motor control flags placed in I/O space for fast access.
 *
 *  This variable contains all the flags used for motor control.
 *  It is placed in GPIOR0 register, which allows usage of several
 *  fast bit manipulation/branch instructions.
 */
__io volatile PMSMflags_t fastFlags                               @0x1e;


/*! \brief Increment used for sine table iteration.
 *
 *  This variable holds the increment used for sine table iteration.
 *  It is stored in an 8.8 fixed point format.
 */
__no_init __regvar volatile uint16_t sineTableIncrement           @14;


/*! \brief Index into sine table
 *
 *  This variable is used as an index into the sine table. It is
 *  stored in a 8.8 fixed point format, so it can be directly incremented by
 *  sineTableIncrement. Only the high byte is used as table index.
 */
__no_init __regvar volatile uint16_t sineTableIndex               @12;


/*! \brief The number of 'ticks' between two hall sensor changes.
 *
 *  This variable is used to count the number of 'ticks' between each hall
 *  sensor change. One 'tick' is one PWM period, 510 CPU clock cycles. This is
 *  used to calculate sine table increment. The speed of the motor is inversely
 *  proportional to this value.
 */
__no_init __regvar volatile uint16_t commutationTicks             @10;


/*! \brief The amplitude of the generated sine waves.
 *
 *  This variable controls the amplitude of the generated sine waves.
 *  The range is 0-255.
 */
__no_init __regvar volatile uint8_t amplitude                     @9;


/*! \brief The lead angle or advance commutation angle
 *
 *  This variable specifies a shift in the sine table that will
 *  generate advance commutaiton. For a 192 element sine table, an increase
 *  of one in advanceCommutationSteps will result in 1.875 degrees lead
 *  angle.
 */
__no_init __regvar volatile uint8_t advanceCommutationSteps              @8;


/*! \brief Index to the end of the current commutation sector
 *
 *  This variable holds the index where the next commutation sector starts,
 *  including the advance commutation angle.
 *
 *  It is used to prevent the output from going further until the hall sensors
 *  changes.
 */
__no_init __regvar volatile uint8_t sineTableNextSectorStart     @7;


/*! \brief Current measurement
 *
 *  The most recent current measurement is stored in this variable. It is not
 *  used for any purpose in this implementation, but the measurement is
 *  updated.
 */
volatile uint8_t current;


//! The most recent speed input measurement.
volatile uint8_t speedInput;

/*! \brief Speed controller run flag.
 *
 *  This variable is set to TRUE every time the speed controller should be run.
 */
volatile uint8_t SpeedControllerRun = FALSE;


#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
//! Struct used to hold PID controller parameters and variables.
pidData_t pidParameters;
#endif


/*! \brief Main function / initialization
 *
 *  The main function initializes all subsystems needed for motor control
 *  and enables interrupts, which kicks off the fully interrupt-driven
 *  motor control. The main function goes into an eternal loop where it
 *  does nothing.
 */
void main(void)
{
  //Initialize peripherals.
  PortsInit();
  TimersInit();
  PinChangeIntInit();
  ADCInit();

  SetAdvanceCommutation(0);

#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
  PID_Init(PID_K_P, PID_K_I, PID_K_D, &pidParameters);
#endif

  //Do not start until everything is ready.
  CheckEmergencyShutdown();

  //Initialize fastflags. Use temporary variable to avoid several accesses to
  //volatile variable.
  {
    PMSMflags_t fastFlagsInitial;

    // Set motorStopped to FALSE at startup. This will make sure that the motor
    // is not started if it is not really stopped. If it is stopped, this variable
    // will quickly be updated.
    fastFlagsInitial.motorStopped = FALSE;

    // Set motorSyncronized to FALSE at startup. This will prevent the motor from being
    // driven until the motor is in synch or stopped.
    fastFlagsInitial.motorSynchronized = FALSE;
    fastFlagsInitial.actualDirection = DIRECTION_UNKNOWN;
    fastFlagsInitial.desiredDirection = 0;
    fastFlagsInitial.driveWaveform = WAVEFORM_UNDEFINED;

    fastFlags = fastFlagsInitial;
  }

  DesiredDirectionUpdate();

  // Enable Timer1 capture event interrupt.
  TIMSK1 |= (1 << ICIE1);

  //Enable interrupts globally and let motor driver take over.
  __enable_interrupt();
  for(;;)
  {
    if (SpeedControllerRun)
    {
      SpeedController();
      SpeedControllerRun = FALSE;
    }
  }
}


/*! \brief Initializes I/O port directions and pull-up resistors
 *
 *  This function initializes all I/O ports with correct
 *  direction and pull-up resistors, if needed.
 */
static void PortsInit(void)
{
#if (HALL_PULLUP_ENABLE == TRUE)
  // Set hall sensor pins as input, pullups enabled.
  PORTC = (1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN);
#endif

  // PORTD outputs
  DDRD = (1 << REV_ROTATION_PIN) | (1 << TACHO_OUTPUT_PIN);

  //Enable pull-up on direction signal.
  PORTD |= (1 << DIRECTION_COMMAND_PIN);
}


/*! \brief Initializes and synchronizes Timers
 *
 *  This function sets the correct prescaler and starts all
 *  three timers. The timers are synchronized to ensure that
 *  all PWM signals are aligned.
 */
static void TimersInit(void)
{
  //Set all timers in "Phase correct mode". Do not enable outputs yet.
  TCCR0A = (1 << WGM00);
  TCCR1A = (1 << WGM11);
  TCCR2A = (1 << WGM20);

  //Set top value of Timer/counter1.
  ICR1 = 0xff;

  //Synchronize timers
  TCNT0 = 0;
  TCNT1 = 2;
  TCNT2 = 4;

  // Start all 3 timers.
  TCCR0B = (0 << CS01) | (1 << CS00);
  TCCR1B = (1 << WGM13) | (0 << CS11) | (1 << CS10);
  TCCR2B = (0 << CS21) | (1 << CS20);
}


/*! \brief Initialize pin change interrupts.
 *
 *  This function initializes pin change interrupt on hall
 *  sensor input pins input, emergency shutdown
 *  input and motor direction control input.
 */
static void PinChangeIntInit(void)
{
  // Initialize pin change interrupt on emergency shutdown pin.
  PCMSK0 = (1 << PCINT5);

  // Initialize pin change interrupt on hall sensor inputs.
  PCMSK1 = (1 << PCINT10) | (1 << PCINT9) | (1 << PCINT8);

  // Initialize pin change interrupt on direction input pin.
  PCMSK2 = (1 << PCINT18);

  // Enable pin change interrupt on ports with pin change signals
  PCICR = (1 << PCIE2) | (1 << PCIE1) | (1 << PCIE0);
}


/*! \brief Initializes the ADC
 *
 *  This function initializes the ADC for speed reference measurements.
 *  The ADC will operate in free-running mode, and reading is done in the
 *  ADC complete interrupt service routine.
 */
static void ADCInit(void)
{
  //Select initial AD conversion channel.
  ADMUX = ADMUX_SPEED_REF;

  //Initialize ADC.
  ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADATE) | (1 << ADIF) | (1 << ADIE) | ADC_PRESCALER;

  //Set trigger source to Timer/Counter0 overflow.
  ADCSRB = (1 << ADTS2) | (0 << ADTS1) | (0 << ADTS0);
}


/*! \brief Checks whether emergency shutdown is set at startup.
 *
 *  This function checks whether emergency shut-off signal is
 *  set at startup and takes action accordingly. This function is only needed
 *  at startup, since pin change interrupts are enabled for these signals.
 *
 *  \note In this version, nothing is done in these situations.
 *
 *  \todo Write code to handle a situation where either emergency shut-off
 *  set at startup.
 */
static void CheckEmergencyShutdown(void)
{
  if ( (PINB & (1 << EMERGENCY_SHUTDOWN_PIN)) != 0)
  {
    //Insert code here to handle emercency shutdown signal at startup.
  }
}


/*! \brief  Speed regulator loop.
 *
 *  This function is called every SPEED_REGULATOR_TIME_BASE ticks. In this
 *  implementation, a simple PID controller loop is called, but this function
 *  could be replaced by any speed (or other regulator).
 */
static void SpeedController(void)
{
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)

  //Calculate an increment setpoint from the analog speed input.
  int16_t incrementSetpoint = ((uint32_t)speedInput * SPEED_CONTROLLER_MAX_INCREMENT) / SPEED_CONTROLLER_MAX_INPUT;

  //PID regulator with feed forward from speed input.
  int32_t outputValue;
  outputValue = (uint32_t)speedInput;
  outputValue += PID_Controller(incrementSetpoint, (int16_t)sineTableIncrement, &pidParameters);

  if (outputValue < 0)
  {
    outputValue = 0;
  }
  else if (outputValue > 255)
  {
    outputValue = 255;
  }

  amplitude = outputValue;
#else
  amplitude = speedInput;
#endif
}


/*! \brief Configures timers for sine wave generation
 *
 *  This function is called every time sine wave generation is
 *  needed. PWM outputs are safely disabled while configuration
 *  registers are changed to avoid unintended driving or shoot-
 *  through.
 */
#pragma inline=forced
static void TimersSetModeSinusoidal(void)
{
  //Set PWM pins to input (Hi-Z) while changing modes.
  DisablePWMOutputs();

  //Sets all 3 timers in inverted pair mode.
  TCCR0A = (1 << COM0A1) | (0 << COM0A0) | (1 << COM0B1) | (1 << COM0B0) | (1 << WGM00);
  TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11);
  TCCR2A = (1 << COM2A1) | (0 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20);

  //Make sure all outputs are turned off before PWM outputs are enabled.
  OCR0A = OCR1AL = OCR2A = 0;
  OCR0B = OCR1BL = OCR2B = 0xff;

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

  fastFlags.driveWaveform = WAVEFORM_SINUSOIDAL;

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


/*! \brief Configures timers for block commutation.
 *
 *  This function is called every time block commutation is
 *  needed. PWM outputs are safely disabled while configuration
 *  registers are changed to avoid unintended driving or shoot-
 *  through.
 */
#pragma inline=forced
static void TimersSetModeBlockCommutation(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);