STM32F4 DISCOVERYで簡易なPID制御をやってみました.
PID制御は,制御入力が以下の式で表される制御のことです.
上記はIdeal形式の表記で,Parallelでは以下のようになりますが,本質的には同じです.
今回はIdeal形式の方で,制御則を組んでいます.
目的としては,ポテンショメータの値をAD変換し,PWMでモータを駆動して,位置サーボをするというものです.
まずは,GPIOの設定です.
STM32のシリーズでは,ピンとして用意されているGPIO(PA0, PA1, …)のそれぞれに色々な機能がついています.
このため,何か機能を使いたいときには,使いたい機能を持っているGPIOを設定してから,個別の機能を設定する必要があります.
GPIO(General Purpose IO)には,デジタル(1か0)の入力,出力の他に,それぞれのピンに割り振られた特殊機能を使うことができます.
特殊機能の詳細はベンダーホームページのデザイン・リソースから,UM1472: Discovery kit for STM32F407/417 linesをみるとわかります.
この表を見ると,PA1
にはADC_123_IN1
が,PA2
にはTIM2_CH3
があることが分かります.
このため,PA1
をADCとして,PA2
をPWM(タイマ機能の一部であるため)の出力として利用します.
GPIO No. |
Alternate function |
details |
PA1 |
ADC1 |
AD変換器1.ポテンショメータの値を読む用 |
PA2 |
TIM2 CH3 |
タイマ2のチャンネル3.モータへのPWM出力(正回転) |
このときのGPIOの設定はこちら.
GPIO_InitTypeDef GPIO_InitStructure
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1
GPIO_Init(GPIOA, &GPIO_InitStructure)
GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2
GPIO_Init(GPIOA, &GPIO_InitStructure)
まずは,GPIOA全体の有効化として,以下を記述.
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
次に,GPIOAの1番ピンはADCとして使うので,
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
としてAN(Analog)モードに.
2番ピンはPWM(タイマ)として使うので,
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
としてAF(Alternate function)モードに設定します.
次に,ADCの設定.
個別にADCの設定をしていきます.
ADC_CommonInitTypeDef ADC_CommonInitStructure
ADC_InitTypeDef ADC_InitStructure
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE)
ADC_StructInit(&ADC_InitStructure)
ADC_DeInit()
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles
ADC_CommonInit(&ADC_CommonInitStructure)
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None
ADC_InitStructure.ADC_NbrOfConversion = 1
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b
ADC_InitStructure.ADC_ScanConvMode = DISABLE
ADC_Init(ADC1, &ADC_InitStructure)
ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_15Cycles)
ADC_Cmd(ADC1, ENABLE)
ADC_SoftwareStartConv(ADC1)
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC))
まずは,今回使うADC1の有効化.
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
次にADC全般の設定.
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles
ADC_CommonInit(&ADC_CommonInitStructure)
今回は,DMAを使わず,個別にADCするので,DMAAccessMode
はDisableにしています.
次に,もう少し詳細なADCの設定.
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None
ADC_InitStructure.ADC_NbrOfConversion = 1
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b
ADC_InitStructure.ADC_ScanConvMode = DISABLE
ADC_Init(ADC1, &ADC_InitStructure)
前述のように個別でADCするので,ContinuousConvMode
はDisableに.
また,今回はADCはこの1chしか使わないので,NbrOfConversion
は1に設定しています.
DataAlign
は右揃えに,Resolution
は12bit精度にしています.
設定が終わったら,有効化して,
ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_Cmd(ADC1, ENABLE);
一度だけADC1をテスト動作しています.テスト動作が終わる(ADC_FLAG_EOC
が立つ)まで,待っています.
ADC_SoftwareStartConv(ADC1);
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
これでADCの設定は終わり.
次はタイマの設定.
タイマ個別の設定は以下.
#define PWM_FREQ 20e3
#define PWM_DIV 200
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
TIM_TimeBaseInitStructure.TIM_ClockDivision = 0
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up
TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1)
TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1)
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure)
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable)
TIM_Cmd(TIM2, ENABLE)
TIM_CtrlPWMOutputs(TIM2, ENABLE)
TIM_OC3Init(TIM2, &TIM_OCInitStructure)
上部defineのPWM_FREQ
はPWMの周波数,PWM_DIV
はPWMの分解能を表しています.
PWM_DIV
を分解能と呼ぶのは少し正規の表現から外れていますが,便宜上こう呼びます.
PWM_DIV
は,PWMのデューティ比を何分割までできるかを表しており,
今回は200分割,つまり0.5%単位でのデューティ比の設定が可能となっています.
PWMの場合,モータドライバの電圧によって同じデューティ比でも実際にモータにかかる電圧が変わってくるので,
こういった設定のほうがいいんじゃないかということで.
たとえば,モータドライバの最大出力電圧が10Vで,
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100;
とすると,出力電圧は10Vの25%である,2.5V相当となります.
次にタイマ2の有効化.
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
PWMを200分割で使用するための設定.
TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1);
プリスケーラの設定.
TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1);
タイマ2の設定の反映.
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
次に,タイマをPWMで使用することや,タイマのピン出力をONにすることなどを設定し,反映しています.
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High
TIM_OCInitStructure.TIM_Pulse = PWM_DIV * 25 / 100
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable)
TIM_Cmd(TIM2, ENABLE)
TIM_CtrlPWMOutputs(TIM2, ENABLE)
TIM_OC3Init(TIM2, &TIM_OCInitStructure)
途中,TIM_Pulse
に値を代入しているのは,ただのテストです.
これで設定は終わり.
次は時間割り込みの処理について記述します.
PIDでは積分器および微分器を使うので,PIDの計算が周期的に実行されてほしいです.
このため,PIDが20ms周期で実行されるように,時間割り込みの処理について記述しています.
STM32F407では,周期割り込みが関数として記述されているみたいなので,それを使います.
SysTick_Handler
という関数です.
別スクリプトの設定で,SysTick_Handler
には1ms周期で入るようになっています.
#define STROKE 25.0
#define PID_SMPL_PERIOD 20
void SysTick_Handler(void){
static float c = 0, u = 0, pos = 0;
static int i = 0;
i++;
if(!(i % 20)){
ADC_SoftwareStartConv(ADC1);
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
}
if(!(i % PID_SMPL_PERIOD)){
u = PID(c, pos);
if(u > PWM_MAX_VOLTAGE){
u = PWM_MAX_VOLTAGE;
}else{
u = 0;
}
TIM_OCInitStructure.TIM_Pulse = (uint32_t)(PWM_DIV * (u / PWM_MAX_VOLTAGE));
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
}
if(i == 10000) i = 0;
}
この関数内では,AD変換は20msごとに実行され,
ポテンショメータの電圧から,実際のストロークに変換されています.
AD変換をスタートし,
ADC_SoftwareStartConv(ADC1);
終了するまで待っています.
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
そのAD変換値ADC_GetConversionValue(ADC1)
から,実際の位置pos
に変換しています.
pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
ここで,AD変換が12bitなため,その最大値((1<<12) -1)
で割っています.
PIDの計算は,PID_SMPL_PERIOD
ごとに行われます.
今回はADCの周期と同じ20msごとです.
u = PID(c, pos)
PID()
は,目標値c
と現在の位置pos
を引数として与えて,返り値として計算された操作量u
を得る関数です.
ここでは,計算された操作量がドライバで出力できるかどうかの判断を行っています.
if(u > PWM_MAX_VOLTAGE){
u = PWM_MAX_VOLTAGE;
}else{
u = 0;
}
あとは,操作量(電圧)からPWMのデューティ比を計算し,
TIM_OCInitStructure.TIM_Pulse = (uint32_t)(PWM_DIV * (u / PWM_MAX_VOLTAGE));
実際の出力に反映させています.
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
最後に,PID()
関数の中身について.
PID()
関数は非常にシンプルで,以下のようになっています.
float PID(float c, float pos){
static float e = 0, ei = 0, ed = 0, e_pre = 0;
e = c - pos;
ei += e * (PID_SMPL_PERIOD * 1e-3);
ed = (e - e_pre) / (PID_SMPL_PERIOD * 1e-3);
e_pre = e;
return Kp * (e + 1 / Ti * ei + Td * ed);
}
ここでc
は目標の位置,pos
は現在の位置です.
まず,現在の偏差を計算し,
e = c - pos;
偏差の積分値,
ei += e * (PID_SMPL_PERIOD * 1e-3);
偏差の微分値,を計算します.
ed = (e - e_pre) / (PID_SMPL_PERIOD * 1e-3);
微分計算では,前回の誤差と現在の誤差を使って計算するので,前回の誤差e_pre
を更新します.
e_pre = e
これらの値を元に,を計算し,return
します.
return Kp * (e + 1 / Ti * ei + Td * ed);
これらの記述で,STM32F4DISCOVERYでPID制御ができます.
以下はこれらを基にしたプログラムリスト(main.c, timer.c, timer.h)です.
このままビルドするとSysTick_Handler()
が重複しているとかでエラーが出るので,
stm32f4xx_it.c
内のSysTick_Handler()
を消してあげてください.
下の方のCall back関数は消すとリンクエラーが出るので,
これを消したい場合にはリンク先の箇所を消せば動くと思います.
今回は正方向の出力しかできないので,両方向にモータを回転させたい場合は,
PWMのチャンネルを1つ追加する必要があると思います.