STM32F4 DISCOVERYでのPID制御

STM32F4 DISCOVERYで簡易なPID制御をやってみました．

PID制御は，制御入力が以下の式で表される制御のことです．

$$u = K_{p}(e(t) + \frac{1}{T_{I}}\int e(t) \ \mathrm{dt} + T_{D}\mathrm{\frac{d}{dt}}e(t)) \\ \because K_{p}: \mathrm{比例ゲイン, const.}, \\ T_{I}: \mathrm{積分時間, const.},\\ T_{D}: \mathrm{微分時間, const.},\\ e(t): \mathrm{誤差}$$
上記はIdeal形式の表記で，Parallelでは以下のようになりますが，本質的には同じです．

$$u = K_{P} \ e(t) +\frac{1}{T_{I}}\int e(t) \ \mathrm{dt}+T_{D}\mathrm{\frac{d}{dt}}e(t)$$

今回は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;

/* GPIO for ADC */
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 for TIM2 */
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);

/* timer 2 for PWM */
TIM_TimeBaseInitStructure.TIM_ClockDivision = 0;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1); //20kHz, 200div, 0.5percent per div, period = 84MHz / (prescaler + 1) / 20kHz - 1
TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1); //desired : 200steps in 20kHz, prescaler = 84 MHz (Core clock) / (20kHz*200step) - 1
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);

/* configuration for PWM */
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; //25% duty
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; //25% duty
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++;

    /* ADC */
    if(!(i % 20)){
        ADC_SoftwareStartConv(ADC1);
        while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
        pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
    }

    /* PID */
    if(!(i % PID_SMPL_PERIOD)){
        u = PID(c, pos);

        /* saturation */
        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を得る関数です．

ここでは，計算された操作量がドライバで出力できるかどうかの判断を行っています．

/* saturation */
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(t)$を計算し，
e = c - pos;

偏差の積分値$\int{e(t)} \ \mathrm{dt}$，
ei += e * (PID_SMPL_PERIOD * 1e-3);

偏差の微分値$\mathrm{\frac{d}{dt}}e(t)$，を計算します．
ed = (e - e_pre) / (PID_SMPL_PERIOD * 1e-3);

微分計算では，前回の誤差と現在の誤差を使って計算するので，前回の誤差e_preを更新します．
e_pre = e

これらの値を元に，$K_{p}(e(t) + \frac{1}{T_{I}}\int e(t) \ \mathrm{dt} + T_{D}\mathrm{\frac{d}{dt}}e(t))$を計算し，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つ追加する必要があると思います．

PIDのゲインは未設定です．

main.c

#include "stm32f4_discovery.h"
#include "timer.h"

#define PWM_FREQ 20e3
#define PWM_DIV 200
#define PWM_MAX_VOLTAGE 3.3
#define PID_SMPL_PERIOD 20 //ms
#define STROKE 25.0 //maximum stroke of a linear potentiometer
#define Kp 1.0
#define Ti 0.0
#define Td 0.0

/* TODO, configure pins for ADC, DAC, GPIO(DATA) from references */

TIM_OCInitTypeDef TIM_OCInitStructure;

void ADC_Config(void);
void TIM_Config(void);
void GPIO_Config(void);

uint8_t DATA = 0;

int main(void)
{
    TimerInit();
    GPIO_Config();
    ADC_Config();
    TIM_Config();

    /* Infinite loop */
    while(1)
    {
    }
}

void ADC_Config(void){
    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));
}

void TIM_Config(void){
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

    /* timer 2 for PWM */
    TIM_TimeBaseInitStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_Period = (uint32_t)(PWM_DIV - 1); //20kHz, 200div, 0.5percent per div, period = 84MHz / (prescaler + 1) / 20kHz - 1
    TIM_TimeBaseInitStructure.TIM_Prescaler = (uint16_t)((float)84e6 / (PWM_FREQ * PWM_DIV) - 1); //desired : 200steps in 20kHz, prescaler = 84 MHz (Core clock) / (20kHz*200step) - 1
    TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);

    /* configuration for PWM */
    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; //25% duty
    TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
    TIM_Cmd(TIM2, ENABLE);
    TIM_CtrlPWMOutputs(TIM2, ENABLE);
    TIM_OC3Init(TIM2, &TIM_OCInitStructure);
}

void GPIO_Config(void){
    GPIO_InitTypeDef GPIO_InitStructure;

    /* GPIO for ADC */
    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 for TIM2 */
    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);
}

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);
}

/* SysTick _Handler is interrupts per 1ms */
void SysTick_Handler(void){
    static float c = 0, u = 0, pos = 0;
    static int i = 0;

    i++;

    /* ADC */
    if(!(i % 20)){
        ADC_SoftwareStartConv(ADC1);
        while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
        pos = STROKE * (float)ADC_GetConversionValue(ADC1) / (float)((1<<12) - 1);
    }

    /* PID */
    if(!(i % PID_SMPL_PERIOD)){
        u = PID(c, pos);

        /* saturation */
        if(u > PWM_MAX_VOLTAGE){
            u = PWM_MAX_VOLTAGE;
        }else if(u < -PWM_MAX_VOLTAGE){
            u = 0;
        }

        TIM_OCInitStructure.TIM_Pulse = (uint32_t)(PWM_DIV * (u / PWM_MAX_VOLTAGE));
        TIM_OC3Init(TIM2, &TIM_OCInitStructure);
    }

    if(i == 10000) i = 0;
}

/*
 * Callback used by stm32f4_discovery_audio_codec.c.
 * Refer to stm32f4_discovery_audio_codec.h for more info.
 */
void EVAL_AUDIO_TransferComplete_CallBack(uint32_t pBuffer, uint32_t Size){
    /* TODO, implement your code here */
    return;
}

/*
 * Callback used by stm324xg_eval_audio_codec.c.
 * Refer to stm324xg_eval_audio_codec.h for more info.
 */
uint16_t EVAL_AUDIO_GetSampleCallBack(void){
    /* TODO, implement your code here */
    return -1;
}

timer.c

#include "timer.h"
#include "stm32f4xx.h"

static uint32_t timer_count;
static uint32_t timer_time;

void TimerInit()
{
    SystemCoreClockUpdate();
    SysTick_Config(SystemCoreClock / 1000);
    NVIC_SetPriority(SysTick_IRQn, 0);
}

void TimerWait_ms(uint32_t delay)
{
    timer_count = delay;
    while(timer_count > 0);
}

void TimerTick()
{
    if (timer_count > 0)
        --timer_count;

    ++timer_time;
}

uint32_t TimerNow()
{
    return timer_time;
}

timer.h

#ifndef TIMER_H_
#define TIMER_H_

#include <stdint.h>

void TimerInit();
void TimerWait_ms(uint32_t delay);
void TimerTick();
uint32_t TimerNow();

#endif