main.c

Go to the documentation of this file.

/* This file has been prepared for Doxygen automatic documentation generation.*/
#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

__io volatile PMSMflags_t fastFlags                               @0x1e;


__no_init __regvar volatile uint16_t sineTableIncrement           @14;


__no_init __regvar volatile uint16_t sineTableIndex               @12;


__no_init __regvar volatile uint16_t commutationTicks             @10;


__no_init __regvar volatile uint8_t amplitude                     @9;


__no_init __regvar volatile uint8_t advanceCommutationSteps              @8;


__no_init __regvar volatile uint8_t sineTableNextSectorStart     @7;


volatile uint8_t current;


volatile uint8_t speedInput;

volatile uint8_t SpeedControllerRun = FALSE;


#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)

pidData_t pidParameters;
#endif


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;
    }
  }
}


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);
}


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);
}


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);
}


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);
}


static void CheckEmergencyShutdown(void)
{
  if ( (PINB & (1 << EMERGENCY_SHUTDOWN_PIN)) != 0)
  {
    //Insert code here to handle emercency shutdown signal at startup.
  }
}


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
}


#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();
}


#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);

  //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();
}


#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


#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)) )
  {

  }
}


#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;
}


#pragma inline=forced
static uint8_t GetDesiredDirection(void)
{
  return (uint8_t)fastFlags.desiredDirection;
}


#pragma inline=forced
static uint8_t GetActualDirection(void)
{
  return fastFlags.actualDirection;
}


#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;
}


#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;
}


#pragma inline=forced
static void DesiredDirectionUpdate(void)
{
  if ( (PIND & (1 << DIRECTION_COMMAND_PIN)) != 0 )
  {
    fastFlags.desiredDirection = DIRECTION_REVERSE;
  }
  else
  {
    fastFlags.desiredDirection = DIRECTION_FORWARD;
  }
}


#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;
  }
}


#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
}


#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;
  }
}


#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);
}


#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);
  }
}


#pragma inline=forced
static void SetAdvanceCommutation(const uint8_t advanceCommutation)
{
  advanceCommutationSteps = advanceCommutation;
}


#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
}


#pragma inline=forced
static void EnablePWMOutputs(void)
{
  DDRB |= PWM_PATTERN_PORTB;
  DDRD |= PWM_PATTERN_PORTD;
}


#pragma inline=forced
static void DisablePWMOutputs(void)
{
  DDRB &= ~PWM_PATTERN_PORTB;
  DDRD &= ~PWM_PATTERN_PORTD;
}


#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
    }
  }
}


#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;
  }
}


#pragma inline=forced
static uint8_t IsMotorSynchronized(void)
{
  return (uint8_t)(fastFlags.motorSynchronized);
}


#pragma vector=PCINT0_vect
__interrupt void EmergencyInterruptISR(void)
{
  DisablePWMOutputs();
  for (;;)
  {

  }
}


#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;
}


#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
}


#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;
    }
  }
}


#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);
}

---