モーター制御について

はじめに

私たちの機体はモーターひとつにつきマイコン1つをつけて、メインのマイコンから通信してモーターを回しています。

それはなんのためかというと、モーターを正確にまっすぐ回したくて、PID制御を使って制御しているからです。
ひとつのATmega328pで2つのモーターを制御するのは処理的に難しく、ひとつずつ乗せています(STM使えば良いのにというのは置いといて)。メインCPUのATxmega128a1uがマスターで2台のATmega328pがスレーブのSPI通信をして動作させています。【追記】(これでも性能不足感が否めません、大人しくSTMマイコン使いましょう)

今回は私たちの使用しているPID制御のコードと説明を行いたいです。

PID制御ってなに？

PID制御ってなに？ということで、少しPID制御について説明していきたいと思います。
ざっくり言うとPID制御とは比例微分積分制御のことです。
例えば、エアコンの温度調整とかに使われています。
ひとつずつ話していきましょうかね。

P制御

P制御、つまり比例制御です。これはスピードの調整と思ってもらったら構いません。
これは単純に目標の値に、近づけるために目標と現在の値との差を比例制御ゲイン(以降Kp)をかけたものを操作量とするということです。
つまり

操作量=Kp*(目標値-現在値)

です。
これはどういうことかと言うと、下のgifのKp変化時を見て貰えると分かりますが、動かせば動かすほど操作量は、減っていきます。偏差が大きいほど早く、小さいほどゆっくりと修正しようとされます。
P制御だけでも用途によっては十分だと思います。。

PI制御

さて、P制御が何となく掴めたところでこれには決定的な問題点があります。。これには修正途中に生まれる未回転の部分の修正をしていないので、これではスピードは正確でも距離は不正確になってしまいます。
これを、解決するためにI制御(積分制御)を加えます。つまり、I制御は距離の修正ですね。
まあ、何してるかといえば、積分して距離の誤差を操作量に含めたら出来るんじゃね？ってやつです。

操作量 = Kp*偏差 + Ki*∫[0→t](θd-θ)dτ

なんだこりゃって感じですね。右側の項がI制御です。
簡単にすると

操作量 = Kp*偏差 + Ki*これまでの累計偏差

って感じです。
これも先程P制御のとこででてきたgifを見たら多少掴めるかなと思います。

PD制御

さてさてP制御、PI制御を扱いましたが、もう1つPD制御というものもあります。
これはなんだというと、P制御のゲインを大きくすると、目標速度まで早く到達することが出来ますが、大きくすればそのうち振動し始めます。
D制御はその振動するのを抑制するものになります。
また、外乱からを受けてからの復帰を早めるという役割もあります。
何をしてるのかと言うと、今回の偏差と前回の偏差との差によって操作量を調整するというものです。

操作量 = Kp*偏差 + Kd*(今回の偏差-前回の偏差)

PID制御

上記の制御を合わせたものをPID制御といいます。
きっちりと正確にスピードも距離も制御して、外乱からの復帰も早いというものです。

操作量 = Kp*偏差 + Ki*累計偏差 + Kd*偏差の差

それぞれのゲインによる動作のグラフです。

こちらのサイトが分かりやすかったです。
結構端折ってしまったのでこちらもどうぞ。

---

ではではPID制御が一通りわかって頂いた？ところで、実際の用例を見て見ましょう。

基本的な動作

基本的なスレーブ側のフローは以下のようになっています。

---

とりあえずコード全体

motor.c

/******************************************************************************
PC2:CS
PC3:INB
PC4:INA
PC5:EN
PD6:PWM
PD2:RO
*********************************************************************************/
#define CS (1<<PORTC2)
#define INB (1<<PORTC3)
#define INA (1<<PORTC4)
#define EN (1<<PORTC5)

#include "rx_spi.hpp"

#define rotation 6400
#define tinit 0 //オーバーフローのカウント幅
void init_motor(void); //initialize
void advance(void); 
void reverce(void); 
void brake(void); 
void move(uint8_t i); 
void motor(void);
void count_start(void);
void count_stop(void);

double p = 0.0;//p制御ゲイン
double s = 0.0;//i制御ゲイン
double d = 0.0;//d制御ゲイン

uint8_t debugno = 0;

uint16_t timr = 0;
uint8_t speed = 0;
int32_t cot = 0;
int32_t no=0;
unsigned int dtemp;
int ro = 10;
float timbest = 3900;
float timcnt = 3900;
uint8_t noflag=0;
double ocr = 0;
int32_t kasan = 0;//進むべき距離と進んだ距離との差
int32_t best = 0;
int32_t nox=0;
int32_t dev[2] = {0,0};//一回前の修正時点での進んだ距離と今回進んだ距離
uint8_t fb = 3;
int32_t ircount=0;//修正回数
bool oflag=false;
uint16_t distance[8] = {0,3068,6200,2000,1230,3000,0};//進む距離

void init_motor(void){
    DDRC |= CS|INB|INA|EN;
    DDRD |= (0<<PIND2)|(1<<PORTD6);
    //PORTD |= (1<<PORTD2);     
    EICRA = 0b00000010;
    /*
    00 : LOW level Interrupt
    01 : Both Edge Interrupt
    10 : Falling Edge Interrupt
    11 : Rising Edge Interrupt
    */
    EIFR  = 0b00000001;
    EIMSK = 0b00000001;
    TCNT1 = tinit;
    TCCR0A = 0b10000011;
    TCCR0B = 0b00000011;
}

void advance(void){
    PORTC = INA|EN;
}
void reverce(void){
    PORTC = INB|EN;
}
void brake(void){
    PORTC = INB|INA|EN;
    count_stop();
}

int temp = 0;
unsigned int mflag=0;

void motor(void){
    ircount = no = kasan = dev[0] = dev[1] = 0;
    oflag=false;
    uint8_t num = 0;
    while(!SPI_ReceiveFlag());
    SPI_SetSendData(2);
    num = SPI_ReceiveData();
    uint8_t fb = num>>6;
    speed = (num>>3)&0b00000111;
    uint8_t dis = num&0b00000111;
    if(debugno){
    uart_putstr("fb:");
    uart_putdec(fb);
    uart_putstr("sp:");
    uart_putdec(speed);
    uart_putstr("dis:");
    uart_putdec(dis);
    }
    if(speed==1){
        speed = 2;
    }
    best = speed*50;  //要調整 
    if(fb==3 || speed==0){
        brake();
        SPI_SetSendData(1);
        temp= 1;
        return;
    }
    else if(fb==1){
        advance();
    }
    else if(fb==2){
        reverce();
    }

    OCR0A = ocr = speed * 34;
    count_start();
    sei();

    noflag = 1;
    while(((ircount*best)-kasan)+no < distance[dis] && !SPI_ReceiveFlag()){
        if(oflag){
            TCNT1=tinit;
            /*
            clk/0 : 0.22768ms
            clk/8 : 18.2144ms
            clk/64 : 145.7152ms
            clk/256 : 582.8608ms
            clk/1024 : 2331.4432ms
            */
            nox=no;//for uart
            kasan += best - no;
            dev[1] = best - no;
            ocr+=p*(best-no)+s*kasan+d*(dev[1]-dev[0]);

            if(ocr>=255){
                ocr=255;
            }
            else if(ocr<=0){
                ocr=1;
            }
            OCR0A=(uint8_t)ocr;
            no=0;
            ircount++;
            noflag = 1;
            dev[0] = dev[1];
            count_start();
        }
    }
    uart_putstr("finish-Over:");
    uart_putdec(ircount);
    uart_putstr("OCR0A:");
    uart_putdec(OCR0A);
    brake();
    SPI_SetSendData(1);
    if(debugno){
        uart_putstr(" Finished!");
        uart_putstr("\n\r");
    }
    return;
}
void count_start(void){
    mflag=0; 
    TCNT1 = tinit;
    TIFR1=0x01; //Enable to Overflow Flag.
    TIMSK1=0x01; //Enable to Overflow Interrupt.
    TCCR1B=0x03; //start
    /* 
    000:Stop
    001:clk/0 = 0.000005ms/count : 0.327675ms
    010:clk/8 = 0.0004ms/count : 26.214ms
    011:clk/64 = 0.0032ms/count : 209.712ms
    100:clk/256 = 0.0128ms/count : 838.848ms
    101:clk/1024 = 0.0512ms/count : 3355.392ms
    110,111:External Clock
    */
    noflag = 1;
}
void count_stop(void){
    cli();
    TIMSK1=0x00;
    TIFR1=0x00;
    TCCR1B=0x00; 
    TCNT1 = tinit;
    ircount = no = kasan = 0;
}

ISR(TIMER1_OVF_vect){
    oflag=true;
}

ISR(INT0_vect){
    no++;
    EIFR  = 0b00000001;
    EIMSK = 0b00000001;
}

とまぁ、こんな感じになっています。
一個前のバージョンのソースが若干残ってて使ってないグローバル変数があります。……

上記のように接続されています。

動作環境

ATxmega128a1u 内部32MHz駆動
ATmega328p 外部セラロック20MHz駆動

INT0の外部割り込みを使って、ロータリーエンコーダーを読んでいます。
TIMER1のオーバーフロー割り込みが発生する時にpidの修正を掛けています。分周やカウント幅を変えると修正がかかる速さが早くなりますが、掛けすぎると処理落ちするので気をつけて。

追記 8分の1分周でも動きました。

流れ的には
SPI受信▶データ処理▶モーター回す▶TIMERスタート▶whileから抜ける▶終了
という流れです。

まぁ、重要なのは修正の中身ですね。

motor.cpp

kasan += best - no;
dev[1] = best - no;
ocr+=p*(best-no)+s*kasan+d*(dev[1]-dev[0]);

noが前回処理から回ったロータリーエンコーダーのカウント数です。
kasanは回った距離と回るべき距離との差の合計です。これはI制御に使います。
dev[1]は前回のタイマーオーバーフロー割り込みから回った距離と回るべき距離との差で、dev[0]はdev[1]の前回の値で、前回と比べてどれくらい修正されたかが分かります。これはD制御に使います。

bestですが、これは調整が必要なもので、speed*定数 で調整しています。PIDのゲインが0にして、speedをmaxにした時pwmがmaxちょい下くらいになるように調整します。
pwmがmaxに調整してしまうと、修正することが出来る速度幅がなくなってしまうので注意です。

あと、モーターの状態(今回ってるのかどうか)を確認するために、メインCPUはモーター制御用マイコンに頻繁にデータを送信し、状態を受信しています。
自分は0x00を送信したら状態を返すようにしています。

rx_spi.c

ISR(SPI_STC_vect)
{
    data = SPDR;
    if(data!=0x00){
        flag = 1;
        SPI_SetSendData(2);
    }
    else{
        SPDR = setdata;
    }
}

その他とくにいうことなかったですね……
bestの定数はシビアなのでよく調整してください。ぐらいですかな。

ゲインの調整について

ゲイン調整について、たくさんの方法があると思いますが、自分が普段しているのを紹介します。
まずは全てのゲインを0にしてから、Pゲインを振動するギリギリまで上げます。
それから1オーバーフロー毎の大体の、偏差を確認し、なるべく早く無くなるまでIゲインを上げていきます。
それが出来たらPゲインを若干上げて、振動させて、それがなくなるようにDゲインを上げていきます。
自分もだいたい何がいいのか全然わかりません。。
誰か教えてくださいw
あと、ロボットが、超信地旋回するとき、結構動作が変わってくるので注意です。全体的にいい感じになるように調整しましょう。