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 "PMSM_tables.h"
#include "TinyX61_macros.h"


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


volatile __io PMSMflags_t fastFlags                         @0x0a;


__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 uint16_t amplitude               @8;


__no_init __regvar volatile uint8_t advanceCommutationSteps      @7;


__no_init __regvar volatile uint8_t sineTableNextSectorStart     @6;


__no_init __regvar volatile uint8_t speedControllerTimer @5;


#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
pidData_t pidParameters;
#endif


void main(void)
{
  PortsInit();
  PLLInit();
  PWMInit();
  PinChangeInit();
  ADCInit();

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

  speedControllerTimer= SPEED_CONTROLLER_TIME_BASE;
  SetAdvanceCommutation(0);

  //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 Timer/counter1 overflow.
  TIMSK |= (1 << TOV1);

  //Enable interrupts globally and let motor driver take over.
  __enable_interrupt();

  for (;;)
  {
    uint16_t speedReference;

    if ( !(ADCSRA & (1 << ADSC)) )
    {
      speedReference = ADC;
      ADCSRA |= (1 << ADSC);
    }

    if (speedControllerTimer == 0)
    {
      speedControllerTimer = SPEED_CONTROLLER_TIME_BASE;
      SpeedController(speedReference);
    }
  }
}



static void PortsInit(void)
{
#if (HALL_PULL_UP_ENABLE)
  PORTA = (1 << H1_PIN) | (1 << H2_PIN) | (1 << H3_PIN);
#endif

  DDRA = (1 << PA4);
}


static void PLLInit(void)
{
  //Wait for PLL lock
  while ( !(PLLCSR & (1 << PLOCK)) )
  {

  }
  PLLCSR = (1 << PCKE);
}


static void PWMInit(void)
{
  //Start TCNT1 with prescaler 1.
  TCCR1B = (PWM_INVERT_OUTPUT << PWM1X) | (1 << CS10);

  //Enable fault protection input on INT0 pin, falling edge.
  TCCR1D = (1 << FPIE1) | (1 << FPEN1) | (0 << FPES1);

  //Set dead-time.
  DT1 = (DEAD_TIME << 4) | (DEAD_TIME);

  //Set PWM top value.
  TC1_WRITE_10_BIT_REGISTER(OCR1C, PWM_TOP_VALUE);
}


static void ADCInit(void)
{
  //Use PA7 as ADC input, 2.56V internal voltage reference without bypass capacitor.
  ADMUX = (1 << REFS1) | (0 << MUX4) | (0 << MUX3) | (1 << MUX2) | (1 << MUX1) | (0 << MUX0);

  ADCSRA = (1 << ADEN) |                  //Enable the ADC.
           (1 << ADSC) |                  //Start the first converssion.
           (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);   // Prescaler CK/128.
  ADCSRB = (1 << REFS2); //2.56V internal reference.
}


static void PinChangeInit(void)
{
  //Initialize hall change interrupt
  PCMSK0 = (1 << PCINT2) | (1 << PCINT1) | (1 << PCINT0);
  //Clear all other pin change interrupts.
  PCMSK1 = 0x00;

  //
  GIMSK |= (1 << PCIE1);

  //Force the pin change interrupt to be triggered.
  DDRA |= ((1 << PA2) | (1 << PA1) | (1 << PA0));
  DDRA &= ~((1 << PA2) | (1 << PA1) | (1 << PA0));
}


static void SpeedController(uint16_t speedReference)
{
  #if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)

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

  //PID controller with feed forward from speed input.
  int32_t outputValue;
  outputValue = (uint32_t)speedReference;
  outputValue += pid_Controller(incrementSetpoint, (int16_t)sineTableIncrement, &pidParameters);

  if (outputValue < 0)
  {
    outputValue = 0;
  }
  else if (outputValue > PWM_TOP_VALUE)
  {
    outputValue = PWM_TOP_VALUE;
  }
  __disable_interrupt();
  amplitude = outputValue;
  __enable_interrupt();
#else
  __disable_interrupt();
  amplitude = speedReference;
  __enable_interrupt();
#endif
}


#pragma inline = forced
static void TimerSetModeSinusoidal(void)
{
  //Set PWM pins to input (Hi-Z) while changing modes.
  DisablePWMOutputs();

  //Set Timer/counter1 in phase and frequency correct mode.
  TCCR1A = (0 << COM1A1) | (1 << COM1A0) | (0 << COM1B1) | (1 << COM1B0) | (1 << PWM1A) | (1 << PWM1B);
  TCCR1C =  (0 << COM1A1S) | (1 << COM1A0S) | (0 << COM1B1S) | (1 << COM1B0S) | (0 << COM1D1) | (1 << COM1D0) | (1 << PWM1D);
  TCCR1D &= ~((1 << WGM11) | (1 << WGM10));
  TCCR1D |= (0 << WGM11) | (1 << WGM10);

  TC1_SET_ALL_COMPARE_VALUES(0x0000);

  fastFlags.driveWaveform = WAVEFORM_SINUSOIDAL;

  //Wait for the PWM cycle to complete.
  TIFR = TIFR;
  while ( ! (TIFR & (1 << TOV1)) )
  {

  }

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


#pragma inline = forced
static void TimerSetModeBlockCommutation(void)
{
  //Set PWM pins to input (Hi-Z) while changing modes.
  DisablePWMOutputs();

  //Set Timer/counter1 in dual slope PWM6 mode, clear on upcounting, set on downcounting (non-inverting).
  TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << COM1B1) | (0 << COM1B0) | (1 << PWM1A) | (1 << PWM1B);
  TCCR1C = (1 << COM1A1S) | (0 << COM1A0S) | (1 << COM1B1S) | (0 << COM1B0S) | (1 << COM1D1) | (0 << COM1D0) | (1 << PWM1D);
  TCCR1D |= (1 << WGM11) | (1 << WGM10);

  TCCR1E = 0x00;

  //Set output duty cycle to 0.
  BlockCommutationSetDuty(0x0000);

  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 TimerSetModeBrake(void)
{
  //Set PWM pins to input (Hi-Z) while changing modes.
  DisablePWMOutputs();

  //Set Timer/counter1 in phase and frequency correct mode.
  TCCR1A = (0 << COM1A1) | (1 << COM1A0) | (0 << COM1B1) | (1 << COM1B0) | (1 << PWM1A) | (1 << PWM1B);
  TCCR1C =  (0 << COM1A1S) | (1 << COM1A0S) | (0 << COM1B1S) | (1 << COM1B0S) | (0 << COM1D1) | (1 << COM1D0) | (1 << PWM1D);
  TCCR1D &= ~((1 << WGM11) | (1 << WGM10));
  TCCR1D |= (0 << WGM11) | (1 << WGM10);

  //All output duty cycles at 0 creates a braking force. (Low side braking)
  TC1_SET_ALL_COMPARE_VALUES(0x0000);

  fastFlags.driveWaveform = WAVEFORM_BRAKING;

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


#pragma inline = forced
static void BlockCommutationSetDuty(const uint16_t compareValue)
{
  TC1_PWM6_SET_DUTY_CYCLE(compareValue);
}


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


#pragma inline = forced
static void BlockCommutate(const uint8_t direction, const uint8_t hall)
{
  if (direction == DIRECTION_FORWARD)
  {
    TCCR1E = blockCommutationTableForward[hall];
  }
  else
  {
    TCCR1E = blockCommutationTableReverse[hall];
  }
}


#pragma inline = forced
static uint8_t GetHall(void)
{
  uint8_t hall;

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


#pragma inline = forced
static void DesiredDirectionUpdate(void)
{
  if ( (PINA & (1 << DIRECTION_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 an
  //illegal hall value, but legal table index.
  if (lastHall > 7)
  {
    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 uint16_t SineTableIncrementCalculate(const uint16_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 EnablePWMOutputs(void)
{
  DDRB |= PWM_PIN_MASK_PORTB;
}


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


#pragma inline = forced
static void CommutationTicksUpdate(void)
{
  if (commutationTicks < COMMUTATION_TICKS_STOPPED)
  {
    commutationTicks++;
  }
  else
  {
    fastFlags.motorStopped = TRUE;
    fastFlags.motorSynchronized = FALSE;
    sineTableIncrement = 0;
    if (fastFlags.driveWaveform != WAVEFORM_BLOCK_COMMUTATION)
    {
      TimerSetModeBlockCommutation();
      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
  {
    synchCount = 0;
  }
}


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


#if (SINE_TABLE_SIZE == SINE_TABLE_SIZE_SMALL)
#pragma inline = forced
uint8_t SineTableSmallGetValue(uint8_t index)
{
  //The last 3rd of the table is zero.
  if (index >= (SINE_TABLE_LENGTH * 2 / 3) )
  {
    return 0;
  }

  //The second third of the table is a mirror of the first third.
  if (index >= (SINE_TABLE_LENGTH / 3) )
  {
    index = ((SINE_TABLE_LENGTH * 2 / 3) - 1) - index;
  }

  return sineTable[index];
}
#endif


#if (SINE_TABLE_SIZE == SINE_TABLE_SIZE_LARGE)
#pragma inline = forced
static void SineOutputUpdate(void)
{
  uint8_t const __flash * sineTablePtr = sineTable;
  uint16_t temp;

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

  //Calculate output duty cycles. Since one phase is always zero, the scaling
  //is only performed on the two non-zero phases, saving one multiply.

  //Calculate U phase output duty cycle.
  temp = *sineTablePtr++;
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_U, temp);

  //Calculate U + 240 degree phase output duty cycle.
  temp = *sineTablePtr++;
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  if (GetDesiredDirection() == DIRECTION_FORWARD)
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_V, temp);
  }
  else
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_W, temp);
  }

  //Calculate U + 120 degree phase output duty cycle.
  temp = *sineTablePtr++;
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  if (GetDesiredDirection() == DIRECTION_FORWARD)
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_W, temp);
  }
  else
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_V, temp);
  }
}
#endif


#if (SINE_TABLE_SIZE == SINE_TABLE_SIZE_SMALL)
#pragma inline = forced
static void SineOutputUpdate(void)
{
  uint16_t temp;
  uint8_t tempIndex;

  //Calculate U phase output duty cycle.
  tempIndex = (uint8_t)(sineTableIndex >> 8);

  temp = SineTableSmallGetValue(tempIndex);
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_U, temp);

  //Calculate U phase + 120 degree output duty cycle.
  tempIndex += (SINE_TABLE_LENGTH / 3);
  if (tempIndex >= SINE_TABLE_LENGTH)
  {
    tempIndex -= SINE_TABLE_LENGTH;
  }

  temp = SineTableSmallGetValue(tempIndex);
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  if (GetDesiredDirection() == DIRECTION_FORWARD)
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_W, temp);
  }
  else
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_V, temp);
  }

  //Calculate U phase + 240 degree output duty cycle.
  tempIndex += (SINE_TABLE_LENGTH / 3);
  if (tempIndex >= SINE_TABLE_LENGTH)
  {
    tempIndex -= SINE_TABLE_LENGTH;
  }

  temp = SineTableSmallGetValue(tempIndex);
  if (temp != 0)
  {
    temp = MultiplyUS15x8(amplitude, temp);
  }
  if (GetDesiredDirection() == DIRECTION_FORWARD)
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_V, temp);
  }
  else
  {
    TC1_WRITE_10_BIT_REGISTER(COMPARE_REGISTER_PHASE_W, temp);
  }
}
#endif


#pragma vector = PCINT0_vect
__interrupt void HallChangeISR()
{
  static uint8_t lastHall = 0xff;
  uint8_t hall;

  hall = GetHall();

  //Make sure that the hall sensors really changed.
  if (hall == lastHall)
  {
    return;
  }

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

  //If sinusoidal driving is used, synchronize sine wave generation to the
  //current hall sensor value. Advance commutation (lead angle) 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 the actual direction flag.
  ActualDirectionUpdate(lastHall, hall);

  lastHall = hall;


  //Calculate new step size 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 = TIM1_OVF_vect
__interrupt void Timer1OverflowISR()
{
  PORTA |= (1 << PA4);
  if (fastFlags.driveWaveform == WAVEFORM_SINUSOIDAL)
  {
    AdjustSineTableIndex(sineTableIncrement);
    SineOutputUpdate();
  }
  else if (fastFlags.driveWaveform == WAVEFORM_BLOCK_COMMUTATION)
  {
    uint16_t blockCommutationDuty = amplitude * BLOCK_COMMUTATION_DUTY_MULTIPLIER;

    if (blockCommutationDuty > PWM_TOP_VALUE)
    {
      blockCommutationDuty = PWM_TOP_VALUE;
    }
    BlockCommutationSetDuty(blockCommutationDuty);
  }

  //Update desired direction flag.
  DesiredDirectionUpdate();

  static uint8_t lastDesiredDirection = 0xff;
  if ( (fastFlags.desiredDirection != lastDesiredDirection))
  {
#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.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;
    if (fastFlags.actualDirection != DIRECTION_UNKNOWN)
    {
      TimerSetModeBrake(); // Automatically sets driveWaveform.
    }
#endif

    lastDesiredDirection = fastFlags.desiredDirection;
  }

  CommutationTicksUpdate();

  if (speedControllerTimer > 0)
  {
    speedControllerTimer--;
  }
  PORTA &= ~(1 << PA4);
}


#pragma vector = FAULT_PROTECTION
__interrupt void FaultProtectionISR()
{
  __delay_cycles(10000000);
  TCCR1D |= (1 << FPEN1);
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
  pid_Reset_Integrator(&pidParameters);
#endif
}


#pragma inline = forced
static unsigned int MultiplyUS15x8(const uint16_t m15, const uint8_t m8)
{
  unsigned int result = 0x0000;

  if (m8 & (1 << 0))
  {
    result += m15;
  }
  result >>= 1;

  if (m8 & (1 << 1))
  {
    result += m15;
  }
  result >>= 1;
  if (m8 & (1 << 2))
  {
    result += m15;
  }
  result >>= 1;

  if (m8 & (1 << 3))
  {
    result += m15;
  }
  result >>= 1;

  if (m8 & (1 << 4))
  {
    result += m15;
  }
  result >>= 1;

  if (m8 & (1 << 5))
  {
    result += m15;
  }
  result >>= 1;
  if (m8 & (1 << 6))
  {
    result += m15;
  }
  result >>= 1;

  if (m8 & (1 << 7))
  {
    result += m15;
  }
  result >>= 1;

  return result;
}

---