mc_drv.c

Go to the documentation of this file.

#include "config.h"
#include "config_motor.h"

#include "mc_drv.h"
#include "mc_interface.h"
#include "mc_control.h"

#include "adc\adc_drv.h"
#include "dac\dac_drv.h"
#include "amplifier\amplifier_drv.h"
#include "pll\pll_drv.h"
#include "comparator\comparator_drv.h"

static U8 count = 1;     
static U16 average = 0;  
static U8 ovf_timer = 0; 

static Bool g_mc_read_enable = FALSE;  // the speed can be read
Bool g_tick = FALSE;             

static Bool delay_armed = FALSE;
static U8 delay_30d = 0;
volatile S16 comp_delay_30d = MIN_DELAY;
static U8 count_sub_tick = 8;
static U8 delay_mask = 0;

static S32 Num_turn = 0; 
static U8 hall_state = 0;

static char State = CONV_INIT; 
static char ADC_State = FREE;  


/**********************************************************************************/
/**********************************************************************************/
/*               Hardware Initialization                                          */
/**********************************************************************************/
/**********************************************************************************/

void mc_init_HW(void)
{
  mc_init_port();
  mc_init_IT();

  // Be careful : initialize DAC and Over_Current before PWM.
  init_dac();
  mc_set_Over_Current(200); // 5 => 1A ; 8 => 40A
  mc_init_pwm();

  mc_config_time_estimation_speed();
  mc_config_sampling_period();

  init_comparator0();
  init_comparator1();
  init_comparator2();
}

void mc_init_SW(void)
{
  Enable_interrupt();
}

void mc_init_port(void)
{
  // Output Pin configuration
  // PD0 => H_A     PB7 => L_A
  // PC0 => H_B     PB6 => L_B
  // PB0 => H_C     PB1 => L_C

  //Do not modify PSCOUT Configuration
  // PORT B :
  DDRB = (1<<DDB7)|(1<<DDB6)|(1<<DDB3)|(1<<DDB1)|(1<<DDB0);
  // PORT C :
  DDRC = (1<<DDC0);
  // PORT D :
  DDRD = (1<<DDD0);


  // DDnx = 0:Input 1:Output    (n = B,C,D,E ; x = 0,1,2,3,4,5,6,7)
  // PB3 => EXT1                        PB4 => EXT2
  // PC1 => EXT3                        PC2 => EXT4
  // PB5 => EXT5/POT                    PE1 => EXT6
  // PD3 => EXT7/MOSI/LIN_TxD/TxD       PD4 => EXT8/MISO/LIN_RxD/RxD
  // PE0 => EXT9/NRES                   PD2 => EXT10/MISO

  // Modify DDnx according to your hardware implementation
  // PORT B :
  DDRB |= (0<<DDB5)|(1<<DDB4)|(0<<DDB3);
  // PORT C :
  DDRC |= (0<<DDC2)|(0<<DDC1);
    // PORT D :
  DDRD |= (0<<DDD4)|(0<<DDD3)|(0<<DDD2); // Becareful if using the UART interface or JTAGE ICE mkII.
  // PORT E :
  DDRE |= (1<<DDE2)|(0<<DDE1)|(0<<DDE0); // Becareful PE0 is you by JTAGE ICE mkII.


  // Warning Output Low for MOSFET Drivers
  PORTB &= ~(1<<PORTB7 | 1<<PORTB6 | 1<<PORTB3 |1<<PORTB1 | 1<<PORTB0);
  PORTC &= ~(1<<PORTC0);
  PORTD &= ~(1<<PORTD0);

  // pull up activation
  PORTC |= (1<<PORTC6)|(1<<PORTC1);
  PORTD |= (1<<PORTD7)|(1<<PORTD5)|(1<<PORTD1);

  // Disable Digital Input for amplifier1
  // Digitals Inputs for comparators are not disable.
  DIDR0 = (0<<ADC6D)|(0<<ADC3D)|(0<<ADC2D);
  DIDR1 = (0<<ACMP0D)|(0<<ACMP1D)|(1<<AMP1PD)|(1<<AMP1ND);
}

void mc_init_pwm()
{
  Start_pll_32_mega();
  Wait_pll_ready();

  // In Center Aligned Mode :
  // => PSCx_Init(Period_Half, Dutyx0_Half, Synchro, Dutyx1_Half)
#ifdef MCU_REV_B
  PSC0_Init(255,0,255,0);
  PSC1_Init(255,0,255,0);
  PSC2_Init(255,0,255,0);
#else
  PSC0_Init(255,0,1,0);
  PSC1_Init(255,0,1,0);
  PSC2_Init(255,0,1,0);
#endif

}

void mc_init_IT(void)
{
  EICRA =(0<<ISC21)|(1<<ISC20)|(0<<ISC11)|(1<<ISC10)|(0<<ISC01)|(1<<ISC00);
  EIFR = (1<<INTF2)|(1<<INTF1)|(1<<INTF0); // clear possible IT due to config
  EIMSK=(1<<INT2)|(1<<INT1)|(1<<INT0);
}

// PSC initialization depend on the PSC mode
//  0- One ramp Mode
//  1- Two ramp Mode
//  2- Four ramp Mode
//  3- Center Aligned Mode

void PSC0_Init ( unsigned int OCRnRB,
                 unsigned int OCRnSB,
                 unsigned int OCRnRA,
         unsigned int OCRnSA)
{
  OCR0SAH = HIGH(OCRnSA);
  OCR0SAL = LOW(OCRnSA);
  OCR0RAH = HIGH(OCRnRA);
  OCR0RAL = LOW(OCRnRA);
  OCR0SBH = HIGH(OCRnSB);
  OCR0SBL = LOW(OCRnSB);
  OCR0RBH = HIGH(OCRnRB);
  OCR0RBL = LOW(OCRnRB);

  PCNF0 =  RAMP_MODE_NUMBER | (1<<PCLKSEL0) | OUTPUT_ACTIVE_HIGH ;
  /* use PSC 0  Input A as Fault Input */
  PFRC0A = (1<<PELEV0A)|(1<<PFLTE0A)|(0<<PRFM0A3)|(1<<PRFM0A2)|(1<<PRFM0A1)|(1<<PRFM0A0);
//  PFRC0A = 0;
  PFRC0B = 0;
  PSOC0 = (1<<PSYNC00); //Send signal on match with OCRnSA (during counting up of PSC)
  PCTL0 = (0<<PAOC0A)|(1<<PARUN0)|PRESC_DIV_BY_4; /* AUTORUN !! */
}

void PSC1_Init ( unsigned int OCRnRB,
                 unsigned int OCRnSB,
                 unsigned int OCRnRA,
         unsigned int OCRnSA)
{
  OCR1SAH = HIGH(OCRnSA);
  OCR1SAL = LOW(OCRnSA);
  OCR1RAH = HIGH(OCRnRA);
  OCR1RAL = LOW(OCRnRA);
  OCR1SBH = HIGH(OCRnSB);
  OCR1SBL = LOW(OCRnSB);
  OCR1RBH = HIGH(OCRnRB);
  OCR1RBL = LOW(OCRnRB);

  PCNF1 =  RAMP_MODE_NUMBER | (1<<PCLKSEL1) | OUTPUT_ACTIVE_HIGH ;
  PFRC1A = 0;
  PFRC1B = 0;
  PCTL1 = (0<<PAOC1A)|(1<<PARUN1)|PRESC_DIV_BY_4; /* AUTORUN !! */
}


void PSC2_Init ( unsigned int OCRnRB,
                 unsigned int OCRnSB,
                 unsigned int OCRnRA,
         unsigned int OCRnSA)
{
  OCR2SAH = HIGH(OCRnSA);
  OCR2SAL = LOW(OCRnSA);
  OCR2RAH = HIGH(OCRnRA);
  OCR2RAL = LOW(OCRnRA);
  OCR2SBH = HIGH(OCRnSB);
  OCR2SBL = LOW(OCRnSB);
  OCR2RBH = HIGH(OCRnRB);
  OCR2RBL = LOW(OCRnRB);

  PCNF2 =  RAMP_MODE_NUMBER | (1<<PCLKSEL2) | OUTPUT_ACTIVE_HIGH ;
  PFRC2A = 0;
  PFRC2B = 0;
//  PCTL2 = (0<<PAOC2A)|(1<<PRUN2)|PRESC_DIV_BY_4; /* RUN !! */
}

/**********************************************************************************/
/*                   PSC Stop                                                     */
/**********************************************************************************/
/*  This function stops the PSCs :                                                */
/**********************************************************************************/
void PSC_Stop(void)
{
   Set_none();
   PCTL2 = (1<<PCCYC2);
}

/**********************************************************************************/
/*                   PSC Run                                                     */
/**********************************************************************************/
/*  This function starts the PSCs :                                               */
/**********************************************************************************/
void PSC_Run(void)
{
  PCTL2 = (0<<PAOC2A)|(1<<PRUN2)|PRESC_DIV_BY_4; /* RUN !! */
}

/**********************************************************************************/
/**********************************************************************************/
/*         All functions for motor's phases commutation                           */
/**********************************************************************************/
/**********************************************************************************/

U8 mc_get_hall(void)
{
  return HALL_SENSOR_VALUE(); /* sensor */
//  return ((~HALL_SENSOR_VALUE())&0x07); /* sensorless */
}

#pragma vector = HALL_A()
__interrupt void mc_hall_a(void)
{

  delay_armed = TRUE;
  delay_30d = comp_delay_30d;

  //estimation speed on rising edge of Hall_A
#ifdef USE_INTERNAL_COMPARATORS
  if (ACSR&(1<<AC0O))
#else
  if (PIND&(1<<PORTD7))
#endif
  {
    mc_estimation_speed();
    g_mc_read_enable=FALSE; // Wait 1 period
  }
  else
  {
    g_mc_read_enable=TRUE;
  }


  switch(hall_state)
  {
  case 2 : Num_turn++;break;
  case 3 : Num_turn--;break;
  default: break;
  }
  hall_state = 1;
}

#pragma vector = HALL_B()
__interrupt void mc_hall_b(void)
{

  delay_armed = TRUE;
  delay_30d = comp_delay_30d;

  switch(hall_state)
  {
  case 1 : Num_turn--;break;
  case 3 : Num_turn++;break;
  default: break;
  }
  hall_state = 2;
}

#pragma vector = HALL_C()
__interrupt void mc_hall_c(void)
{

  delay_armed = TRUE;
  delay_30d = comp_delay_30d;

  switch(hall_state)
  {
  case 2 : Num_turn--;break;
  case 1 : Num_turn++;break;
  default: break;
  }
  hall_state = 3;
}

void mc_duty_cycle(U8 level)
{
  U8 duty;
  duty = level;

  PCNF0 = SET_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL0)|(1<<POP0); /* set plock */
  PCNF1 = SET_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL1)|(1<<POP1); /* set plock */
  PCNF2 = SET_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL2)|(1<<POP2); /* set plock */

  // Duty = 0   => Duty Cycle   0%
  // Duty = 255 => Duty Cycle 100%

  // Set the duty cycle for PSCn0
  OCR0SAH = 0;
  OCR0SAL = 255 - duty;

  OCR1SAH = 0;
  OCR1SAL = 255 - duty;

  OCR2SAH = 0;
  OCR2SAL = 255 - duty;

  // Set the duty cycle for PSCn1 according to the PWM strategy
  #ifdef HIGH_AND_LOW_PWM
  // apply PWM on high side and low side switches
    OCR0SBH = 0;
    OCR0SBL = 255 - duty  ;

    OCR1SBH = 0;
    OCR1SBL = 255 - duty  ;

    OCR2SBH = 0;
    OCR2SBL = 255 - duty  ;
  #else
  // PWM is only applied on high side switches
  // 100% duty cycle on low side switches
    OCR0SBH = 0;
    OCR0SBL = 2;

    OCR1SBH = 0;
    OCR1SBL = 2;

    OCR2SBH = 0;
    OCR2SBL = 2;
  #endif

  Disable_interrupt();
  PCNF0 = RELEASE_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL0)|(1<<POP0); /* release plock */
  PCNF1 = RELEASE_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL1)|(1<<POP1); /* release plock */
  PCNF2 = RELEASE_PLOCK | RAMP_MODE_NUMBER |(1<<PCLKSEL2)|(1<<POP2); /* release plock */
  Enable_interrupt();
}

void mc_switch_commutation(U8 position)
{
  // get the motor direction to commute the right switches.
  char direction = mci_get_motor_direction();

  // Switches are commuted only if the user start the motor and
  // the speed consign is different from 0.
  if ((mci_motor_is_running()) && (mci_get_motor_speed()!=0))
  {
    mc_duty_cycle(mc_get_Duty_Cycle());
    switch(position)
    {
    // cases according to rotor position
      case HS_001:  if (direction==CCW)  {Set_Q3Q2();}
                    else                 {Set_Q5Q2();}
                    break;

      case HS_101:  if (direction==CCW)  {Set_Q5Q2();}
                    else                 {Set_Q5Q4();}
                    break;

      case HS_100:  if (direction==CCW)  {Set_Q5Q4();}
                    else                 {Set_Q1Q4();}
                    break;

      case HS_110:  if (direction==CCW)  {Set_Q1Q4();}
                    else                 {Set_Q1Q6();}
                    break;

      case HS_010:  if (direction==CCW)  {Set_Q1Q6();}
                    else                 {Set_Q3Q6();}
                    break;

      case HS_011:  if (direction==CCW)  {Set_Q3Q6();}
                    else                 {Set_Q3Q2();}
                    break;
      default : break;
      }
  }
  else
  {
    Set_none(); // all switches are switched OFF
  }
}

/********************************************************************/
/********************************************************************/
/*         TIMER 1 :    Sampling time configuration                 */
/********************************************************************/
/********************************************************************/

void mc_config_sampling_period(void)
{
  TCCR1A = 0;         /* Normal port operation + Mode CTC */
  TCCR1B = 1<<WGM12 | 1<<CS11 | 1<<CS10 ; /* Mode CTC + prescaler 64 */
  TCCR1C = 0;
  OCR1AH = 0;
  OCR1AL = 7;         /* 31.25 µS */
  TIMSK1=(1<<OCIE1A); /* Output compare B Match interrupt Enable */
}

#pragma vector = TIMER1_COMPA_vect
__interrupt void timer1_periodic_interrupt(void)
{
  count_sub_tick --;
  if (count_sub_tick == 0)
  {
    g_tick = TRUE;
    count_sub_tick = 8;
  }

  if (delay_armed == TRUE)
  {
    delay_30d --;
    if (delay_30d == 0)
    {
      delay_armed = FALSE;
      delay_mask = MASK_DEMAG;
      Disable_comparator0_interrupt();
      Disable_comparator1_interrupt();
      Disable_comparator2_interrupt();
      mc_switch_commutation(HALL_SENSOR_VALUE());
    }
  }
  if (delay_mask != 0)
  {
    delay_mask --;
    if (delay_mask == 0)
    {
      ACSR = (1<<AC2IF)|(1<<AC1IF)|(1<<AC0IF);
      Enable_comparator0_interrupt();
      Enable_comparator1_interrupt();
      Enable_comparator2_interrupt();
    }
  }
}

/********************************************************************/
/********************************************************************/
/*       TIMER 0 :  Estimation speed                                */
/********************************************************************/
/********************************************************************/

void mc_config_time_estimation_speed(void)
{
  TCCR0A = 0;
  TCCR0B = (0<<CS02)|(1<<CS01)|(1<<CS00); // 64 prescaler (8us)
  TIMSK0 = (1<<TOIE0);
}

#pragma vector = TIMER0_OVF_vect
__interrupt void ovfl_timer(void)
{
  TCNT0=0x00;
  ovf_timer++;
  // if they are no commutation after 125 ms
  // 125 ms = (61<<8) * 8us
  if(ovf_timer >= 100)
  {
    ovf_timer = 0;
    mci_store_measured_speed(0);
  }
}

void mc_estimation_speed(void)
{
  U16 timer_value;
  U32 new_measured_speed;

  if (g_mc_read_enable==OK)
  {
    // Two 8 bits variables are use to simulate a 16 bits timers
    timer_value = (ovf_timer<<8) + TCNT0;

    /* compute delay for 30 degres */
    comp_delay_30d = (timer_value / 94) - FILTER_DELAY;
    /* apply a saturation */
    if (comp_delay_30d < (S16)MIN_DELAY) comp_delay_30d = MIN_DELAY;
    else if (comp_delay_30d > (S16)MAX_DELAY) comp_delay_30d = MAX_DELAY;

    if (timer_value == 0) {timer_value += 1 ;} // warning DIV by 0
    new_measured_speed = K_SPEED / timer_value;
    if(new_measured_speed > 255) new_measured_speed = 255; // Variable saturation


      // To avoid noise an average is realized on 16 samples
      average += new_measured_speed;
      if(count >= 16)
      {
        count = 1;
        mci_store_measured_speed(average >> 4);
        average = 0;
      }
      else count++;

    // Reset Timer 0 register and variables
    TCNT0=0x00;
    ovf_timer = 0;
    g_mc_read_enable=KO;
  }
}

/********************************************************************/
/********************************************************************/
/*     ADC         use for current measure and potentiometer...     */
/********************************************************************/
/********************************************************************/

#pragma vector = ADC_vect
__interrupt void ADC_EOC(void)
{
  if(State == CONV_CURRENT) mci_store_potentiometer_value(Adc_get_8_bits_result());
  if(State == CONV_POT) mci_store_measured_current(Adc_get_10_bits_result());
  ADC_State = FREE;
}

void mc_ADC_Scheduler(void)
{
  switch(State)
  {
  case CONV_INIT :
    init_adc();
    init_amp1();
    ADC_State = FREE;
    State = CONV_POT;
    break;

  case CONV_POT :
    if(ADC_State == FREE)
    {
      ADC_State = BUSY;
      State= CONV_CURRENT;
      Left_adjust_adc_result();
      Start_conv_channel(6);
    }
    break;

  case CONV_CURRENT :
    if(ADC_State == FREE)
    {
      ADC_State = BUSY;
      State = CONV_POT;
      Right_adjust_adc_result();
      Start_amplified_conv_channel(12);
//      Start_conv_channel(9);
    }
    break;
  }

}
/********************************************************************/
/********************************************************************/
/*      Over Current Detection                                      */
/********************************************************************/
/********************************************************************/


void mc_set_Over_Current(U8 Level)
{
  Set_dac_8_bits(Level);
}


/********************************************************************/
/********************************************************************/
/*                              Number of turns calculation         */
/********************************************************************/
/********************************************************************/

S32 mc_get_Num_Turn()
{
  return Num_turn;
}

void mc_reset_Num_Turn()
{
  Num_turn = 0;
}

---