2024-05-02 Share: FOC和SVPWM的C語言代碼實作

https://cloud.tencent.com/developer/article/2094752

Local Backup

SVPWM的原理解說在這裡：https://blog.csdn.net/qlexcel/article/details/74787619#comments
現在開始分析C語言的程式碼（程式碼建議複製到notepad++中查看），為方便讀者試驗，每個程式碼都是獨立的子模組，複製到工程中就可以編譯運行：

一、配置進階定時器TIM1產生6路互補PWM，附煞車保護

詳細設定程式碼如下，把下面的程式段拷貝到main.c中直接就可以輸出PWM波形（要確保BKIN下拉），方便讀者驗證：

程式碼語言： javascript

static void TIM1_GPIO_Config(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB, ENABLE);
                                
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_8 | GPIO_Pin_9| GPIO_Pin_10 | GPIO_Pin_11;  //CH1--A8   CH2--A9   CH3--A10  CH4-A11 
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;       //复用推挽输出                  CH1N-B13  CH2N-B14  CH3N-B15  BKIN-B12
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
	
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_12 | GPIO_Pin_13| GPIO_Pin_14 | GPIO_Pin_15;  
    GPIO_Init(GPIOB, &GPIO_InitStructure);                                              

    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_12;                           //BKIN-B12
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
	
    GPIO_PinLockConfig(GPIOA, GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 );    //锁住高侧IO口的配置寄存器，避免后面误修改		
}

#define CKTIM	    ((u32)72000000uL)  //主频
#define PWM_PRSC    ((u8)0)            //TIM1分频系数
#define PWM_FREQ    ((u16) 15000)      //PWM频率(Hz)
#define PWM_PERIOD  ((u16) (CKTIM / (u32)(2 * PWM_FREQ *(PWM_PRSC+1))))
#define REP_RATE    (1)                //该参数可以调整电流环的刷新频率，刷新周期：(REP_RATE + 1)/(2*PWM_FREQ) 秒
                                       //因为电流环的采样是靠TIM1来触发的
#define DEADTIME_NS	((u16)1000)         //死区时间(ns)，范围：0-3500
#define DEADTIME    (u16)((unsigned long long)CKTIM/2 * (unsigned long long)DEADTIME_NS/1000000000uL) 
	
static void TIM1_Mode_Config(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_OCInitTypeDef  TIM_OCInitStructure;
    TIM_BDTRInitTypeDef TIM1_BDTRInitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
	
    TIM_TimeBaseStructure.TIM_Period = PWM_PERIOD;                 //计数周期
    TIM_TimeBaseStructure.TIM_Prescaler = PWM_PRSC;                //分频系数
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV2;        //设置外部时钟TIM1ETR的滤波时间
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;  //中央对齐模式1，从0计数到 TIM_Period 然后开始减到0，循环
    TIM_TimeBaseStructure.TIM_RepetitionCounter = REP_RATE;        //重复计数，就是重复溢出多少次才产生一个溢出中断（产生更新事件，用来触发ADC采样）
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;              //配置为PWM模式1
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;  //使能CHx的PWM输出
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;//互补输出使能，使能CHxN的PWM输出
    TIM_OCInitStructure.TIM_Pulse = 800;                           //设置跳变值，当计数器计数到这个值时，电平发生跳变
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;      //CHx有效电平的极性为高电平(高侧)
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;    //CHxN有效电平的极性为高电平(低侧)
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;   //在空闲时CHx输出低(高侧), 调用TIM_CtrlPWMOutputs(TIM1, DISABLE)后就是空闲状态。
    TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;   //在空闲时CHxN输出高(低侧)，打开低侧管子可以用来锁电机	
	                                                               //TIM_OCIdleState 和 TIM_OCNIdleState不能同时为高
    TIM_OC1Init(TIM1, &TIM_OCInitStructure);                       //配置CH1

    TIM_OCInitStructure.TIM_Pulse = 800;                         
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);                       //配置CH2

    TIM_OCInitStructure.TIM_Pulse = 800;                         
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);                       //配置CH3

    //设置刹车特性，死区时间，锁电平，OSSI，OSSR状态和AOE（自动输出使能）
    TIM1_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable; //MOE=1且定时器不工作时，CHx和CHxN的输出状态
    TIM1_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable; //MOE=0且定时器不工作时，CHx和CHxN的输出状态(详情看用户手册，一般都是ENABLE，不用深究)
    TIM1_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_1;      //BDTR寄存器写保护等级，防止软件错误误写。
    TIM1_BDTRInitStructure.TIM_DeadTime = DEADTIME;              //设置死区时间
    TIM1_BDTRInitStructure.TIM_Break = TIM_Break_Enable;	     //使能TIM1刹车输入(BKIN),要把BKIN引脚拉低才有PWM输出
    TIM1_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;          //刹车输入(BKIN)输入高电平有效
    TIM1_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;   //刹车有效标志只能被软件清除，不能被自动清除
    TIM_BDTRConfig(TIM1, &TIM1_BDTRInitStructure);

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);	          //4个抢先级、4个子优先级	
    /* Enable the TIM1 BRK Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_BRK_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;		      
    NVIC_Init(&NVIC_InitStructure);
	
    TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);	  //使用TIM1的更新事件作为触发输出，这个输出可以触发ADC进行采样，电流环的采样
    TIM_ClearITPendingBit(TIM1, TIM_IT_Break);                //清除刹车中断，BKIN输入导致的中断
    TIM_ITConfig(TIM1, TIM_IT_Break, ENABLE);                 //使能刹车中断
    TIM_CtrlPWMOutputs(TIM1, ENABLE);                         //PWM输出使能    
    TIM_Cmd(TIM1, ENABLE);                                    //使能TIM1
}
void TIM1_PWM_Init(void)
{
    TIM1_GPIO_Config();
    TIM1_Mode_Config();
}

/*******************************************************************************
* Function Name  : TIM1_BRK_IRQHandler
* Description    : This function handles TIM1 Break interrupt request.
*******************************************************************************/
void TIM1_BRK_IRQHandler(void)
{
    //关闭IGBT，并报错
    TIM_ClearITPendingBit(TIM1, TIM_IT_Break);
}

1、配置TIM1的CH1–A8、CH2–A9、CH3–A10、CH4-A11、CH1N-B13、CH2N-B14、CH3N-B15、BKIN-B12
- BKIN作為警報訊號或煞車訊號的輸入，當偵測此訊號時，TIM1的PWM會硬體上停止輸出，即時性好，起到保護硬體電路的作用。
2.觀察SVPWM的PWM波形是對稱的：
- FOC和SVPWM的C語言代碼實作
- 剛好配置TIM1為中央對齊模式1，在上面程式碼的配置中，載波週期為15KHz，TIM_Period(ARR)=2400，CH1的TIM_Pulse(CCR)=800。所採用的PWM1模式，即CNT小於CCR時，輸出有效電平，大於CCR小於ARR時，輸出無效電平，又配置CHx的有效電平為高電平，CHxN的有效電平為高電平，則可以得到下面的PWM波形：
- FOC和SVPWM的C語言代碼實作
- 如果CHxN的有效電平是低電平，則輸出的CHx和CHxN的波形是相同的。（可能CHx和CHxN有效電平的叫法相反）
3.配置CHx和CHxN空閒時的電平，呼叫TIM_CtrlPWMOutputs(TIM1, DISABLE)後，就進入空閒狀態了，高側沒什麼用，讓空閒時低側的管子導通，可以使相線連在一起，起到鎖定的作用。
4.改變REP_RATE 的值，可以更改TRGO訊號輸出的頻率。因為相電流取樣由TIM1的TRGO訊號觸發，故更改REP_RATE可以調整電流環的計算頻率(每次相電流取樣後，會進行一次FOC運算)。取樣頻率關係：(2*PWM_FREQ)/(REP_RATE + 1)，如：當PWM_FREQ=15KHz，REP_RATE=0，則取樣頻率為30KHz。
5.TIM_OSSRState和TIM_OSSIState直接Enable就可以了，詳情可以去看使用手冊。
6.使用TIM_SelectOutputTrigger(TIM1, TIM_TRGOSource_Update);函數設定TRGO訊號的產生來源，TIM_TRGOSource_Update參數代表TIM1產生一次更新事件，就輸出一次TRGO訊號。 TRGO訊號用來觸發相電流的取樣。
當然也可以使用TIM1的CH4來觸發相電流取樣，參數為TIM_TRGOSource_OC4Ref ，再開啟CH4，並配置CH4的比較值，例如配置比較值為PWM_PERIOD-5。這樣當CNT計數到PWM_PERIOD-5時就會觸發相電流的ADC取樣，此方法比較靈活，可以合理設定CH4的比較值，讓相電流取樣點避開開關雜訊。
FOC和SVPWM的C語言代碼實作

二、配置雙ADC模式和規則組、注入組，其中註入組由TIM1的TRGO觸發

程式碼語言： javascript

//A相电流采样
#define PHASE_A_ADC_CHANNEL               ADC_Channel_11
#define PHASE_A_GPIO_PORT                 GPIOC
#define PHASE_A_GPIO_PIN                  GPIO_Pin_1
//B相电流采样
#define PHASE_B_ADC_CHANNEL               ADC_Channel_10
#define PHASE_B_GPIO_PORT                 GPIOC
#define PHASE_B_GPIO_PIN                  GPIO_Pin_0
//读取散热器温度(过热保护)
#define TEMP_FDBK_CHANNEL                 ADC_Channel_13
#define TEMP_FDBK_CHANNEL_GPIO_PORT       GPIOC
#define TEMP_FDBK_CHANNEL_GPIO_PIN        GPIO_Pin_3
//读取总线电压值(过压、欠压保护)
#define BUS_VOLT_FDBK_CHANNEL             ADC_Channel_14
#define BUS_VOLT_FDBK_CHANNEL_GPIO_PORT   GPIOC
#define BUS_VOLT_FDBK_CHANNEL_GPIO_PIN    GPIO_Pin_4
//读取电位器值，可以用来调速等
#define POT1_VOLT_FDBK_CHANNEL             ADC_Channel_12
#define POT1_VOLT_FDBK_CHANNEL_GPIO_PORT   GPIOC
#define POT1_VOLT_FDBK_CHANNEL_GPIO_PIN    GPIO_Pin_2
//读取母线电流值(过流保护)
#define BUS_SHUNT_CURR_CHANNEL             ADC_Channel_15
#define BUS_SHUNT_CURR_CHANNEL_GPIO_PORT   GPIOC
#define BUS_SHUNT_CURR_CHANNEL_GPIO_PIN    GPIO_Pin_5
//读取刹车电阻电流(刹车电阻过流保护)
#define BRK_SHUNT_CURR_CHANNEL             ADC_Channel_7
#define BRK_SHUNT_CURR_CHANNEL_GPIO_PORT   GPIOA
#define BRK_SHUNT_CURR_CHANNEL_GPIO_PIN    GPIO_Pin_7
//备用通道
#define AIN0_VOLT_FDBK_CHANNEL             ADC_Channel_8
#define AIN0_VOLT_FDBK_CHANNEL_GPIO_PORT   GPIOB
#define AIN0_VOLT_FDBK_CHANNEL_GPIO_PIN    GPIO_Pin_0
//备用导通
#define AIN1_VOLT_FDBK_CHANNEL             ADC_Channel_9
#define AIN1_VOLT_FDBK_CHANNEL_GPIO_PORT   GPIOB
#define AIN1_VOLT_FDBK_CHANNEL_GPIO_PIN    GPIO_Pin_1

#define   ADC1_DR_Address    ((uint32_t)0x4001244C)       //ADC数据寄存器地址
#define   BufferLenght       36    
volatile u32  ADC_DualConvertedValueTab[BufferLenght];
volatile u16  ADC1_RegularConvertedValueTab[BufferLenght];
volatile u16  ADC2_RegularConvertedValueTab[BufferLenght];
static u16 hPhaseAOffset;
static u16 hPhaseBOffset;

void ADC_DMA_Init(void)
{
    u8 bIndex;
	
    GPIO_InitTypeDef GPIO_InitStructure;
    ADC_InitTypeDef ADC_InitStructure;
    DMA_InitTypeDef   DMA_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
	
    RCC_ADCCLKConfig(RCC_PCLK2_Div6);                        //ADCCLK = PCLK2/6=72M/6=12MHz，ADC最大频率不能超过14MHz
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);       //DMA1
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA |
                           RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOE, ENABLE);	

    GPIO_StructInit(&GPIO_InitStructure);                         //Fills each GPIO_InitStruct member with its default value    
    GPIO_InitStructure.GPIO_Pin = PHASE_A_GPIO_PIN;     
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(PHASE_A_GPIO_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = PHASE_B_GPIO_PIN;      
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(PHASE_B_GPIO_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = TEMP_FDBK_CHANNEL_GPIO_PIN;          
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(TEMP_FDBK_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = BUS_VOLT_FDBK_CHANNEL_GPIO_PIN;      
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(BUS_VOLT_FDBK_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = POT1_VOLT_FDBK_CHANNEL_GPIO_PIN;     
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(POT1_VOLT_FDBK_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = BUS_SHUNT_CURR_CHANNEL_GPIO_PIN;     
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(BUS_SHUNT_CURR_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = BRK_SHUNT_CURR_CHANNEL_GPIO_PIN;     
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(BRK_SHUNT_CURR_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = AIN0_VOLT_FDBK_CHANNEL_GPIO_PIN | AIN1_VOLT_FDBK_CHANNEL_GPIO_PIN;  
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(AIN0_VOLT_FDBK_CHANNEL_GPIO_PORT, &GPIO_InitStructure);

    //设置DMA1，用于自动存储ADC1和ADC2规则通道的转换值
    DMA_DeInit(DMA1_Channel1);
    DMA_StructInit(&DMA_InitStructure);
    DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;                  //ADC数据寄存器地址(源地址)
    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ADC_DualConvertedValueTab;  //转换值存储地址(目标地址)
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;                           //从外设到内存
    DMA_InitStructure.DMA_BufferSize = BufferLenght;                             //传输大小
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;             //外设地址不增
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;                      //内存地址自增
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;      //外设数据单位(每次传输32位)
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;              //内存数据单位(每次传输32位)
    DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;                              //循环模式
    DMA_InitStructure.DMA_Priority = DMA_Priority_High;                          //本DMA通道优先级(用了多个通道时，本参数才有效果)
    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;                                 //没有使用内存到内存的传输
    DMA_Init(DMA1_Channel1, &DMA_InitStructure);
	
    DMA_ClearITPendingBit(DMA1_IT_TC1);                //清除通道1传输完成中断          
    DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE);    //打开通道1传输完成中断	
    DMA_Cmd(DMA1_Channel1, ENABLE);                    //使能DMA1

/****使用双ADC模式，ADC1为主，ADC2为从。当ADC转换配置成由外部事件触发时，用户必须设置成仅触发主ADC，从ADC设置成软件触发，这样可以防止意外的触发从转换。
但是，主和从ADC的外部触发必须同时被激活，要调用 ADC_ExternalTrigConvCmd(ADC2, ENABLE);//ADC2外部触发使能****/
    ADC_DeInit(ADC1);
    ADC_DeInit(ADC2);
    ADC_StructInit(&ADC_InitStructure);
    ADC_InitStructure.ADC_Mode = ADC_Mode_RegInjecSimult;  //ADC1工作在混合同步规则及注入模式
    ADC_InitStructure.ADC_ScanConvMode = ENABLE;           //轮流采集各个通道的值
    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;     //连续转换模式，触发后就会一直转换
    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //转换触发信号选择，使用一个软件信号触发ADC1
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Left;  //数据左对齐，ADC是12位，要存到DR寄存器的高16位或低16位，就有左右对齐问题，决定了高4位无效或低4位无效
    ADC_InitStructure.ADC_NbrOfChannel = 3;	               //要进行ADC转换的通道数：BUS_SHUNT(母线电压)+BREAK_SHUNT(刹车电阻电流)+Chip Temp(MCU温度)
    ADC_Init(ADC1, &ADC_InitStructure);

    ADC_DMACmd(ADC1, ENABLE);                              //使能ADC1的DMA

    ADC_StructInit(&ADC_InitStructure);
    ADC_InitStructure.ADC_Mode = ADC_Mode_RegInjecSimult;  //ADC2工作在混合同步规则及注入模式
    ADC_InitStructure.ADC_ScanConvMode = ENABLE;           
    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;     //连续转换模式，触发后就会一直转换
    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;  //转换触发信号选择，由软件给信号触发，双ADC模式的从ADC必须设置为软件触发
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Left;
    ADC_InitStructure.ADC_NbrOfChannel = 3;                //要进行ADC转换的通道数：POT1(电位器)+AIN0(备用)+AIN1(备用)
    ADC_Init(ADC2, &ADC_InitStructure);
    ADC_ExternalTrigConvCmd(ADC2, ENABLE);                 //ADC2外部触发使能，双ADC模式的从ADC必须要用这条语句
	
    ADC_RegularChannelConfig(ADC1, ADC_Channel_16, 1, ADC_SampleTime_239Cycles5);           //MCU温度
    ADC_RegularChannelConfig(ADC1, BRK_SHUNT_CURR_CHANNEL, 2, ADC_SampleTime_239Cycles5);   //刹车电阻电流
    ADC_RegularChannelConfig(ADC1, BUS_SHUNT_CURR_CHANNEL, 3, ADC_SampleTime_239Cycles5);   //母线电流
    ADC_RegularChannelConfig(ADC2, POT1_VOLT_FDBK_CHANNEL, 1, ADC_SampleTime_239Cycles5);   //电位器
    ADC_RegularChannelConfig(ADC2, AIN0_VOLT_FDBK_CHANNEL, 2, ADC_SampleTime_239Cycles5);   //备用
    ADC_RegularChannelConfig(ADC2, AIN1_VOLT_FDBK_CHANNEL, 3, ADC_SampleTime_239Cycles5);	//备用 
	
    ADC_Cmd(ADC1, ENABLE);	                           //ADC1使能
    ADC_TempSensorVrefintCmd(ENABLE);                  //开启MCU内存温度传感器及Vref通道
	
    ADC_ResetCalibration(ADC1);                        //复位校准寄存器
    while(ADC_GetResetCalibrationStatus(ADC1));       //等待校准寄存器复位完成
    ADC_StartCalibration(ADC1);                        //ADC1开始校准
    while(ADC_GetCalibrationStatus(ADC1));            //等待校准完成

    ADC_Cmd(ADC2, ENABLE);                             //ADC2使能
    ADC_ResetCalibration(ADC2);                        //复位校准寄存器
    while(ADC_GetResetCalibrationStatus(ADC2));       //等待校准寄存器复位完成
    ADC_StartCalibration(ADC2);                        //ADC2开始校准  
    while(ADC_GetCalibrationStatus(ADC2));	           //等待校准完成

/**** 获取A、B相零电流值，下面是临时配置 ****/
    ADC_InjectedSequencerLengthConfig(ADC1,2);         //设置ADC1注入组通道数量
    ADC_ITConfig(ADC1, ADC_IT_JEOC, DISABLE);          //关闭注入组转换完成中断
    hPhaseAOffset=0;                                   //A相零电流值
    hPhaseBOffset=0;                                   //B相零电流值
    ADC_ExternalTrigInjectedConvConfig(ADC1, ADC_ExternalTrigInjecConv_None);        //ADC1注入组转换的触发信号选择,注入组转换由软件触发
    ADC_ExternalTrigInjectedConvCmd(ADC1,ENABLE);                                    //使能外部信号触发注入组转换的功能
    ADC_InjectedChannelConfig(ADC1, PHASE_A_ADC_CHANNEL,1,ADC_SampleTime_7Cycles5);  //配置ADC1的注入组通道，设置它们的转化顺序和采样时间
    ADC_InjectedChannelConfig(ADC1, PHASE_B_ADC_CHANNEL,2,ADC_SampleTime_7Cycles5);  //A相电流和B相电流

    ADC_ClearFlag(ADC1, ADC_FLAG_JEOC);                    //清除注入组转换完成中断标志
    ADC_SoftwareStartInjectedConvCmd(ADC1,ENABLE);         //给一个软件触发信号，开始注入组转换

    for(bIndex=16; bIndex !=0; bIndex--)
    {
        while(!ADC_GetFlagStatus(ADC1,ADC_FLAG_JEOC)) { }  //等待注入组转换完成  

        //求Q1.15格式的零电流值，16个（零电流值/8）的累加，把最高符号位溢出。
        hPhaseAOffset += (ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1)>>3);  //注入组左对齐，数据要右移3位才是真实数据
        hPhaseBOffset += (ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2)>>3);
        ADC_ClearFlag(ADC1, ADC_FLAG_JEOC);                                                //清除注入组转换完成中断标志 
        ADC_SoftwareStartInjectedConvCmd(ADC1,ENABLE);                                     //给一个软件触发信号，开始注入组转换
    }
/**** 获取A、B相零电流值的临时配置使用结束，下面恢复ADC1的正常配置 ****/

    ADC_InjectedChannelConfig(ADC1, PHASE_A_ADC_CHANNEL,  1, ADC_SampleTime_7Cycles5);     //A相电流
    ADC_InjectedChannelConfig(ADC1, BUS_VOLT_FDBK_CHANNEL,2, ADC_SampleTime_7Cycles5);     //母线电压值
    ADC_ExternalTrigInjectedConvConfig(ADC1, ADC_ExternalTrigInjecConv_T1_TRGO);           //ADC1注入组转换的触发信号选择,注入组转换由TIM1的TRGO触发
    ADC_ITConfig(ADC1, ADC_IT_JEOC, ENABLE);                                               //这里才能打开注入组转换完成中断	

    ADC_InjectedSequencerLengthConfig(ADC2,2);        //设置ADC2注入组通道数量
    ADC_InjectedChannelConfig(ADC2, PHASE_B_ADC_CHANNEL, 1,ADC_SampleTime_7Cycles5);       //B相电流                    
    ADC_InjectedChannelConfig(ADC2, TEMP_FDBK_CHANNEL,   2,ADC_SampleTime_7Cycles5);       //散热器温度                      
    ADC_ExternalTrigInjectedConvCmd(ADC2,ENABLE);	  //使能外部信号触发注入组转换的功能
	
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);	  //4个抢先级、4个子优先级

    NVIC_InitStructure.NVIC_IRQChannel = ADC1_2_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

//    ADC_ExternalTrigConvCmd(ADC1, ENABLE);   //ADC1外部触发使能，如果ADC的触发信号是外部就要调用
//    ADC_ExternalTrigConvCmd(ADC2, ENABLE);   //ADC2外部触发使能
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);    //给主ADC一个软件触发信号，之后ADC就会一直转换下去
}

u16 h_ADCBusvolt;
u16 h_ADCTemp;
u16 h_ADCPhase_A;
u16 h_ADCPhase_B;
void ADC1_2_IRQHandler(void)		//AD中断有三种情况：AD_EOC、AD_JEOC、AD_AWD
{	
	if((ADC1->SR & ADC_FLAG_JEOC) == ADC_FLAG_JEOC) 
	{
		ADC1->SR = ~(u32)ADC_FLAG_JEOC;           //清除注入组转换完成中断标志           
		
		//获取散热器温度和母线电压值，做出相应的处理
		h_ADCTemp=ADC_GetInjectedConversionValue(ADC2,ADC_InjectedChannel_2);     //散热器温度
		h_ADCBusvolt=ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2);  //母线电压
		
		//获取两相电流值，进行FOC运算
		h_ADCPhase_A=ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1);     //散热器温度
		h_ADCPhase_B=ADC_GetInjectedConversionValue(ADC2,ADC_InjectedChannel_1);  //母线电压		
	}
}

void DMA1_Channel1_IRQHandler(void)
{
	u8 i,j=0;

	if(DMA_GetITStatus(DMA1_IT_TC1))
	{
		DMA_ClearITPendingBit(DMA1_IT_GL1);    //清除DMA通道1传输完成中断
		for(i=0;i<BufferLenght;i++)
		{
			ADC1_RegularConvertedValueTab[j++] = (uint16_t)(ADC_DualConvertedValueTab[i]>>4);   //ADC1规则组左对齐，要右移4位
		}
		//ADC1_RegularConvertedValueTab[0]是MCU温度，ADC1_RegularConvertedValueTab[1]是刹车电阻电流，ADC1_RegularConvertedValueTab[2]是母线电流		
		j = 0;

		for(i=0;i<BufferLenght;i++)
		{
			ADC2_RegularConvertedValueTab[j++] = (uint16_t)(ADC_DualConvertedValueTab[i] >> 20);//ADC2规则组左对齐，要右移20位
		}
		//ADC2_RegularConvertedValueTab[0]是电位器，ADC2_RegularConvertedValueTab[1]是备用，ADC2_RegularConvertedValueTab[2]是备用	
	}
}

1.stm32的ADC轉換速度為1MHz，精度為12位元。採樣時間可設定（1.5到239.5個週期），最小採樣時間107ns。使用雙ADC模式，同時觸發ADC1、ADC2擷取馬達的兩相電流，可確保擷取的兩相電流值時間誤差最小。配置ADC1為主，ADC2為從，用ADC1觸發ADC2。
2.在眾多的ADC取樣通道中，A相、B相、母線電壓值、散熱器溫度是對即時性要求比較高的，於是把他們配置成注入組通道，其餘的配置成規則組。（注入組與規則組的關係和main中的while循環與中斷類似，當注入組被觸發時會打斷規則組的ADC轉換，優先轉換注入組的通道，當注入組轉換完成，規則組才繼續轉換）
3.在雙ADC模式下，DR暫存器的高16位元儲存了ADC2的轉換數據，低16位元儲存了ADC1的轉換數據：
- FOC和SVPWM的C語言代碼實作
4.左右對齊的問題，ADC的轉換精度只有12位，要保存的資料寬度為16位，因此有靠左還是靠右的問題。
- FOC和SVPWM的C語言代碼實作
- 如上，注入組合規則組的左右對齊並不一樣。建議使用右對齊，直接取低12位即可。（上面的例程使用的左對齊，要做修改）
5.為了取得A、B相零電流時的值用於後面馬達運轉電流的矯正，先把注入組設定為軟體觸發，把零電流值擷取完成後，再把配置修改成用TIM1的TRGO訊號觸發。因為是雙ADC模式，只要配置ADC1的觸發訊號就可以了。
6.求Q1.15格式的零電流值，16個（零電流值/8）的累加，把最高符號位元溢出：
- 我們都知道MCU處理定點數會很快，處理浮點數比較慢，但是相電流採樣值一般都比較小，會有小數，因此我們要使用Q格式來讓浮點資料轉換為定點數，提高處理速度。
- FOC和SVPWM的C語言代碼實作
- 因此我們上面所使用的Q1.15格式（也稱為Q15），就是用15位元來表示小數部分，最高位元是符號位元。浮點數轉換為Q15，要將資料乘以2的15次方。 Q15資料轉換為浮點數，將資料除以2的15次方。
- 更多可以看這兩篇文章：文章1 文章2，不想深究的，只需要導致浮點數和Q格式數的轉換方法即可。
- 因此得到A、B相零電流的過程如下：
- 已知：暫存器中的值/4096*Vref 是實際取樣電阻上的電壓值。 hPhaseAOffset、hPhaseBOffset是16個零電流值的和。 hPhaseAOffset、hPhaseBOffset是U16類型。
- 於是A相零電流的Q15格式值為：(hPhaseAOffset/16)/4096*Vref*2^15=(((hPhaseAOffset>>4)>>12)*Vref)<<15，當Vref=2V，A相零電流的Q15格式值就剛好等於hPhaseAOffset。為方便計算，那我們就統一Vref=2了。（在程式中，Vref取多少都沒有關係，只要統一就好）

三、編碼器的配置

程式碼語言： javascript

#define U32_MAX          ((u32)4294967295uL)
#define	POLE_PAIR_NUM 	 (u8)2        //电机的极对数
#define ENCODER_PPR      (u16)(1000)  //编码器线数，即转一圈，编码器输出的脉冲数
#define ALIGNMENT_ANGLE  (u16)90      //上电时，电机转子的初始电角度，范围：0-359
#define COUNTER_RESET    (u16)((((s32)(ALIGNMENT_ANGLE)*4*ENCODER_PPR/360)-1)/POLE_PAIR_NUM)  //根据初始电角度计算TIM2_CNT值
#define ICx_FILTER       (u8) 8       // 8<-> 670nsec

void ENC_Init(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //TIM2_CH1--PA0  TIM2_CH2--PA1
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;	            //设定浮空模式
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    /* Enable the TIM2 Update Interrupt */
    NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    TIM_DeInit(TIM2);
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Prescaler = 0x0;                   //No prescaling ,可以改变编码器计数对于脉冲输入个数的倍数
	                                                             //即当TIM_Prescaler=0，输入一个脉冲，CNT增加4.当TIM_Prescaler=1，输入一个脉冲，CNT只增加2   
    TIM_TimeBaseStructure.TIM_Period = (4*ENCODER_PPR)-1;        //ARR值，当CNT增加到ARR会溢出，产生更新中断
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //向上计数
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);  //4倍计数，输入A、B信号不反相

    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
    TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;    //滤波值是1-15，看情况设定，是用来滤除编码器信号干扰
    TIM_ICInit(TIM2, &TIM_ICInitStructure);           //配置通道1的滤波值

    TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
    TIM_ICInit(TIM2, &TIM_ICInitStructure);           //配置通道2的滤波值

    TIM_ClearFlag(TIM2, TIM_FLAG_Update);
    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
    TIM2->CNT = COUNTER_RESET;                        //设定电机转子初始电角度，一般电机校零后，掉电时要把电角度保存下来，再次上电时直接使用，就不用再校零了 
    TIM_Cmd(TIM2, ENABLE);
}

void TIM2_IRQHandler(void)
{
    TIM_ClearFlag(TIM2, TIM_FLAG_Update);
    //TIM2->CNT向上溢出或向下溢出，都会触发此中断，即3999加一到0，或0减一到3999
}

/*******************************************************************************
* Description    : 返回电机转子的电角度
* Return         : Rotor electrical angle: 0 -> 0 degrees, 
*                                          S16_MAX-> 180 degrees, 
*                                          S16_MIN-> -180 degrees
*******************************************************************************/
s16 ENC_Get_Electrical_Angle(void)
{
  s32 temp;
  
  temp = (s32)(TIM_GetCounter(TIM2)) * (s32)(U32_MAX / (4*ENCODER_PPR));         
  temp *= POLE_PAIR_NUM;  
  return((s16)(temp/65536));   //s16 result
}

1.定時器的編碼器模式只用CH1、CH2，因此把編碼器的A、B訊號接在TIM2_CH1、TIM2_CH2即可。
2.編碼器的模式選擇TIM_EncoderMode_TI12，即TI1和TI2都要計數，用兩張圖就能說明白編碼器模式的原理了：
- FOC和SVPWM的C語言代碼實作
- TI1和TI2對應了編碼器的A、B訊號，第一列是指編碼器的3種模式，即只計數TI1、只計數TI2和都要計數。第二列是相對訊號的電平，例如我們討論TI1的邊緣，它的相對訊號就是TI2。
- 我們就可以來看了，先看第二行，只計數TI1訊號的模式。當它的相對訊號(即TI2)是高電位時，如果TI1來一個上升沿，那麼CNT就要向下計數(Down)，如果來一個下降沿，那麼CNT就要向上計數(Up)。示意圖如下：
- FOC和SVPWM的C語言代碼實作
- 在時刻1，當TI2為低電平時，TI1來一個上升沿，CNT向上計數1.
- 在時刻3，當TI2為高電位時，TI1來一個下降沿，CNT又向上數1.
- 同樣的道理你也可以理解只計數TI2訊號的模式，然後把這兩個模式加起來你也可以理解同時計數TI1和TI2的模式。

3、TIM_ICPolarity_Rising 表示極性不反相。 TIM_ICPolarity_falling:表示極性反向。庫函數中的配置程式碼是這樣的：

程式碼語言： javascript

  /* Set the TI1 and the TI2 Polarities */
  tmpccer &= (uint16_t)(((uint16_t)~((uint16_t)TIM_CCER_CC1P)) & ((uint16_t)~((uint16_t)TIM_CCER_CC2P)));
  tmpccer |= (uint16_t)(TIM_IC1Polarity | (uint16_t)(TIM_IC2Polarity << (uint16_t)4));
  /* Write to TIMx CCER */
  TIMx->CCER = tmpccer;

配置選項有3種：

#define  TIM_ICPolarity_Rising             ((uint16_t)0x0000)
#define  TIM_ICPolarity_Falling            ((uint16_t)0x0002)
#define  TIM_ICPolarity_BothEdge           ((uint16_t)0x000A)

寄存器：
FOC和SVPWM的C語言代碼實作
實際配置的是CC1P、CC1NP、CC2P、CC2NP位，改位說明如下：
FOC和SVPWM的C語言代碼實作

4.ENC_Get_Electrical_Angle函數的問題
- 1)、首先是S16_MAX對應180度、S16_MIN對應-180的問題。這裡其實就是利用了u32和s32資料型態表示的範圍不同，而巧妙的產生了負數。我們知道u32的範圍是：0到4294967295。而s32的範圍是：-2147483648到2147483647。
  - (s32)(U32_MAX / (4*ENCODER_PPR))就是把4294967295分成4000份，如果CNT的值在0-2000，那麼得到的結果最大也就是4294967295的一半，即2147483647，這時還沒有超過。當CNT=2001，得到2148555741，超過了s32範圍，那麼會怎麼樣呢？會從s32範圍的最小值開始往上增加，就像一個環一樣，最大值和最小值之間只差1。於是2148555741超出了2147483647：2148555741-2147483647=1072094，繞了一圈後得到：-2147483648+1072094-1=-2146411555。
  - 有沒有發現資料型態的這種特性和馬達的電角度也是類似的？馬達的電角度從0度增加到180度，再增加就變成-179度，再從-179度增加到0度，完成一圈。和資料類型的：從0增加到2147483647，再增加就變成-2147483648，又從-2147483648增加到0，完成一圈。
  - FOC和SVPWM的C語言代碼實作
  - 最後再把s32的數整除65536，就可以得到s16的資料型態了。（不能移位，會把符號位也移動了）
- 2)、極對數和電角度的關係
  - 在函數還有一句：temp *= POLE_PAIR_NUM，也就是電角度要被極對數放大，這是為什麼？
  - 首先看極對數是什麼：極對數是每相勵磁繞組所含有的磁極個數。
  - 若極對數是1，即每相只有一對磁極（一對磁極=2極，兩對磁極=4極）
  - FOC和SVPWM的C語言代碼實作
  - 那麼這3個相的磁極互差120度分佈，相電流呈現正弦規律變化一次，合成電壓向量旋轉一圈，旋轉磁場也會旋轉一圈：
  - FOC和SVPWM的C語言代碼實作
  - 若極對數是2，即每相有2對磁極：
  - FOC和SVPWM的C語言代碼實作
  - 那麼這3個相的磁極互差60度分佈，相電流呈正弦規律變化一次，合成電壓向量旋轉半圈，旋轉磁場也旋轉半圈：
  - （此處差一個gif。。。有沒有誰知道上面那種gif怎麼畫的。。）
  - 因此SVPWM輸入的電角度和馬達轉子的機械角度之間就有極對數的倍數關係了：
    - 如果極對數是1，那麼SVPWM輸出的磁場旋轉一圈，馬達轉子也旋轉一圈，電角度和馬達轉子角度是一一對應的。
    - 如果極對數是2，那麼SVPWM輸出的磁場旋轉一圈，馬達轉子只旋轉半圈，電角度是馬達轉子角度的2倍。
    - 編碼器的角度反映的是馬達轉子的機械角度。
- 後話：既然1對極馬達就能轉了，幹嘛還要2對、4對呢？雖然極對數越多，轉速越慢，但是扭力可以越大。參考：https://toutiao.1688.com/article/1067976.htm

四、FOC相關變換的代碼實現

前面中的程式碼中我們用TIM1的TRGO訊號觸發ADC注入組的轉換。然後ADC的注入組轉換完成後會產生中斷，然後在中斷函數對相電流進行取樣，再經過clark變換把Ia, Ib, Ic變換成Iα, Iβ，再經過park變換，把Iα, Iβ變換成Iq , Id。
FOC和SVPWM的C語言代碼實作
取得相電流取樣值、clark變換、park變換、反park變換的函數如下：
程式碼語言： javascript

typedef struct 
{
	s16 qI_Component1;
	s16 qI_Component2;
} Curr_Components;  //电流值结构体

typedef struct 
{
  s16 qV_Component1;
  s16 qV_Component2;
} Volt_Components;

typedef struct     //电压值结构体
{
  s16 hCos;
  s16 hSin;
} Trig_Components;  //存放角度sin和cos函数值的结构体

#define S16_MAX    ((s16)32767)
#define S16_MIN    ((s16)-32768)

/**********************************************************************************************************
在注入组采样完成中断中调用，获取相电流的采样值。返回(电流采样值-零电流值)，Q15格式
**********************************************************************************************************/ 
Curr_Components GET_PHASE_CURRENTS(void)               
{
    Curr_Components Local_Stator_Currents;
    s32 wAux;

    wAux = ((ADC1->JDR1)<<1)-(s32)(hPhaseAOffset);     //把电流采样值转换为Q1.15格式，再减去零电流值
    if (wAux < S16_MIN)
        Local_Stator_Currents.qI_Component1= S16_MIN;  //下限幅
    else  if (wAux > S16_MAX)
        Local_Stator_Currents.qI_Component1= S16_MAX;  //上限幅
    else
        Local_Stator_Currents.qI_Component1= wAux;

    wAux = ((ADC2->JDR1)<<1)-(s32)(hPhaseBOffset);     //B相电流
    if (wAux < S16_MIN)
        Local_Stator_Currents.qI_Component2= S16_MIN;
    else  if (wAux > S16_MAX)
        Local_Stator_Currents.qI_Component2= S16_MAX;
    else
        Local_Stator_Currents.qI_Component2= wAux;

    return(Local_Stator_Currents);
}

#define divSQRT_3	(s16)0x49E6      //1/sqrt(3)的Q15格式，1/sqrt(3)*2^15=18918=0x49E6 

/**********************************************************************************************************
Clarke变换，输入Ia,Ib，得到Ialpha和Ibeta
**********************************************************************************************************/ 
Curr_Components Clarke(Curr_Components Curr_Input)         
{
  Curr_Components Curr_Output; 
  s32 qIa_divSQRT3_tmp;
  s32 qIb_divSQRT3_tmp;    //定义32位有符号数，用来暂存Q30格式  
  s16 qIa_divSQRT3;
  s16 qIb_divSQRT3 ;

  Curr_Output.qI_Component1 = Curr_Input.qI_Component1;     //Ialpha = Ia

  qIa_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component1;  //计算Ia/√3
  qIa_divSQRT3_tmp /=32768;                                 //两个Q15数相乘，会变成Q30，因此要右移15位，变回Q15	    
  qIb_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component2;  //计算Ib/√3
  qIb_divSQRT3_tmp /=32768;
  
  qIa_divSQRT3=((s16)(qIa_divSQRT3_tmp));	                //s32赋值给s16	 		
  qIb_divSQRT3=((s16)(qIb_divSQRT3_tmp));				
     
  Curr_Output.qI_Component2=(-(qIa_divSQRT3)-(qIb_divSQRT3)-(qIb_divSQRT3));  //Ibeta = -(2*Ib+Ia)/sqrt(3) 
  return(Curr_Output); 
}

#define SIN_MASK  0x0300
#define U0_90     0x0200
#define U90_180   0x0300
#define U180_270  0x0000
#define U270_360  0x0100
const s16 hSin_Cos_Table[256] = {\
0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,\
0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,\
0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,\
0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,\
0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,\
0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,\
0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,\
0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,\
0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,\
0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,\
0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,\
0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,\
0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,\
0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,\
0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,\
0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,\
0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,\
0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,\
0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,\
0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,\
0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,\
0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,\
0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,\
0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,\
0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,\
0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,\
0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,\
0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,\
0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,\
0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,\
0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,\
0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};	
/*******************************************************************************
* Function Name  : Trig_Functions 
* Description    : 本函数返回输入角度的cos和sin函数值
* Input          : angle in s16 format
* Output         : Cosine and Sine in s16 format
*******************************************************************************/
Trig_Components Trig_Functions(s16 hAngle)  //hAngle=0,转子电角度=0度。hAngle=S16_MAX,转子电角度=180度。hAngle=S16_MIN,转子电角度=-180度
{
  u16 hindex;
  Trig_Components Local_Components;
  
  /* 10 bit index computation  */  
  hindex = (u16)(hAngle + 32768);  
  hindex /= 64;      
  
  switch (hindex & SIN_MASK) 
  {
  case U0_90:
    Local_Components.hSin = hSin_Cos_Table[(u8)(hindex)];
    Local_Components.hCos = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U90_180:  
     Local_Components.hSin = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(hindex)];
    break;
  
  case U180_270:
     Local_Components.hSin = -hSin_Cos_Table[(u8)(hindex)];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U270_360:
     Local_Components.hSin =  -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos =  hSin_Cos_Table[(u8)(hindex)]; 
    break;
  default:
    break;
  }
  return (Local_Components);
}

Trig_Components Vector_Components;
/**********************************************************************************************************
Park变换，输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id
**********************************************************************************************************/ 
Curr_Components Park(Curr_Components Curr_Input, s16 Theta)       
{ 
  Curr_Components Curr_Output;
  s32 qId_tmp_1, qId_tmp_2;
  s32 qIq_tmp_1, qIq_tmp_2;     
  s16 qId_1, qId_2;  
  s16 qIq_1, qIq_2;  
  
  Vector_Components = Trig_Functions(Theta);                      //计算电角度的cos和sin
  
  qIq_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hCos;  //计算Ialpha*cosθ	
  qIq_tmp_1 /= 32768;
  qIq_tmp_2 = Curr_Input.qI_Component2 *Vector_Components.hSin;   //计算Ibeta*sinθ
  qIq_tmp_2 /= 32768;
 
  qIq_1 = ((s16)(qIq_tmp_1));
  qIq_2 = ((s16)(qIq_tmp_2));
  Curr_Output.qI_Component1 = ((qIq_1)-(qIq_2));	//Iq=Ialpha*cosθ- Ibeta*sinθ
  
  qId_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hSin;  //计算Ialpha*sinθ
  qId_tmp_1 /= 32768;
  qId_tmp_2 = Curr_Input.qI_Component2 * Vector_Components.hCos;  //计算Ibeta*cosθ
  qId_tmp_2 /= 32768;
  
  qId_1 = (s16)(qId_tmp_1);		 
  qId_2 = (s16)(qId_tmp_2);					
  Curr_Output.qI_Component2 = ((qId_1)+(qId_2));   //Id=Ialpha*sinθ+ Ibeta*cosθ
  
  return (Curr_Output);
}

/**********************************************************************************************************
反park变换，输入Uq、Ud得到Ualpha、Ubeta
**********************************************************************************************************/ 
Volt_Components Rev_Park(Volt_Components Volt_Input)
{ 	
  s32 qValpha_tmp1,qValpha_tmp2,qVbeta_tmp1,qVbeta_tmp2;
  s16 qValpha_1,qValpha_2,qVbeta_1,qVbeta_2;
  Volt_Components Volt_Output;
   
  qValpha_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hCos;  //Uq*cosθ
  qValpha_tmp1 /= 32768; 
  qValpha_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hSin;  //Ud*sinθ
  qValpha_tmp2 /= 32768;
		
  qValpha_1 = (s16)(qValpha_tmp1);		
  qValpha_2 = (s16)(qValpha_tmp2);			
  Volt_Output.qV_Component1 = ((qValpha_1)+(qValpha_2));             //Ualpha=Uq*cosθ+ Ud*sinθ
  
  qVbeta_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hSin;   //Uq*sinθ
  qVbeta_tmp1 /= 32768;  
  qVbeta_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hCos;   //Ud*cosθ
  qVbeta_tmp2 /= 32768;

  qVbeta_1 = (s16)(qVbeta_tmp1);				
  qVbeta_2 = (s16)(qVbeta_tmp2);  				
  Volt_Output.qV_Component2 = -(qVbeta_1)+(qVbeta_2);                //Ubeta=Ud*cosθ- Uq*sinθ
 
  return(Volt_Output);
}

1.讀取ADC注入組的轉換值：ADC1→JDR1和ADC2→JDR1。資料的對齊還是滿足下表的關係:
- FOC和SVPWM的C語言代碼實作
- 我們是注入組左對齊，因此右移3位才是真實的轉換值。電流取樣值的Q15格式為：((ADC1->JDR1<<1)>>4)/4096*Vref*2^15=((((ADC1->JDR1<<1)>>4)>>12 )*Vref)<<15=ADC1->JDR1<<1.
2.用ADC採集馬達的兩相電流，即可透過Ia+Ib+Ic=0得到第三相的電流。馬達的三相電流是在時間上互差120度，成正弦規律變化的波形。電流波形如下：
- FOC和SVPWM的C語言代碼實作
- 表示為向量：
- FOC和SVPWM的C語言代碼實作
- 我們透過clark變換，把Ia、Ib、Ic變換為Iα、Iβ：
- FOC和SVPWM的C語言代碼實作
- 變換公式：（可以當成向量分解來理解）
- FOC和SVPWM的C語言代碼實作
- 公式中有一個係數k，一般取2/3。有興趣可以去這裡了解：https://blog.csdn.net/daidi1989/article/details/89926324。帶入2/3：
- FOC和SVPWM的C語言代碼實作
- 再代入Ia+Ib+Ic=0：
- FOC和SVPWM的C語言代碼實作
- FOC和SVPWM的C語言代碼實作
- 註：ST的FOC庫，所採用的Iβ軸（即虛軸）滯後Iα90度的表示方法。與其他地方說的Iβ提前Iα90度是一樣的效果。只是表示方法不同而已，選擇了這個表示方法，後面的park變換也和提前90度的不同。
3.Trig_Functions函數，使用查表的方法計算目標角度的三角函數。速度快，精度也還可以，讀者可以自己測試精度。
4.使用park變換將電流Iα、Iβ 和轉子的電角度θ轉換為電流Iq、Id。
- FOC和SVPWM的C語言代碼實作
- 公式為：
- FOC和SVPWM的C語言代碼實作
- FOC和SVPWM的C語言代碼實作
5.反park變換就是上面park變換的逆過程，公式就不推了。
- FOC和SVPWM的C語言代碼實作

五、PID控制

程式碼語言： javascript

typedef struct
{
    s16 hKp_Gain;			   //比例系数
    u16 hKp_Divisor;		   //比例系数因子
    s16 hKi_Gain;		       //积分系数
    u16 hKi_Divisor;  	       //积分系数因子
    s16 hLower_Limit_Output;   //总输出下限
    s16 hUpper_Limit_Output;   //总输出上限
    s32 wLower_Limit_Integral; //积分项下限
    s32 wUpper_Limit_Integral; //积分项上限
    s32 wIntegral;			   //积分累积和
    s16 hKd_Gain;			   //微分系数
    u16 hKd_Divisor;		   //微分系数因子
    s32 wPreviousError;	       //上次误差
} PID_Struct_t;

/****************************** 扭矩的PID参数，即q轴 *******************************************************/
#define PID_TORQUE_REFERENCE   (s16)3000  //q轴的设定值，PID的目的就是要让测量的q轴值与设定值误差为0    
#define PID_TORQUE_KP_DEFAULT  (s16)1578  //Kp默认值
#define PID_TORQUE_KI_DEFAULT  (s16)676   //Ki默认值
#define PID_TORQUE_KD_DEFAULT  (s16)100   //Kd默认值

/****************************** 转子磁通的PID参数，即d轴 *******************************************************/
#define PID_FLUX_REFERENCE   (s16)0       //d轴的设定值
#define PID_FLUX_KP_DEFAULT  (s16)1578
#define PID_FLUX_KI_DEFAULT  (s16)676
#define PID_FLUX_KD_DEFAULT  (s16)100

/****************************** q轴和d轴PID参数的放大倍数 *******************************************************/
#define TF_KPDIV ((u16)(1024))    //因为Kp、Ki、Kd值很小，而我们需要整数计算，所以需要放大。得出计算结果之后，再缩小。
#define TF_KIDIV ((u16)(16384))
#define TF_KDDIV ((u16)(8192))

/****************************** 速度环的PID参数 *******************************************************/
#define PID_SPEED_REFERENCE_RPM   (s16)1500            //电机的设定转速
#define PID_SPEED_REFERENCE       (u16)(PID_SPEED_REFERENCE_RPM/6)  //电机转速和速度环的设定值一般都不相等，电机不同，它们的关系也不同
#define PID_SPEED_KP_DEFAULT      (s16)300
#define PID_SPEED_KI_DEFAULT      (s16)100
#define PID_SPEED_KD_DEFAULT      (s16)0000
#define NOMINAL_CURRENT           (s16)5289            //motor nominal current (0-pk)，3倍的额定电流
#define IQMAX                     NOMINAL_CURRENT	   //速度环输出最大值

/****************************** 速度环PID参数的放大倍数 *******************************************************/
#define SP_KPDIV ((u16)(16))
#define SP_KIDIV ((u16)(256))
#define SP_KDDIV ((u16)(16))

volatile s16 hTorque_Reference;   //q轴设定值
volatile s16 hFlux_Reference;     //d轴设定值
volatile s16 hSpeed_Reference;    //速度环设定值
void PID_Init (PID_Struct_t *PID_Torque, PID_Struct_t *PID_Flux, PID_Struct_t *PID_Speed)
{
    hTorque_Reference = PID_TORQUE_REFERENCE;          //q轴设定值初始化
/******************************************* 下面是控制扭矩的PID参数，即q轴大小 **************************************************************/
    PID_Torque->hKp_Gain    = PID_TORQUE_KP_DEFAULT;   //Kp参数，放大了hKp_Divisor倍。调节结果除以hKp_Divisor才是真实结果
    PID_Torque->hKp_Divisor = TF_KPDIV;                //Kp参数分数因子
    PID_Torque->hKi_Gain = PID_TORQUE_KI_DEFAULT;      //Ki参数
    PID_Torque->hKi_Divisor = TF_KIDIV;                //Ki参数分数因子
    PID_Torque->hKd_Gain = PID_TORQUE_KD_DEFAULT;      //Kd参数
    PID_Torque->hKd_Divisor = TF_KDDIV;                //Kd参数分数因子
    PID_Torque->wPreviousError = 0;                    //上次计算的误差值，用于D调节
    PID_Torque->hLower_Limit_Output=S16_MIN;           //PID输出下限幅
    PID_Torque->hUpper_Limit_Output= S16_MAX;          //PID输出上限幅
    PID_Torque->wLower_Limit_Integral = S16_MIN * TF_KIDIV;  //I调节的下限福
    PID_Torque->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;  //I调节的上限幅
    PID_Torque->wIntegral = 0;                         //I调节的结果，因为是积分，所以要一直累积
/******************************************* 上面是控制扭矩的PID参数，即q轴大小 **************************************************************/
 
    hFlux_Reference = PID_FLUX_REFERENCE;              //对于SM-PMSM电机，Id = 0
/******************************************* 下面是控制转子磁通的PID参数，即d轴大小 **************************************************************/
    PID_Flux->hKp_Gain    = PID_FLUX_KP_DEFAULT;
    PID_Flux->hKp_Divisor = TF_KPDIV;
    PID_Flux->hKi_Gain = PID_FLUX_KI_DEFAULT;
    PID_Flux->hKi_Divisor = TF_KIDIV;
    PID_Flux->hKd_Gain = PID_FLUX_KD_DEFAULT;
    PID_Flux->hKd_Divisor = TF_KDDIV;
    PID_Flux->wPreviousError = 0;
    PID_Flux->hLower_Limit_Output=S16_MIN;   
    PID_Flux->hUpper_Limit_Output= S16_MAX;   
    PID_Flux->wLower_Limit_Integral = S16_MIN * TF_KIDIV;
    PID_Flux->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;
    PID_Flux->wIntegral = 0;
/******************************************* 上面是控制转子磁通的PID参数，即d轴大小 **************************************************************/

    hSpeed_Reference = PID_SPEED_REFERENCE;
/******************************************* 下面是速度环的PID参数 **************************************************************/
    PID_Speed->hKp_Gain    = PID_SPEED_KP_DEFAULT;
    PID_Speed->hKp_Divisor = SP_KPDIV;
    PID_Speed->hKi_Gain = PID_SPEED_KI_DEFAULT;
    PID_Speed->hKi_Divisor = SP_KIDIV;
    PID_Speed->hKd_Gain = PID_SPEED_KD_DEFAULT;
    PID_Speed->hKd_Divisor = SP_KDDIV;
    PID_Speed->wPreviousError = 0;
    PID_Speed->hLower_Limit_Output= -IQMAX;   
    PID_Speed->hUpper_Limit_Output= IQMAX;   
    PID_Speed->wLower_Limit_Integral = -IQMAX * SP_KIDIV;
    PID_Speed->wUpper_Limit_Integral = IQMAX * SP_KIDIV;
    PID_Speed->wIntegral = 0;
/******************************************* 上面是速度环的PID参数 **************************************************************/
}

//#define DIFFERENTIAL_TERM_ENABLED    //不使用PID的D调节
typedef signed long long s64;
s16 PID_Regulator(s16 hReference, s16 hPresentFeedback, PID_Struct_t *PID_Struct)
{
    s32 wError, wProportional_Term,wIntegral_Term, houtput_32;
    s64 dwAux;
#ifdef DIFFERENTIAL_TERM_ENABLED                         //如果使能了D调节
	s32 wDifferential_Term;
#endif    
	
    wError= (s32)(hReference - hPresentFeedback);		 //设定值-反馈值，取得需要误差量delta_e
    wProportional_Term = PID_Struct->hKp_Gain * wError;	 //PID的P调节，即比例放大调节：wP = Kp * delta_e

    if (PID_Struct->hKi_Gain == 0)                       //下面进行PID的I调节，即误差的累积调节
    {
        PID_Struct->wIntegral = 0;                       //如果I参数=0，I调节就=0 
    }
    else
    {
        wIntegral_Term = PID_Struct->hKi_Gain * wError;		    //wI = Ki * delta_e	，本次积分项
        dwAux = PID_Struct->wIntegral + (s64)(wIntegral_Term);	//积分累积的调节量 = 以前的积分累积量 + 本次的积分项

        if (dwAux > PID_Struct->wUpper_Limit_Integral)		    //对PID的I调节做限幅
        {
            PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;	//上限
        }
        else if (dwAux < PID_Struct->wLower_Limit_Integral)				//下限
        {
            PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
        }
        else
        {
            PID_Struct->wIntegral = (s32)(dwAux);		          //不超限, 更新积分累积项为dwAux
        }
    }
#ifdef DIFFERENTIAL_TERM_ENABLED						          //如果使能了D调节
	{
	s32 wtemp;
  
	wtemp = wError - PID_Struct->wPreviousError;			      //取得上次和这次的误差之差
	wDifferential_Term = PID_Struct->hKd_Gain * wtemp;	          //D调节结果，wD = Kd * delta_d
	PID_Struct->wPreviousError = wError;    				      //更新上次误差，用于下次运算	

	}
	houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor+     //输出总的调节量 = 比例调节量/分数因子 +
                  PID_Struct->wIntegral/PID_Struct->hKi_Divisor + //				 + 积分调节量/分数因子
                  wDifferential_Term/PID_Struct->hKd_Divisor); 	  //				 + 微分调节量/分数因子

#else  	
	//把P调节和I调节结果除以分数因子再相加，得到PI控制的结果
    houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor + PID_Struct->wIntegral/PID_Struct->hKi_Divisor);       
#endif
    if (houtput_32 >= PID_Struct->hUpper_Limit_Output)	   //PI控制结果限幅
    {
        return(PID_Struct->hUpper_Limit_Output);
    }
    else if (houtput_32 < PID_Struct->hLower_Limit_Output) //下限
    {
        return(PID_Struct->hLower_Limit_Output);
    }
    else
    {
        return((s16)(houtput_32)); 						   //不超限。输出结果 houtput_32
    }
}

PID控制沒什麼好說的，網路資料很多。此處用的PID也很普通，很容易看懂。
使用了巨集定義，選擇是否使用PID的D調節。一般PI控制就已經足夠。

6.使用RevPark_Circle_Limitation函數對PID的輸出進行歸一化

看下面的FOC的處理過程：
FOC和SVPWM的C語言代碼實作
可以發現在PID計算後，要進行CircleLimitation處理，為什麼呢？因為我們需要控制旋轉磁場大小的恆定，如果d、q軸給得太大了，那麼輸出的旋轉磁場就會過調製，圓形磁場就會凸起來一塊。 PID控制器只能對單獨的d、q軸大小進行限制，可是它控制不了d、q軸合成向量的大小。例如：最大值要求限制在1，d=0.8，q=0.9，他們各自的大小都沒有超過1，可是它們的合成向量大小卻超過了1。示意圖如下，就是要用比例關係，把外圓上的d、q值等比例縮小到內圓上：
FOC和SVPWM的C語言代碼實作
程式碼語言： javascript

/**** 根据载波频率来选择调制系数，频率越大，调制系数越小（实质是控制的最大占空比） ****/
//#define MAX_MODULATION_100_PER_CENT     // up to 11.4 kHz PWM frequency 
//#define MAX_MODULATION_99_PER_CENT      // up to 11.8 kHz
//#define MAX_MODULATION_98_PER_CENT      // up to 12.2 kHz  
//#define MAX_MODULATION_97_PER_CENT      // up to 12.9 kHz  
//#define MAX_MODULATION_96_PER_CENT      // up to 14.4 kHz  
#define   MAX_MODULATION_95_PER_CENT      // up to 14.8 kHz
//#define MAX_MODULATION_94_PER_CENT      // up to 15.2 kHz  
//#define MAX_MODULATION_93_PER_CENT      // up to 16.7 kHz
//#define MAX_MODULATION_92_PER_CENT      // up to 17.1 kHz
//#define MAX_MODULATION_89_PER_CENT      // up to 17.5 kHz

/**** 以下是根据选择的调制系数，计算d、q轴合成矢量模的最大值。用宏定义，优化计算速度 ****/
#ifdef MAX_MODULATION_77_PER_CENT
#define MAX_MODULE      25230   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*77% 
#endif

#ifdef MAX_MODULATION_79_PER_CENT
#define MAX_MODULE      25885   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*79% 
#endif

#ifdef MAX_MODULATION_81_PER_CENT
#define MAX_MODULE      26541   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*81% 
#endif

#ifdef MAX_MODULATION_83_PER_CENT
#define MAX_MODULE      27196   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*83% 
#endif

#ifdef MAX_MODULATION_85_PER_CENT
#define MAX_MODULE      27851   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*85%  
#endif

#ifdef MAX_MODULATION_87_PER_CENT
#define MAX_MODULE      28507   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*87%  
#endif

#ifdef MAX_MODULATION_89_PER_CENT
#define MAX_MODULE      29162   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*89%
#endif

#ifdef MAX_MODULATION_91_PER_CENT
#define MAX_MODULE      29817   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*91%
#endif

#ifdef MAX_MODULATION_92_PER_CENT
#define MAX_MODULE      30145   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*92%
#endif

#ifdef MAX_MODULATION_93_PER_CENT
#define MAX_MODULE      30473   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*93%
#endif

#ifdef MAX_MODULATION_94_PER_CENT
#define MAX_MODULE      30800   //root(Vd^2+Vq^2) <= MAX_MODULE = 32767*94%
#endif

#ifdef MAX_MODULATION_95_PER_CENT
#define MAX_MODULE      31128   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*95%
#endif

#ifdef MAX_MODULATION_96_PER_CENT
#define MAX_MODULE      31456   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*96%
#endif

#ifdef MAX_MODULATION_97_PER_CENT
#define MAX_MODULE      31783   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*97%
#endif

#ifdef MAX_MODULATION_98_PER_CENT
#define MAX_MODULE      32111   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*98%
#endif

#ifdef MAX_MODULATION_99_PER_CENT
#define MAX_MODULE      32439   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*99%
#endif

#ifdef MAX_MODULATION_100_PER_CENT
#define MAX_MODULE      32767   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*100%
#endif

#ifdef MAX_MODULATION_77_PER_CENT
#define START_INDEX     37
static const u16 circle_limit_table[91]=     
{
 32468,32176,31893,31347,31084,30578,30334,29863,29636,29414,28984,28776,28373,
 28178,27987,27616,27436,27086,26916,26585,26425,26267,25959,25809,25518,25375,
 25098,24962,24829,24569,24442,24194,24072,23953,23719,23604,23381,23271,23056,
 22951,22848,22644,22545,22349,22254,22066,21974,21883,21704,21616,21444,21359,
 21275,21111,21030,20872,20793,20640,20564,20490,20343,20270,20128,20058,19920,
 19852,19785,19653,19587,19459,19396,19271,19209,19148,19028,18969,18852,18795,
 18738,18625,18570,18460,18406,18299,18246,18194,18091,18040,17940,17890,17792
};
#endif

#ifdef MAX_MODULATION_79_PER_CENT
#define START_INDEX     39
static const u16 circle_limit_table[89]=     
{
32489,32217,31952,31442,31195,30719,30489,30045,29830,29413,29211,28819,28629,
28442,28080,27904,27562,27396,27072,26914,26607,26457,26165,26022,25882,25608,
25475,25214,25086,24836,24715,24476,24359,24130,24019,23908,23692,23586,23378,
23276,23077,22979,22787,22692,22507,22416,22326,22150,22063,21893,21809,21645,
21564,21405,21327,21173,21097,21022,20875,20802,20659,20589,20450,20382,20247,
20181,20051,19986,19923,19797,19735,19613,19553,19434,19375,19260,19202,19090,
19034,18979,18871,18817,18711,18659,18555,18504,18403,18354,18255
};
#endif

#ifdef MAX_MODULATION_81_PER_CENT
#define START_INDEX     41
static const u16 circle_limit_table[87]=     
{
32508,32255,32008,31530,31299,30852,30636,30216,30012,29617,29426,29053,28872,
28520,28349,28015,27853,27536,27382,27081,26934,26647,26507,26234,26101,25840,
25712,25462,25340,25101,24984,24755,24643,24422,24315,24103,24000,23796,23696,
23500,23404,23216,23123,22941,22851,22763,22589,22504,22336,22253,22091,22011,
21854,21776,21624,21549,21401,21329,21186,21115,20976,20908,20773,20706,20575,
20511,20383,20320,20196,20135,20015,19955,19838,19780,19666,19609,19498,19443,
19334,19280,19175,19122,19019,18968,18867,18817,18719
};
#endif

#ifdef MAX_MODULATION_83_PER_CENT
#define START_INDEX     44
static const u16 circle_limit_table[84]=     
{
32291,32060,31613,31397,30977,30573,30377,29996,29811,29451,29276,28934,28768,
28444,28286,27978,27827,27533,27390,27110,26973,26705,26574,26318,26069,25948,
25709,25592,25363,25251,25031,24923,24711,24607,24404,24304,24107,24011,23821,
23728,23545,23456,23279,23192,23021,22854,22772,22610,22530,22374,22297,22145,
22070,21922,21850,21707,21636,21497,21429,21294,21227,21096,21032,20904,20778,
20717,20595,20534,20416,20357,20241,20184,20071,20015,19905,19851,19743,19690,
19585,19533,19431,19380,19280,19182
};
#endif

#ifdef MAX_MODULATION_85_PER_CENT
#define START_INDEX     46
static const u16 circle_limit_table[82]=     
{
32324,32109,31691,31489,31094,30715,30530,30170,29995,29654,29488,29163,29005,
28696,28397,28250,27965,27825,27552,27418,27157,26903,26779,26535,26416,26182,
26067,25842,25623,25515,25304,25201,24997,24897,24701,24605,24415,24230,24139,
23960,23872,23699,23614,23446,23282,23201,23042,22964,22810,22734,22584,22437,
22365,22223,22152,22014,21945,21811,21678,21613,21484,21421,21296,21234,21112,
21051,20932,20815,20757,20643,20587,20476,20421,20312,20205,20152,20048,19996
,19894,19844,19744,19645
};
#endif

#ifdef MAX_MODULATION_87_PER_CENT
#define START_INDEX     48
static const u16 circle_limit_table[81]=     
{
32559,32154,31764,31575,31205,31025,30674,30335,30170,29847,
29689,29381,29083,28937,28652,28375,28239,27974,27844,27589,
27342,27220,26983,26866,26637,26414,26305,26090,25984,25777,
25575,25476,25280,25184,24996,24811,24720,24542,24367,24281,
24112,24028,23864,23703,23624,23468,23391,23240,23091,23018,
22874,22803,22662,22524,22456,22322,22191,22126,21997,21934,
21809,21686,21625,21505,21446,21329,21214,21157,21045,20990,
20880,20772,20719,20613,20561,20458,20356,20306,20207,20158,
20109
};
#endif

#ifdef MAX_MODULATION_89_PER_CENT
#define START_INDEX     50
static const u16 circle_limit_table[78]=     
{
32574,32197,32014,31656,31309,31141,30811,30491,30335,30030,
29734,29589,29306,29031,28896,28632,28375,28249,28002,27881,
27644,27412,27299,27076,26858,26751,26541,26336,26235,26037,
25844,25748,25561,25378,25288,25110,24936,24851,24682,24517,
24435,24275,24118,24041,23888,23738,23664,23518,23447,23305,
23166,23097,22962,22828,22763,22633,22505,22442,22318,22196,
22135,22016,21898,21840,21726,21613,21557,21447,21338,21284,
21178,21074,21022,20919,20819,20769,20670,20573
};
#endif

#ifdef MAX_MODULATION_91_PER_CENT
#define START_INDEX     52
static const u16 circle_limit_table[76]=     
{
32588,32411,32066,31732,31569,31250,30940,30789,30492,30205,
29925,29788,29519,29258,29130,28879,28634,28395,28278,28048,
27823,27713,27497,27285,27181,26977,26777,26581,26485,26296,
26110,26019,25840,25664,25492,25407,25239,25076,24995,24835,
24679,24602,24450,24301,24155,24082,23940,23800,23731,23594,
23460,23328,23263,23135,23008,22946,22822,22701,22641,22522,
22406,22291,22234,22122,22011,21956,21848,21741,21636,21584,
21482,21380,21330,21231,21133,21037
};
#endif


#ifdef MAX_MODULATION_92_PER_CENT
#define START_INDEX     54
const u16 circle_limit_table[74]=     
{
32424,32091,31929,31611,31302,31002,30855,30568,30289,30017,
29884,29622,29368,29243,28998,28759,28526,28412,28187,27968,
27753,27648,27441,27238,27040,26942,26750,26563,26470,26288,
26110,25935,25849,25679,25513,25350,25269,25111,24955,24803,
24727,24579,24433,24361,24219,24079,23942,23874,23740,23609,
23479,23415,23289,23165,23042,22982,22863,22745,22629,22572,
22459,22347,22292,22183,22075,21970,21917,21813,21711,21610,
21561,21462,21365,21268
};
#endif

#ifdef MAX_MODULATION_93_PER_CENT
#define START_INDEX     55
const u16 circle_limit_table[73]=     
{
32437,32275,31959,31651,31353,31207,30920,30642,30371,30107,
29977,29723,29476,29234,29116,28883,28655,28433,28324,28110,
27900,27695,27594,27395,27201,27011,26917,26733,26552,26375,
26202,26116,25948,25783,25621,25541,25383,25228,25076,25001,
24854,24708,24565,24495,24356,24219,24084,24018,23887,23758,
23631,23506,23444,23322,23202,23083,23025,22909,22795,22683,
22627,22517,22409,22302,22250,22145,22042,21941,21890,21791,
21693,21596,21500
};
#endif

#ifdef MAX_MODULATION_94_PER_CENT
#define START_INDEX     56
const u16 circle_limit_table[72]=     
{
32607,32293,31988,31691,31546,31261,30984,30714,30451,30322,
30069,29822,29581,29346,29231,29004,28782,28565,28353,28249,
28044,27843,27647,27455,27360,27174,26991,26812,26724,26550,
26380,26213,26049,25968,25808,25652,25498,25347,25272,25125,
24981,24839,24699,24630,24494,24360,24228,24098,24034,23908,
23783,23660,23600,23480,23361,23245,23131,23074,22962,22851,
22742,22635,22582,22477,22373,22271,22170,22120,22021,21924,
21827,21732
};
#endif

#ifdef MAX_MODULATION_95_PER_CENT
#define START_INDEX     57
const u16 circle_limit_table[71]=     
{
32613,32310,32016,31872,31589,31314,31046,30784,30529,30404,
30158,29919,29684,29456,29343,29122,28906,28695,28488,28285,
28186,27990,27798,27610,27425,27245,27155,26980,26808,26639,
26473,26392,26230,26072,25917,25764,25614,25540,25394,25250,
25109,24970,24901,24766,24633,24501,24372,24245,24182,24058,
23936,23816,23697,23580,23522,23408,23295,23184,23075,23021,
22913,22808,22703,22600,22499,22449,22349,22251,22154,22059,
21964
};
#endif

#ifdef MAX_MODULATION_96_PER_CENT
#define START_INDEX     58
const u16 circle_limit_table[70]=     
{
32619,32472,32184,31904,31631,31365,31106,30853,30728,30484,
30246,30013,29785,29563,29345,29238,29028,28822,28620,28423,
28229,28134,27946,27762,27582,27405,27231,27061,26977,26811,
26649,26489,26332,26178,26027,25952,25804,25659,25517,25376,
25238,25103,25035,24903,24772,24644,24518,24393,24270,24210,
24090,23972,23855,23741,23627,23516,23461,23352,23244,23138,
23033,22930,22828,22777,22677,22579,22481,22385,22290,22196
};
#endif

#ifdef MAX_MODULATION_97_PER_CENT
#define START_INDEX     60
const u16 circle_limit_table[68]=     
{
32483,32206,31936,31672,31415,31289,31041,30799,30563,30331,
30105,29884,29668,29456,29352,29147,28947,28750,28557,28369,
28183,28002,27824,27736,27563,27393,27226,27062,26901,26743,
26588,26435,26360,26211,26065,25921,25780,25641,25504,25369,
25236,25171,25041,24913,24788,24664,24542,24422,24303,24186,
24129,24015,23902,23791,23681,23573,23467,23362,23258,23206,
23105,23004,22905,22808,22711,22616,22521,22429
};
#endif

#ifdef MAX_MODULATION_98_PER_CENT
#define START_INDEX     61
const u16 circle_limit_table[67]=     
{
32494,32360,32096,31839,31587,31342,31102,30868,30639,30415,
30196,29981,29771,29565,29464,29265,29069,28878,28690,28506,
28325,28148,27974,27803,27635,27470,27309,27229,27071,26916,
26764,26614,26467,26322,26180,26039,25901,25766,25632,25500,
25435,25307,25180,25055,24932,24811,24692,24574,24458,24343,
24230,24119,24009,23901,23848,23741,23637,23533,23431,23331,
23231,23133,23036,22941,22846,22753,22661
};
#endif

#ifdef MAX_MODULATION_99_PER_CENT
#define START_INDEX     62

const u16 circle_limit_table[66]=     
{
32635,32375,32121,31873,31631,31394,31162,30935,30714,30497,
30284,30076,29872,29672,29574,29380,29190,29003,28820,28641,
28464,28291,28122,27955,27791,27630,27471,27316,27163,27012,
26864,26718,26575,26434,26295,26159,26024,25892,25761,25633,
25569,25444,25320,25198,25078,24959,24842,24727,24613,24501,
24391,24281,24174,24067,23963,23859,23757,23656,23556,23458,
23361,23265,23170,23077,22984,22893
};
#endif

#ifdef MAX_MODULATION_100_PER_CENT
#define START_INDEX     63
const u16 circle_limit_table[65]=     
{
32767,32390,32146,31907,31673,31444,31220,31001,30787,30577,30371,
30169,29971,29777,29587,29400,29217,29037,28861,28687,28517,
28350,28185,28024,27865,27709,27555,27404,27256,27110,26966,
26824,26685,26548,26413,26280,26149,26019,25892,25767,25643,
25521,25401,25283,25166,25051,24937,24825,24715,24606,24498,
24392,24287,24183,24081,23980,23880,23782,23684,23588,23493,
23400,23307,23215,23125
};
#endif

Volt_Components Stat_Volt_q_d;
/**********************************************************************************************************
把经过PID调整过的d、q值的合成矢量模值限制在最大值，如果超过了，那么等比例缩小d、q值
**********************************************************************************************************/ 
void RevPark_Circle_Limitation(void)
{
	s32 temp;
    //计算Vd^2+Vq^2
	temp = Stat_Volt_q_d.qV_Component1 * Stat_Volt_q_d.qV_Component1 + Stat_Volt_q_d.qV_Component2 * Stat_Volt_q_d.qV_Component2;  

	if ( temp > (u32)(( MAX_MODULE * MAX_MODULE) ) ) //如果(Vd^2+Vq^2)大于MAX_MODULE^2，就要进行比例缩小
	{
		u16 index;                             //假设Vq=x*32767， Vd=y*32767

		temp /= (u32)(512*32768);              //(Vq^2+Vd^2)/(512*32768) = (x^2+y^2)*32767^2/(512*32768)=64(x^2+y^2)
		temp -= START_INDEX ;                  //程序中，载波频率=15K，因此调制系数选择95%，MAX_MODULE=32767*95%=31128，START_INDEX=57
		index = circle_limit_table[(u8)temp];  //当Vq、Vd接近于最大值32767，即x、y接近于1，temp=64(x^2+y^2)-START_INDEX=128-57有最大值71
                                               //当Vd^2+Vq^2的值只比MAX_MODULE^2大一点，temp=(Vq^2+Vd^2)/(512*32768)-START_INDEX=MAX_MODULE^2/(512*32768)-57有最小值0
		                                       //因此调制系数为95%的circle_limit_table表中只有72个数，并且根据Vd^2+Vq^2的大小来选择缩小的系数
		temp = (s16)Stat_Volt_q_d.qV_Component1 * (u16)(index);   //使用缩小系数来缩小Vq
		Stat_Volt_q_d.qV_Component1 = (s16)(temp/32768);  

		temp = (s16)Stat_Volt_q_d.qV_Component2 * (u16)(index);   //使用缩小系数来缩小Vd
		Stat_Volt_q_d.qV_Component2 = (s16)(temp/32768);  
	}
}

上面的過程也沒有什麼好說的，具體可以看註解。為了優化處理速度，能用查表的地方都用查表了。

7.SVPWM的實現

根據FOC的處理過程，相電流採樣後經過clark變換、park變換得到d、q軸值，然後和參考值做PID運算，再經過歸一化，反park變換得到Vα、Vβ，這就是FOC的處理結果，然後輸出給SVPWM去執行，下面我們來看看SVPWM處理函數：
程式碼語言： javascript

#define SQRT_3		1.732051           //根号3
#define T		    (PWM_PERIOD * 4)   //TIM1 ARR值的4倍
#define T_SQRT3     (u16)(T * SQRT_3)
#define SECTOR_1	(u32)1
#define SECTOR_2	(u32)2
#define SECTOR_3	(u32)3
#define SECTOR_4	(u32)4
#define SECTOR_5	(u32)5
#define SECTOR_6	(u32)6

void CALC_SVPWM(Volt_Components Stat_Volt_Input)
{
    u8 bSector;
    s32 wX, wY, wZ, wUAlpha, wUBeta;
    u16  hTimePhA=0, hTimePhB=0, hTimePhC=0;		

    wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3;
    wUBeta = -(Stat_Volt_Input.qV_Component2 * T);

    wX = wUBeta;
    wY = (wUBeta + wUAlpha)/2;
    wZ = (wUBeta - wUAlpha)/2;

    //下面是查找定子电流的扇区号
    if (wY<0)
    {
        if (wZ<0)
        {
            bSector = SECTOR_5;
        }
        else // wZ >= 0
            if (wX<=0)
            {
                bSector = SECTOR_4;
            }
            else // wX > 0
            {
                bSector = SECTOR_3;
            }
    }
    else // wY > 0
    {
        if (wZ>=0)
        {
            bSector = SECTOR_2;
        }
        else // wZ < 0
            if (wX<=0)
            {
                bSector = SECTOR_6;
            }
            else // wX > 0
            {
                bSector = SECTOR_1;
            }
    }							   

    switch(bSector)		  //根据所在扇区号，计算三相占空比
    {
    case SECTOR_1:
    case SECTOR_4:
        hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
        hTimePhB = hTimePhA + wZ/131072;
        hTimePhC = hTimePhB - wX/131072;
        break;
    case SECTOR_2:
    case SECTOR_5:
        hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
        hTimePhB = hTimePhA + wZ/131072;
        hTimePhC = hTimePhA - wY/131072;
        break;

    case SECTOR_3:
    case SECTOR_6:
        hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
        hTimePhC = hTimePhA - wY/131072;
        hTimePhB = hTimePhC + wX/131072;
        break;
    default:
        break;
    }

    TIM1->CCR1 = hTimePhA;
    TIM1->CCR2 = hTimePhB;
    TIM1->CCR3 = hTimePhC;
}

1.上面的處理的處理函數，先用輸入的Vα、Vβ計算wUAlpha、wUBeta，再根據wUAlpha、wUBeta計算wX、wY、wZ，再由wX、wY、wZ判斷扇區號和占空比，計算過程完全同手冊說的一樣：
- 計算wUAlpha、wUBeta：
- FOC和SVPWM的C語言代碼實作
- 計算wX、wY、wZ：
- FOC和SVPWM的C語言代碼實作
- ，
- FOC和SVPWM的C語言代碼實作
- 判斷扇區號：
- FOC和SVPWM的C語言代碼實作
- 計算佔空比：
- FOC和SVPWM的C語言代碼實作
2.扇區判斷和占空比計算的原理與這篇文章說的一樣，這裡就不細說了。程式碼中有疑慮就是那個T/8和131072是什麼意思。
- 程式碼語言： javascript
- ```
        hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
        hTimePhB = hTimePhA + wZ/131072;
        hTimePhC = hTimePhB - wX/131072;
```
- 因為T等於4倍的ARR值，所以T/8就等於ARR/2，相當於把3個通道的比較值平移了半個週期。因為當3個通道的波形相同時，實際上並沒有輸出（只有3個低側開關管或高側開關管打開），真正有輸出的是3個通道波形不一樣的地方（有高側管子打開也有低側管子打開），所以讓3個通道同時移動相同ARR/2對輸出並沒有什麼影響，同時方便了計算，避免了出現通道比較值為負的情況。
- 至於131072，網路上有人是這麼說的：（2^15）*4 = 32768*4 = 131072 #define T (PWM_PERIOD * 4)，這裡有一個4倍的放大。然後電流採用了Q15表示（左對齊），2^15 = 32768。所以最後計算需要除以131072。
- 這篇貼文也有相關討論：鏈接
- 因為用的st的函式庫，svpwm的實作函數也沒怎麼去研究過。感覺這個函數有點繞，讓人暈，其實知道原理後，讀者可以自己實現了。

8.FOC總的處理函數：

FOC和SVPWM的C語言代碼實作
程式碼語言： javascript

Curr_Components Stat_Curr_a_b;            
Curr_Components Stat_Curr_alfa_beta;       
Curr_Components Stat_Curr_q_d;             
Curr_Components Stat_Curr_q_d_ref_ref;   //电流环的给定值，用于电流环Id,Iq和前馈电流控制的给定值
Volt_Components Stat_Volt_q_d;             
Volt_Components Stat_Volt_alfa_beta;      

void FOC_Model(void)	  //电流环处理函数
{
	Stat_Curr_a_b = GET_PHASE_CURRENTS();			//读取2相的电流值
	Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b);	//Ia,Ib通过Clark变换得到Ialpha和Ibeta  
	Stat_Curr_q_d = Park( Stat_Curr_alfa_beta,ENC_Get_Electrical_Angle() );  //输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id
																   
	//q轴的pid运算，得到Vq  
	Stat_Volt_q_d.qV_Component1 = PID_Regulator(Stat_Curr_q_d_ref_ref.qI_Component1,Stat_Curr_q_d.qI_Component1, &PID_Torque_InitStructure);
	//d轴的pid运算，得到Vd  
	Stat_Volt_q_d.qV_Component2 = PID_Regulator(Stat_Curr_q_d_ref_ref.qI_Component2,Stat_Curr_q_d.qI_Component2, &PID_Flux_InitStructure);  	

	RevPark_Circle_Limitation();                      //归一化
	Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);    //反Park变换
	CALC_SVPWM(Stat_Volt_alfa_beta);                  //svpwm实现函数，实际的电流输出控制		
}

把這個函數放在ADC注入組轉換完成中斷中呼叫即可。
現在我們梳理一下整個過程：配置TIM1、ADC，並設定ADC的注入組轉換由TIM1觸發。這樣在每個載波週期，都會觸發一次ADC注入組取樣，相電流取樣完成後呼叫FOC_Model進行FOC運算處理，然後把最後的計算結果更新到TIM1的輸出。現在還有一個問題：現在自己手中有上面的程式碼、相關的硬體和一個電機，要怎麼讓電機開始轉起來呢？那就要看這篇文章了。

9.弱磁控制
- 待續。。
- PS：工程要的人多，我放在這裡了https://www.cirmall.com/circuit/16544，賣3塊，平台收30%，得2.1，當賞瓶可樂吧。
- 工程中的程式碼在本文中已經基本給出來了，為了方便大家直接看，才給出工程的，少去了你自己去整理程式碼。至於為什麼賣3塊，我已經說了是“賞”，你覺得看了文章有幫助，“賞”給我的。
- 那些因為我賣錢，就罵我的白嫖黨也是牛逼，你覺得我花時間寫這些東西給你們看，是天經地義的？愛看看，不看滾。
- 發布者：全端程式設計師棧長，轉載請註明出處：https://javaforall.cn/136757.html
- 原文連結：https://javaforall.cn

2024-05-02 Share: FOC和SVPWM的C語言代碼實作

Local Backup

一、配置進階定時器TIM1產生6路互補PWM，附煞車保護

二、配置雙ADC模式和規則組、注入組，其中註入組由TIM1的TRGO觸發

三、編碼器的配置

四、FOC相關變換的代碼實現

五、PID控制

TAGS