モーター制御について

#**はじめに**
私たちの機体はモーターひとつにつきマイコン1つをつけて、メインのマイコンから通信してモーターを回しています。
![IMG_20180826_184700.jpg](https://qiita-image-store.s3.amazonaws.com/0/283010/d47b19aa-d6cb-4cca-5485-473f7c047c4a.jpeg)

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

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

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

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

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

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

![350px-PID_Compensation_Animated.gif](https://qiita-image-store.s3.amazonaws.com/0/283010/9154da51-bc29-6379-fd54-2bac5e68f627.gif)

## 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*偏差の差

![350px-PID_Compensation_Animated.gif](https://qiita-image-store.s3.amazonaws.com/0/283010/9154da51-bc29-6379-fd54-2bac5e68f627.gif)

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


[こちら](https://www.yukisako.xyz/entry/pid_control)のサイトが分かりやすかったです。
結構端折ってしまったのでこちらもどうぞ。

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

# **基本的な動作**

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

![PDI.jpg](https://qiita-image-store.s3.amazonaws.com/0/283010/fb2ead8d-eeb8-7b64-a181-bc690cf81cb3.jpeg)

---
とりあえずコード全体

```c: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;
}
```
とまぁ、こんな感じになっています。
一個前のバージョンのソースが若干残ってて使ってないグローバル変数があります。……

![モーター.png](https://qiita-image-store.s3.amazonaws.com/0/283010/2784bdef-181b-fa5c-2c81-4b2b4a9a69a5.png)

上記のように接続されています。
>動作環境
>>ATxmega128a1u 内部32MHz駆動
>>ATmega328p 外部セラロック20MHz駆動


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

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




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

```c: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を送信したら状態を返すようにしています。

```c: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
あと、ロボットが、`超信地旋回`するとき、結構動作が変わってくるので注意です。全体的にいい感じになるように調整しましょう。