mc_drv.c

Go to the documentation of this file.

//_____  I N C L U D E S ___________________________________________________

#ifdef __ICCAVR__ // IAR C Compiler
#include "config.h"
#include "lib_mcu/compiler.h"
#include "lib_mcu/mcu.h"
#include "inavr.h"
#include <stdio.h>
#endif

#ifdef __GNUC__  // GNU C Compiler
#include "config_for_gcc.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include "mc_interface.h"
#endif



#include "table_sin81.h"
#include "lib_mcu/pll/pll_drv.h"


//_____ M A C R O S ________________________________________________________
/* definition of Hall sectors MC200 */
/*#define HAS1     (1<<PIND7) | (0<<PINB2) | (1<<PIND6)
#define HAS2     (0<<PIND7) | (0<<PINB2) | (1<<PIND6)
#define HAS3     (0<<PIND7) | (1<<PINB2) | (1<<PIND6)
#define HAS4     (0<<PIND7) | (1<<PINB2) | (0<<PIND6)
#define HAS5     (1<<PIND7) | (1<<PINB2) | (0<<PIND6)
#define HAS6     (1<<PIND7) | (0<<PINB2) | (0<<PIND6)*/

/* definition of Hall sectors MC100 */
/* be careful this definition is one sector in advance */
/*                Hall_C      Hall_B        Hall_A     */
#define HAS1     (1<<PIND5) | (0<<PINC6) | (1<<PIND7)
#define HAS2     (0<<PIND5) | (0<<PINC6) | (1<<PIND7)
#define HAS3     (0<<PIND5) | (1<<PINC6) | (1<<PIND7)
#define HAS4     (0<<PIND5) | (1<<PINC6) | (0<<PIND7)
#define HAS5     (1<<PIND5) | (1<<PINC6) | (0<<PIND7)
#define HAS6     (1<<PIND5) | (0<<PINC6) | (0<<PIND7)

//_____ D E F I N I T I O N S ______________________________________________

void   init(void);
void   PSC_Init(unsigned int ot0,  unsigned int ot1);
void   PSC_Load (unsigned int dt0a,  unsigned int dt1a,
                 unsigned int dt0b,  unsigned int dt1b,
                 unsigned int dt0c,  unsigned int dt1c);
U16    mc_control_speed_16b(U16 speed_ref , U16 speed_measure);
void   SVPWM(U16 amp, U8 Counter_Value, U8 Top, U8 Sector);
S16    read_acquisition() ;
void   PSC_Start(void);
void   PSC_Stop(void);
void   Do_Sensor_Interrupt(void);
void   Start_ADC(void);

//_____ D E C L A R A T I O N S ____________________________________________
volatile U8   Hall_Sector = HAS1 ;     
volatile U8   Mem_Hall_Sector = HAS1 ; 
volatile U8   Sensor_Counter_Up = 0;   
volatile U8   Sensor_Counter_Down = 1; 
volatile U8   Top_Counter = 1;         
volatile U8   Mem_Top_Counter = 1;     
volatile U16  Mem_Top_CounterX8 = 1;   
volatile U8   Delay_For_Change = 1;    

volatile U8   Prescaler_Main_Tick;     

volatile U16  Amplitude = 0;           
volatile U16  PWM0, PWM1, PWM2;        
volatile U16  Measured_Speed;          

volatile U16  Permanent_Clock = 0;
volatile U8   Hall_Event_Count = 8;    
volatile U16  Mem_Hall_Time = 0;       
volatile U16  Hall_Period = 65535;     

extern volatile Bool mci_run_stop;     
extern volatile U8   Main_Tick;        
extern Bool mci_direction;


void PSC_Init(unsigned int ot0,  unsigned int ot1)
{
   Start_pll_64_mega();   // start the PLL at 64 MHz
   Wait_pll_ready();

   OCR0RAH = HIGH(ot0);
   OCR0RAL = LOW(ot0);
   OCR0RBH = HIGH(ot1);
   OCR0RBL = LOW(ot1);
   PCNF0 = (1<<PMODE01) | (1<<PMODE00) | (1<<POP0) | (1<<PCLKSEL0) ;  /* fast clock input used */
   PFRC0A = 0;
//   PFRC0A = (1<<PFLTE0A)|(0<<PRFM0A3)|(1<<PRFM0A2)|(1<<PRFM0A1)|(1<<PRFM0A0); /* overcurrent */
   PFRC0B = 0;
   PCTL0 = (1<<PARUN0) | (1<<PCCYC0);

   OCR1RAH = HIGH(ot0);
   OCR1RAL = LOW(ot0);
   OCR1RBH = HIGH(ot1);
   OCR1RBL =  LOW(ot1);
   PCNF1 = (1<<PMODE11) | (1<<PMODE10) | (1<<POP1) | (1<<PCLKSEL1);   /* fast clock input used */
   PFRC1A = 0;
   PFRC1B = 0;
   PCTL1 = (1<<PARUN1) | (1<<PCCYC1);

   OCR2RAH = HIGH(ot0);
   OCR2RAL = LOW(ot0);
   OCR2RBH = HIGH(ot1);
   OCR2RBL = LOW(ot1);
   PCNF2 = (1<<PMODE21) | (1<<PMODE20) | (1<<POP2) | (1<<PCLKSEL2);   /* fast clock input used */
   PFRC2A = 0;
   PFRC2B = 0;
//   PCTL2 = (1<<PCCYC2) | (1<<PRUN2) ;

   // connect the PSC waveform generator outputs to the port outputs
   PSOC0 = (1<<POEN0B) | (1<<POEN0A) ;
   PSOC1 = (1<<POEN1B) | (1<<POEN1A) ;
   PSOC2 = (1<<POEN2B) | (1<<POEN2A) ;

}


inline void PSC_Load (unsigned int dt0a,  unsigned int dt1a,
               unsigned int dt0b,  unsigned int dt1b,
               unsigned int dt0c,  unsigned int dt1c)
{
   PCNF0 = (1<<PLOCK0)|(1<<PMODE01) | (1<<PMODE00) | (1<<POP0) | (1<<PCLKSEL0);
   PCNF1 = (1<<PLOCK1)|(1<<PMODE11) | (1<<PMODE10) | (1<<POP1) | (1<<PCLKSEL1);
   PCNF2 = (1<<PLOCK2)|(1<<PMODE21) | (1<<PMODE20) | (1<<POP2) | (1<<PCLKSEL2);

   OCR0SAH = HIGH(dt0a);
   OCR0SAL = LOW(dt0a);
   OCR0SBH = HIGH(dt1a);
   OCR0SBL = LOW(dt1a);

   OCR1SAH = HIGH(dt0b);
   OCR1SAL = LOW(dt0b);
   OCR1SBH = HIGH(dt1b);
   OCR1SBL = LOW(dt1b);

   OCR2SAH = HIGH(dt0c);
   OCR2SAL = LOW(dt0c);
   OCR2SBH = HIGH(dt1c);
   OCR2SBL = LOW(dt1c);

   PCNF0 = (1<<PMODE01) | (1<<PMODE00) | (1<<POP0) | (1<<PCLKSEL0);
   PCNF1 = (1<<PMODE11) | (1<<PMODE10) | (1<<POP1) | (1<<PCLKSEL1);
   PCNF2 = (1<<PMODE21) | (1<<PMODE20) | (1<<POP2) | (1<<PCLKSEL2);
}

void PSC_Start(void)
{
   PCTL2 = (1<<PCCYC2) | (1<<PRUN2) ;
}


void PSC_Stop(void)
{
   PCTL2 = (1<<PCCYC2);
}



/* ------------------ TIMER 0 - INTERRUPT --------------------*/
// interrupt vector for the sampling period
#ifdef __ICCAVR__ // IAR C Compiler
#pragma vector = TIMER0_COMPA_vect
__interrupt void Timer0_IT(void)
#endif
#ifdef __GNUC__  // GNU C Compiler
ISR(TIMER0_COMPA_vect)
#endif
{
   U16    tau1, tau2 ;
   U8     theta1 , theta2 ;
   U16    Thetai_1;
   Set_PC1();

   Permanent_Clock++;

   if (Delay_For_Change != 0)
   {

      Delay_For_Change --;
      if (Delay_For_Change == 0)
      {
         /* Change_Sector */
         Top_Counter = Mem_Top_Counter;
         if (Top_Counter == 0) Top_Counter++; /* to avoid "divide by 0" */
         Sensor_Counter_Down = Mem_Top_Counter;
         Hall_Sector = Mem_Hall_Sector;
         /* end Change_Sector */
      }
   }

   /* increment Sensor_Counter_Up */
   if ( Sensor_Counter_Up < 255 )
   {
      Sensor_Counter_Up += 1;
   }

   /* decrement Sensor_Counter_Down */
   if (Sensor_Counter_Down != 0) Sensor_Counter_Down -= 1;


   // -------------- space vector PWM algorithm -----------------
   Thetai_1 = ((U16)(Top_Counter - Sensor_Counter_Down) * MAX_THETA) / Top_Counter;

   Enable_interrupt();

   theta2 = (unsigned char) Thetai_1 ;

   theta1 = (unsigned char) (MAX_THETA - theta2) ;

   /* 128 is the max value of the normalized sin table */
   tau1 = ((U32)Amplitude * tab_sin[theta1]) / 128 ;
   tau2 = ((U32)Amplitude * tab_sin[theta2]) / 128 ;

   if (mci_direction == CW)
   {
      switch (Hall_Sector)
      {
      case HAS5 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ; break ;
      case HAS4 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ; break ;
      case HAS3 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ; break ;
      case HAS2 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ; break ;
      case HAS1 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ; break ;
      case HAS6 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ; break ;
      default :
         PWM0 = (unsigned short int) (MAX_PWM/2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2) ; break ;
      }
      PSC_Load (PWM0, PWM0+DEAD_TIME, PWM2, PWM2+DEAD_TIME, PWM1, PWM1+DEAD_TIME); // CW
   }
   else
   {
      switch (Hall_Sector)
      {
      case HAS1 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ; break ;
      case HAS2 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ; break ;
      case HAS3 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ; break ;
      case HAS4 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ; break ;
      case HAS5 :
         PWM0 = (unsigned short int) (MAX_PWM/2 + tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ; break ;
      case HAS6 :
         PWM0 = (unsigned short int) (MAX_PWM/2 - tau1 - tau2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2 + tau1 + tau2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2 - tau1 + tau2) ; break ;
      default :
         PWM0 = (unsigned short int) (MAX_PWM/2) ;
         PWM1 = (unsigned short int) (MAX_PWM/2) ;
         PWM2 = (unsigned short int) (MAX_PWM/2) ; break ;
      }
      PSC_Load (PWM0, PWM0+DEAD_TIME, PWM1, PWM1+DEAD_TIME, PWM2, PWM2+DEAD_TIME); // CCW
   }

   // -------- generate the main Tick ---------------------------
   Prescaler_Main_Tick -= 1;
   if (Prescaler_Main_Tick == 0)
   {
//      Prescaler_Main_Tick = 21; /* 21 * 48µS generates 1.008mS */
//      Prescaler_Main_Tick = 104; /* 104 * 48µS generates 5mS */
      Prescaler_Main_Tick = 78; /* 78 * 64µS generates 5mS */
      Main_Tick=1;
   }
   Clear_PC1();
}




/* ---------------- SENSOR - INTERRUPT ------------------*/
inline void Do_Sensor_Interrupt(void)
{
   if ( Sensor_Counter_Up > 3 )
   {

      if (Delay_For_Change != 0)
      {
         /* Last Delay not finished : Force Change_Sector */
         Top_Counter = Mem_Top_Counter;
         if (Top_Counter == 0) Top_Counter++; /* to avoid "divide by 0" */

         Sensor_Counter_Down = Mem_Top_Counter;
         Hall_Sector = Mem_Hall_Sector;
         /* end Change_Sector */
      }

      /* Filter and memorize the Conuter_Up in the Top_Counter */
      Mem_Top_CounterX8 = (((U16)7 * Mem_Top_CounterX8) + (U16)8 * Sensor_Counter_Up) >>3;
      Mem_Top_Counter = Mem_Top_CounterX8 >> 3;

      Sensor_Counter_Up = 0; /* reset the Counter_Up */

      /* Get the status of the 3 Hall sensors */
      /* Hall_Sector = PIND7.PIND6.0.0.0.PINB2.0 */
//      Mem_Hall_Sector = ( PIND & ( (1<<PIND7)|(1<<PIND6) ) ) | (PINB & (1<<PINB2)); //MC200
      Mem_Hall_Sector = ( PIND & ( (1<<PIND7)|(1<<PIND5) ) ) | (PINC & (1<<PINC6)); //MC100

      /* arm the Delay_For_Change */
      Delay_For_Change = Mem_Top_Counter - ADVANCE;

   } /* else false interrupt */

}

/* -------------- COMPARATOR 0 - INTERRUPT -----------------*/
#ifdef __ICCAVR__ // IAR C Compiler
#pragma vector = ANACOMP_0_vect
__interrupt void Hall_A(void)
#endif
#ifdef __GNUC__  // GNU C Compiler
ISR(ANALOG_COMP_0_vect)
#endif
{
   Do_Sensor_Interrupt();
   if (Hall_Event_Count-- == 0)
   {
      Hall_Event_Count = 7;
      if (Permanent_Clock >= Mem_Hall_Time)
      {
         Hall_Period = Permanent_Clock - Mem_Hall_Time;
      }
      else
      {
         Hall_Period = 65535 - Mem_Hall_Time + Permanent_Clock ;
      }
      Mem_Hall_Time = Permanent_Clock;
   }
}

/* -------------- COMPARATOR 1 - INTERRUPT -----------------*/
#ifdef __ICCAVR__ // IAR C Compiler
#pragma vector = ANACOMP_1_vect
__interrupt void Hall_B(void)
#endif
#ifdef __GNUC__  // GNU C Compiler
ISR(ANALOG_COMP_1_vect)
#endif
{
   Do_Sensor_Interrupt();
   Start_ADC();
}

/* -------------- COMPARATOR 2 - INTERRUPT -----------------*/
#ifdef __ICCAVR__ // IAR C Compiler
#pragma vector = ANACOMP_2_vect
__interrupt void Hall_C(void)
#endif
#ifdef __GNUC__  // GNU C Compiler
ISR(ANALOG_COMP_2_vect)
#endif
{
   Do_Sensor_Interrupt();
}


void svpwm_init(void)
{
   Sensor_Counter_Up = 4;
   Mem_Top_Counter = 200;
   Top_Counter = Mem_Top_Counter+1;
   Delay_For_Change = 200;
   Hall_Sector = Mem_Hall_Sector ;
}

U16 get_Hall_Period(void)
{
   U16 value_to_return;

   Disable_interrupt();
   value_to_return = Hall_Period;
   Enable_interrupt();
   return value_to_return;
}

void store_new_amplitude(U16 new_val)
{
   Disable_interrupt();
   Amplitude = new_val;
   Enable_interrupt();
}

---