STM32F做的PMSM电机控制程序 请教高手看看哪个算法有问题

fanhoufa

STM32F做的PMSM电机控制程序
signed char SectorTable[] = {-1,4,2,3,0,5,1,-1};//hc hb ha
// These are the definitions for various angles used in the SVM
// routine.  A 16-bit unsigned value is used as the angle variable.
// The SVM algorithm determines the 60 degree sector
#define        VECTOR1        0                                // 0 degrees
#define        VECTOR2        0x2aaa                        // 60 degrees
#define        VECTOR3        0x5555                        // 120 degrees
#define        VECTOR4        0x8000                        // 180 degrees
#define        VECTOR5        0xaaaa                        // 240 degrees
#define        VECTOR6        0xd555                        // 300 degrees
#define        SIXTY_DEG        0x2aaa

#define PHASE_ZERO         57344
#define PHASE_ONE        (unsigned int) ((PHASE_ZERO + 65536/6) % 65536)//2730
#define PHASE_TWO        (unsigned int) ((PHASE_ONE + 65536/6) % 65536)//13652
#define PHASE_THREE        (unsigned int )((PHASE_TWO + 65536/6) % 65536)//24574
#define PHASE_FOUR        (unsigned int )((PHASE_THREE + 65536/6) % 65536)//35496
#define PHASE_FIVE        (unsigned int) ((PHASE_FOUR + 65536/6) % 65536//46418

int   PTPER = (CKTIM/(PWM_FREQ*2) -1)>>1;

const int sinetable[] =
{0,201,401,602,803,1003,1204,1404,1605,1805,
2005,2206,2406,2606,2806,3006,3205,3405,3605,3804,4003,4202,4401,4600,
4799,4997,5195,5393,5591,5789,5986,6183,6380,6577,6773,6970,7166,7361,
7557,7752,7947,8141,8335,8529,8723,8916,9109,9302,9494,9686,9877,10068,
10259,10449,10639,10829,11018,11207,11395,11583,11771,11958,12144,
12331,12516,12701,12886,13070,13254,13437,13620,13802,13984,14165,
14346,14526,14706,14885,15063,15241,15419,15595,15772,15947,16122,
16297,16470,16643,16816,16988,17159,17330,17500,17669,17838,18006,
18173,18340,18506,18671,18835,18999,19162,19325,19487,19647,19808,
19967,20126,20284,20441,20598,20753,20908,21062,21216,21368,21520,
21671,21821,21970,22119,22266,22413,22559,22704,22848,22992,23134,
23276,23417,23557,23696,23834,23971,24107,24243,24377,24511,24644,
24776,24906,25036,25165,25293,25420,25547,25672,25796,25919,26042,
26163,26283,26403,26521,26638,26755,26870,26984,27098,27210,27321,
27431,27541,27649,27756,27862,27967,28071,28174,28276,28377};

void SVM2(int volts, unsigned int angle)
{
// These variables hold the normalized sector angles used to find
// t1, t2.
unsigned int angle1, angle2;

// These variables hold the space vector times.
unsigned int half_t0,t1,t2;//tpwm;

// Calculate the total PWM count period, which is twice the value
// in the PTPER register.
//tpwm = PTPER << 1;//PTPER = (FCY/FPWM - 1) >> 1;

// Limit volts input to avoid overmodulation.
//if(volts > VOLTS_LIMIT) volts = VOLTS_LIMIT;

if(angle < VECTOR2)
        {
        angle2 = angle - VECTOR1;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];

        // Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 1  (0 - 59 degrees)
         PhaseADuty = t1 + t2 + half_t0;
         PhaseBDuty = t2 + half_t0;
         PhaseCDuty = half_t0;
        }

else if(angle < VECTOR3)
        {
        angle2 = angle - VECTOR2;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];

        // Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 2  (60 - 119 degrees)
         PhaseADuty = t1 + half_t0;
         PhaseBDuty = t1 + t2 + half_t0;
         PhaseCDuty = half_t0;
        }

else if(angle < VECTOR4)
        {
        angle2 = angle - VECTOR3;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];

        // Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 3  (120 - 179 degrees)
         PhaseADuty = half_t0;
         PhaseBDuty = t1 + t2 + half_t0;
         PhaseCDuty = t2 + half_t0;
        }

else if(angle < VECTOR5)               
        {
        angle2 = angle - VECTOR4;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];
// Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 4  (180 - 239 degrees)
         PhaseADuty = half_t0;
         PhaseBDuty = t1 + half_t0;
         PhaseCDuty = t1 + t2 + half_t0;
        }

else if(angle < VECTOR6)
        {
        angle2 = angle - VECTOR5;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];

        // Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 5  (240 - 299 degrees)
         PhaseADuty = t2 + half_t0;
         PhaseBDuty = half_t0;
         PhaseCDuty = t1 + t2 + half_t0;
        }

else
        {
        angle2 = angle - VECTOR6;                // Reference SVM angle to the current
                                    // sector
        angle1 = SIXTY_DEG - angle2;        // Calculate second angle referenced to
                                    // sector

        t1 = sinetable[(unsigned char)(angle1 >> 6)];        // Look up values from
                                                    // table.
        t2 = sinetable[(unsigned char)(angle2 >> 6)];

        // Scale t1 to by the volts variable.
        t1 = ((long)t1*(long)volts) >> 15;
        // Scale t1 for the duty cycle range.
        t1 = ((long)t1*(long)T_SQRT3) >> 15;
        // Scale t2 time
        t2 = ((long)t2*(long)volts) >> 15;
        t2 = ((long)t2*(long)T) >> 15;

        half_t0 = (T - t1 - t2) >> 1;                // Calculate half_t0 null time from
                                            // period and t1,t2
       
        // Calculate duty cycles for Sector 6  ( 300 - 359 degrees )
         PhaseADuty = t1 + t2 + half_t0;
         PhaseBDuty = half_t0;
         PhaseCDuty = t1 + half_t0;
        }
        TIM1->CCR1 = PhaseADuty;
         TIM1->CCR2 = PhaseBDuty;
        TIM1->CCR3 = PhaseCDuty;
            
}                        // end SVM()

void ForceCommutation(void)
{
        HALL.HB = GPIO_ReadInputDataBit(HAPORT,HAPIN);
          HALL.HA = GPIO_ReadInputDataBit(HBPORT,HBPIN);
          HALL.HC = GPIO_ReadInputDataBit(HCPORT,HCPIN);
        //添加中断处理程序   
        PingPang = 1;
       // EXTI_ClearITPendingBit(EXTI_Line6);
        HALL.HallValue = HALL.HA|HALL.HB<<1|HALL.HC<<2;
        HALL.Sector = SectorTable[HALL.HallValue];        // Get Sector from table
        if (HALL.Sector != -1)        // If the sector is invalid don't do anything
        {
                // Depending on the required direction, a new phase is fetched
                if (Required_Direction == CW)
                {
                        // Motor is required to run forward, so read directly the table
                        Phase = PhaseValues[HALL.Sector];
                }
                else
                {
                        // Motor is required to run reverse, so calculate new phase and
            // add offset to compensate asymmetries
                        Phase = PhaseValues[(HALL.Sector + 3) % 6] + PhaseOffset;
                }
               
        }
        return;
}

void TIM1_UP_IRQHandler(void)
{
    TIM1->SR = (u16)~TIM_FLAG_Update;//clear flag
if(MotorStat == MotorRun)
          {
                if (Required_Direction == CW)
                {
                        if (Current_Direction == CW)
                               Phase += PhaseInc;  
                   //     if(Phase>120) Phase = 0;   
                  //      if(Phase<=0) Phase = 0;
                        // Increment Phase if CW to generate the
                                          // sinewave only if both directions are equal
                        // If Required_Direction is CW (forward) POSITIVE voltage is applied
                        #ifdef PHASE_ADVANCE
                        SVM(ControlOutput, Phase + PhaseAdvance);        // PhaseAdvance addition
                                                                                                                // produces the sinewave
                                                                                                                // phase shift
                        #else
                       // SVM( ControlOutput, 50,HALL.Sector);
                       SVM2(8000, Phase);
                        #endif
                }
                else
                {
                        if (Current_Direction == CCW)       
                                Phase -= PhaseInc;      // Decrement Phase if CCW to generate
                            //   if(Phase>=120) Phase = 120;                                                 // the sinewave only if both
                                                                                // directions are equal
                        // If Required_Direction is CCW (reverse) NEGATIVE voltage is applied
                        #ifdef PHASE_ADVANCE
                        SVM(-(ControlOutput+1), Phase + PhaseAdvance);// PhaseAdvance addition
                                                                                                                  // produces the sinewave
                                                                                                                  // phase shift
                        #else
                       //   SVM( ControlOutput, Phase,HALL.Sector);
                  SVM2(-ControlOutput, Phase);
                        #endif
                }
          }   
   Tim3+=1;
   if(Tim3>14)
  {
    PositionCal();
     Tim3 = 0;
   }
     
}
//编码器是360线，四倍后是1440线。
void PositionCal(void)
  {
        static         int PrePos; //上一次的位置
        int        TempPos;//差值 = POSCNT - PREPOS
        TempPos =  TIM2->CNT - PrePos;
        PrePos = TIM2->CNT;
        if(TempPos>719)        {        TempPos -= 1440 ;        }//如果大于719则为负数
        if(TempPos<(-719))        {        TempPos += 1440 ;         }//小于-719则为正数
       
        /*if((TempPos<-50)||(TempPos>50))//
        {        TempPos = 0;       
                 PhaseInc = 0;
        
        }*/
        
          
        
        if(TempPos == 0)
          {
                    PhaseInc = 0;
         
          }
        else if(TempPos<0)
          {
              
            
                PhaseInc   = (-TempPos)*5;//为正X3
                 Ptpos += (-TempPos*2);//累加位置
               //  PhaseInc = 0;
          }
        else
          {
                PhaseInc  = TempPos*5;//为正X3
                 Ptpos += (TempPos*2);//累加位置
                // PhaseInc = 8;//为正X3
               //  PhaseInc = 0;
          }
  
  }


电机输出有波形，不知道算法对不对。请高手指点一下，区间的时间计算是否正确

liuzq

fanhoufa :
你好，你的QQ是啥？跟你联系

denglu

mark

dingliming

mark

136420962

哈哈哈..看不懂

wangling0716

liuzq 发表于 2012-1-27 18:25
fanhoufa :
你好，你的QQ是啥？跟你联系

你好，我QQ是493925724，我想请教你一些关于SVPWM扇区、相位角等方面的问题，麻烦加一下我

kassandra

你这是开环标量控制的

bingxiin8

MARK, PMSM开环标量控制的

robin林

额 这是微芯的吧