如何在 MCSDK 5.x 中增加絕對位置編碼器

當導體通電後放入磁場中，由於磁場的作用導體上流動的電荷就會因為受到由磁場感應產生的洛倫茲力而發生流通路徑的偏移，這樣在垂直於電流方向上如果去測量的Y1，Y2點電壓，將產生電勢差，這個就是所說的霍爾效應；如果使用ADC對Y1，Y2點進行測量標定即可以反推磁場變化；

打開有絕對位置編碼器的電機可以看到轉子上有一個同步旋轉的磁石，同時有檢測晶片用於絕對角度輸出，絕對位置編碼器晶片輸出PWM波或者內部做通訊輸出，單片機可以讀取相關信息得到到當前轉子的角度。

絕對編碼器晶片角度信息讀取

2. 角度以及速度標定

在程序中需要量化為Q16格式後得到電角度範圍[−180°~180°]；即[-32768~32767]

2.2 速度計算

3. 編碼器SPI輸出模式

3.1 CubeMx配置

3.2 重新生成代碼

3.3 編寫SPI讀取代碼

#define ENCODER_SPI_MAX 16384 
#define POLES 11 
/* 
* Calculate electrical angle based on motor, data in Q16 format 
* 16384 stand for 360degree(65536)*motor poles 
*/ 
SPI_EIAngle = (int16_t)((65536*SPI_Angle_Digital*POLES)/ENCODER_SPI_MAX)%65536;​

3.4 創建絕對值編碼器的結構體

/**   
  * @brief Abs encoder component parameters definition   
  */   
typedef struc​
{        
        SpeednPosFdbk_Handle_t _Super;        
        int16_t Encoder_EIAngle; /* Encoder final electrical angle */       
        int16_t Encoder_MecAngle; /* Encoder final mechanical angle */               
        
        int16_t Encoder_AngleD_Pre; /* Encoder previous digital angle */        
        int16_t Encoder_AngleD_Now; /* Encoder present/now digital angle */        
        int16_t Encoder_Speed_RPM; /* Speed uint in RPM */        
        int16_t Encoder_Average_Speed_RPM; /* Average speed unit in RPM*/               
        
        bool SensorIsReliable;        
        uint8_t mode;               
        
        int32_t Circle_Counter; /* Count the circle for motro run */        
        bool Middle_Flag; /* Middle flag, 1--arrived pass middle*/        

} Abs_Encoder_Handle_t;

3.5 絕對值編碼器函數編寫

參照encoder_speed_pos_fdbk.c文件編寫對應的絕對值編碼器的代碼，接口函數對應起來，這樣可以有效的集成擴展, 分別定義：

3.6 添加絕對位置編碼器代碼

3.6.1 絕對位置編碼器速度計算

參照2.2公式進行速度計算

/* Speed_RPM = Deta(angle)*60*f/65536 */ 
pHandle->Encoder_Speed_RPM = (int16_t)((int32_t)(pHandle-
>Encoder_AngleD_Now - pHandle->Encoder_AngleD_Pre * 60 * SPEED_LOOP_FREQUENCY_HZ/65536);​

3.6.2 添加編碼器初始化代碼

Abs_Encoder_Init(&Abs_Encoder_M1); // Initial absolute encoder sensor 
Abs_Encoder_M1.mode = SENSOR_SPI;​

3.6.3 角度以及速度的代碼添加

中頻任務TSK_MediumFrequencyTaskM1中加入絕對編碼器的角度以及速度計算函數

Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux );​

/* DAC output*/ 
extern UI_Handle_t * pDAC; 
static int16_t temp1; 
static int16_t temp2; 

temp1 = FOCVars[M1].hElAngle; 
temp2 = Abs_Encoder_M1.Encoder_EIAngle; 

DAC_SetUserChannelValue(pDAC, DAC_USER1, temp1);
DAC_SetUserChannelValue(pDAC, DAC_USER2, temp2);​

程序修改完成後編譯下載，讓電機工作在無傳感閉環模式，示波器連接DAC兩路輸出。

#if defined(ABS_ENCODER_MODE) 
      Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux ); 
      IsSpeedReliable = 1; 
#else 
      Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux ); 
      IsSpeedReliable = STO_PLL_CalcAvrgMecSpeedUnit( &STO_PLL_M1, &wAux ); 
#endif​

修改 mc_task.c 中 TSK_HighFrequencyTask() 函數，只保留

MC_FOC_DURATION這個判斷，其他都可以屏蔽掉。

hFOCreturn = FOC_CurrController(M1); 
if(hFOCreturn == MC_FOC_DURATION) 
{    
    STM_FaultProcessing(&STM[M1], MC_FOC_DURATION, 0); 
 }​

3.8.3 修改速度傳感器為絕對編碼傳感器 

case CLEAR:   
  FOCVars[M1].bDriveInput = INTERNAL;   
  STC_SetSpeedSensor( pSTC[M1], &Abs_Encoder_M1._Super );   
  
  if ( STM_NextState( &STM[M1], START ) == true )   
  {     
     FOC_Clear( M1 );     
     
     R3_2_SwitchOnPWM( pwmcHandle[M1] );    
   }    
   break;​

case START:      
      STM_NextState( &STM[M1], START_RUN ); 
break ;​

同樣在狀態START_RUN下，也只需要一條跳轉語句。

case START_RUN:     
      STM_NextState( &STM[M1], RUN ); 
break ;​

MC_ProgramTorqueRampMotor1(500,1000); 
MC_StartMotor1();​

速度輸出已經在上面的中頻任務中計算得到，可以在無傳感模式下對編碼器速度輸出做調試，看輸出速度是否和無傳感匹配，如果匹配後可以轉入絕對編碼器的速度閉環控制

MC_ProgramSpeedRampMotor1 (600/6,1000); // 600RPM 
MC_StartMotor1();​

以表貼電機為例，一般的FOC電機控制是兩環控制，速度環+電流環的控制方式，速度環為外環，電流環為內環，Idref = 0（d軸電流參考為0）的控制為常見控制；參考速度環輸出參考轉矩後供後端電流環路；

如果增加位置環最簡單和直接的方式即為將速度環換為位置環，即變為位置環+電流環的方式：

位置環採用的是線性係數乘以係數直接輸出到速度的參考:

             ????=??∗(??−??)

             ??−位置環係數

             ?? −設定的位置

             ??−當前的位置

當前位置的角度數字量計算如下：

             ??=???∗2∗??∗10000+??

            ??? –絕對值編碼器轉過的整圈數

            ??−當前的機械角度的數字量

            ?? −取值3.1416

3.11.3 位置控制改進

3.11.5 增加位置控制程序

/* Get error position/Angle, unit in rad*10000 */ 
Position_GetErrorAngle(&Abs_Encoder_M1,Target_Angle); 
…… 
Position_CalcSpeedReferrence()​

如果差值縮小到一定範圍內，則進行力矩控制，在函數FOC_CalcCurrRef中進行調用。

__weak void FOC_CalcCurrRef(uint8_t bMotor) 
{ 
       if(FOCVars[bMotor].bDriveInput == INTERNAL) 
       { 
       #if defined(POSITION_CONTROL) 
             /* If in position torque mode, Iqref come from Angle PID*/ 
             if(Position_M1.Mode_Flag == P_TORQUE_MODE) 
             { 
                   FOCVars[bMotor].hTeref = Position_CalcTorqueReferrence(); 
                   FOCVars[bMotor].Iqdref.q = FOCVars[bMotor].hTeref; 
             } 
             else 
       #endif 
             { 
                   FOCVars[bMotor].hTeref = STC_CalcTorqueReference(pSTC[bMotor]); 
                   FOCVars[bMotor].Iqdref.q = FOCVars[bMotor].hTeref; 
              } 
        } 
}​

3.11.6 實際運行效果

3.12 測試說明

spi_pwm_encoder.h中提供了測試宏定義

    A，無傳感運行，用於測試絕對值編碼器，如下宏定義

//#define ABS_ENCODER_MODE 
//#define POSITION_CONTROL 
#define ENCODER_SPI_MODE 
//#define ENCODER_PWM_MODE​

    B，絕對值編碼器，SPI輸出模式的閉環運行

#define ABS_ENCODER_MODE 
//#define POSITION_CONTROL 
#define ENCODER_SPI_MODE 
//#define ENCODER_PWM_MODE​

     C，加入位置環後的三環運行

#define ABS_ENCODER_MODE 
#define POSITION_CONTROL 
#define ENCODER_SPI_MODE 
//#define ENCODER_PWM_MODE​

4.  編碼器PWM輸出模式

4.1 CubeMx配置

編輯中斷優先級

因為TIM2中斷不能妨礙電機運行，因此需要修改中斷優先級為搶占等級為3

/* In PWM mode, use Tim2 PWM capture mode to get frequency and duty cycle, 
* CCR1 value is the frequency of PWM, can be used as calibration value. 
* CCR2 value is for digital angle 
* In this demo, TIM2 work under 72MHz 
*/ 
HAL_TIM_Base_Start(&htim2); 
HAL_TIM_IC_Start(&htim2,TIM_CHANNEL_1); 
HAL_TIM_IC_Start_IT(&htim2,TIM_CHANNEL_2);​

4.3 編寫電角度轉換讀取代碼

void PWM_Encoder_EIangle(uint32_t data) 
{ 
      PWM_Angle_Digital = data; 
      if(PWM_Angle_Digital > ENCODER_PWM_MAX) 
             PWM_Angle_Digital = ENCODER_PWM_MAX; 
      PWM_EIAngle = (int16_t)((65536*PWM_Angle_Digital*POLES)/ENCODER_PWM_MAX)%65536; 
}​

在TIM2的中斷服務中讀取CCR2寄存器內容，為防止誤動作，這邊CCR1數據標定為ENCODER_PWM_MAX，可以仿真條件下讀取這個數據，如果編碼器準確1KHz PWM輸出這個數據為72000，如果為其他數據可以進行對應修改；

#define ENCODER_PWM_MAX 67039 //72000 

void TIM2_IRQHandler(void) 
{ 
    PWM_Encoder_EIangle(TIM2->CCR2); 
    Encoder_EIAngle = PWM_EIAngle; 
    Encoder_MecAngle = (int16_t)(PWM_Angle_Digital*65536/ENCODER_PWM_MAX); 
} ​

4.4 判斷絕對位置編碼器角度準確性

    同3.7所描述

4.5 絕對位置角度閉環程序修改

    同3.8，3.9，3.10的描述

4.6使用絕對編碼器做位置環

   參照 3.11 中的具體操作