モータ制御

R8C/15については基本的な要素が揃ったので、ここでは実際にサーボ制御部を作ってみます。
用意したのは、宇宙ロボコンファイターＸさんから去年の１０月に受け取っていた例のヤツ♪
＝超小型ギアドモータ（ポテンション付き、ごめんなさい、写真はＮＧです（＾＾；）と、
１００円で買えちゃうモータドライバＴＡ７２９１Ｓ（０～２０V、平均０．４Ａ）です。
ＴＡ７２９１Ｓには、モータ電圧制御用のＶｒｅｆ端子がありますが、
今回は、これは素直にモータ用電源Ｖｓに接続して、電圧（＝トルク）制御については、
２つの回転方向入力端子にＰＷＭ信号と反転ＰＷＭ信号を入力することで、
　デューティ　　０％＝正転ＭＡＸ
　デューティ　５０％＝ブレーキ
　デューティ１００％＝反転ＭＡＸ
ってな感じでリニアに制御します。



で、実際、作り始めるとこれが結構時間かかった。（＾＾；；；
その中でも一番手こずったのが、A/D変換の変換速度でした。
なにせ、マニュアルでは１回変換辺り約４μｓのハズなのに、実際に計ってみると４０μｓとかって出てくるから・・・。
まぁ、結局、それは他の割り込みがいっぱい入り込んでるからってことがわかったので、
ポテンション読み取り用のＡ／Ｄ変換は、他の割り込みが干渉しないタイマＣの割り込み
（２０ｋＨｚ＝５０μｓでＰＷＭ信号を生成）の中で３回（約１３μｓ）変換して平均を取り、
そのまま、その場でＰＤ制御に使用することにしました。
（今回はノイズ対策はこの平均値以外なにもしていないのですが、流石に２０ｋＨｚで動かしていると個々のノイズの影響は薄れるみたい）
あと、シリアル通信の割り込みは邪魔だったのでコードでは無視していますが、参考のため残しています。
そうそう、Ａ／Ｄ変換終了チェックは、adstを見るよりir_adicを見た方が何故か早かったりして・・・。（＾＾；；；

Ａ／Ｄ変換の件が落ち着くと、今度はＰＤパラメータの調整に入ります。
最初はＰパラメータだけで調整していたのですが、ゲインを上げる発振するし、
あと、ギアドモータ自体の初動トルクが結構大きいので、パンチ要素を加えて、
最終的には宮田さんの資料（俺サーボ）を参考にＤ要素を導入してどうにか納得できる動作が完成。
ポイントは、制御周期が２０ｋＨｚとかなり細かいので、Ｄゲインは結構大きな値が必要なこと、・・・かな？（＾＾；

では、実際のコードをご覧下さい♪



/************************************************************************/
/* */
/* SR8C15CP　サーボ制御プログラム */
/* */
/* 西　憲一郎 */
/* */
/************************************************************************/
// 本プログラムは、サンハヤトのＨＰのLED点灯プログラムをベースとしています。

/* インクルードファイル */
#include "sfr_r815.h" //SR8C15CPのI/Oレジスタが定義してあるSFRヘッダ（C言語用）

/* プロトタイプ宣言 */

ad_init(void);
timer_c_init(void);
timer_x_init(void);
uart_init(
unsigned short bps, /* 1200～51200 */
unsigned char bit, /* 7 or 8 */
unsigned char parity, /* 0:なし 1:奇数 2:偶数 */
unsigned char stopBit ); /* 1 or 2 */
short uart_put_char_hex(
unsigned char data );
short uart_put_short_dec(
unsigned short data );
short uart_putc(
unsigned char data );
short uart_getc();

/*I/Oの定義*/

/* 使用するI/Oレジスタの定義
* LEDは高電流駆動能力が選択できるPORT１に配線されている
*/

#define LED_GREEN p1_3 // 緑LEDはP1_1（Pin17）に配線されている
#define LED_RED p1_1 //赤LEDはP1_3（Pin14）に配線されている

#define LED_GREEN_DIR pd1_3 // 緑LEDのポートの方向レジスタ（1を設定すると出力ポートに設定される）
#define LED_RED_DIR pd1_1 // 緑LEDのポートの方向レジスタ（1を設定すると出力ポートに設定される）

#define LED_GREEN_DRV drr3 // 使用するI/Oポートの駆動能力を設定するbit
#define LED_RED_DRV drr1 // 1を設定すると駆動能力Highになり、LEDが駆動できる

/* グローバル変数 */

unsigned short pulse = 0; // パルス幅計測用

/*------------------------------------------------------------------------
* ID :なし
* 関数名 ： main
* 機能 : 緑色、赤色LEDを点滅させる
* 引数 :なし
* 戻り値 :なし
* 使用関数:なし
*------------------------------------------------------------------------*/

main()
{

long i = 0; // 点滅時間調整ループカウンタ
short data; // シリアル通信用

/* 高速オンチップオシレータに切り替える */

prc0 = 1; // システムクロック関係レジスタの書き込み許可（p68）
hra00 = 1; // 高速オンチップオシレータ発振（p53）
asm("NOP"); // 発振安定待ち
asm("NOP");
asm("NOP");
asm("NOP");
hra01 = 1; // 高速オンチップオシレータ選択
cm16 = 0; // 分周なしモード
cm17 = 0;
cm06 = 0; // 非1/8分周モード（cm16,17設定有効）
prc0 = 0; // 書き込み禁止

/* A/D変換初期化 */

ad_init();

/* timerC初期化 */

timer_c_init();

/* UART0初期化 */

// uart_init(38400, 8, 0, 1);

/* timerX初期化 */

timer_x_init();

asm("FSET I"); // 全体の割り込み許可

/* I/Oポート初期化*/

pd1_0 = 1; // p1_0/CMP0_0を出力モードに設定
pd3_3 = 1; // p3_3/CMP1_0を出力モードに設定
p1_0 = 1; // CMP信号は、出力データも"H"にしないと出力されません（要注意）。
p3_3 = 1; //

pd1_2 = 0; // p1_2/AN10を入力モードに設定

LED_GREEN_DRV = 1; // LEDを駆動できるよう、1を設定する
LED_RED_DRV = 1; // この設定はP1_0～P1_3でBit単位で設定できる
// 駆動能力をHighに設定すると1ピンあたり約15ｍAの電流を引き込める

LED_GREEN = 1; // LEDの初期状態（赤→消灯、青→点灯）をポートレジスタに設定する　この時点ではまだ入力ポートの設定になっている
LED_RED = 0;

LED_GREEN_DIR = 1; // LEDの接続されているI/Oポートを出力に設定する
LED_RED_DIR = 1;

/* LED点滅処理 */

while(1){

i++;
if (i > 20000) {
i = 0;
LED_GREEN = ~LED_GREEN; // ポートレジスタの値を反転させることでLEDが点滅する
LED_RED = ~LED_RED;
}

/* エコーバック */

// if ( 0 <= data ) {
// uart_put_char_hex((unsigned char)data); /* 数値表示 */
// uart_putc((unsigned char)0x0d); /* CR */
// uart_putc((unsigned char)0x0a); /* LF */
// }
}
}

/****************************************/
/* タイマＣ　初期化 */
/****************************************/

timer_c_init(void)
{

tcc00 = 0; // カウント停止
tcc02 = 0; // カウントソース　00:f1
tcc01 = 0; //

// INT3割り込みは、インプットキャプチャモード時に使用します。

// tcc04 = 0; // INT3割り込み　00:立ち上りエッジ
// tcc03 = 0; //
// tcc06 = 0; // INT3割り込みタイミング　0:タイマＣのカウントソースと同期
// tcc07 = 0; // 0:INT3割り込みに切り替え

// tcc11 = 0; // INT3フィルタ選択　00:フィルタなし
// tcc10 = 0; //

tcc13 = 1; // 1:アウトプットコンペアモード
tcc12 = 1; // 1:コンペア１一致時にカウンタ０セット
tcc15 = 1; // コンペア０出力設定　10:コンペア０の一致で"L"出力
tcc14 = 0; //
tcc17 = 1; // コンペア１出力設定　11:コンペア１の一致で"H"出力
tcc16 = 1; //

tcout0 = 1; // cmp0_0の出力許可
tcout1 = 0; // cmp0_1の出力禁止
tcout2 = 0; // cmp0_2の出力禁止
tcout3 = 1; // cmp1_0の出力許可
tcout4 = 0; // cmp1_1の出力禁止
tcout5 = 0; // cmp1_2の出力禁止
tcout6 = 0; // cmp0_0～cmp0_2からの出力を反転しない
tcout7 = 1; // cmp1_0～cmp1_2からの出力を反転する

// tc = 0; // カウントを停止させると自動的に０セットされる（tcレジスタは書き込みできない）

tm0 = 199; // デューティ D=(TM0+1/TM1+1)=50（先にtcc13=1:コンペア０出力に設定してから書き込む（p129））
tm1 = 399; // 周期設定 f=fCLK/(TM1+1)=8MHz/(399+1)=20kHz

cmp1ic = 0x06; // コンペア１割り込み許可（レベル６）

tcc00 = 1; // カウント開始
}

/****************************************/
/* コンペア１割り込み */
/****************************************/

#pragma interrupt cmp_1 // 割り込み処理関数指定
void cmp_1(void)
{

unsigned short an = 0; // ポテンション
static short e = 0; // 偏差
short d = 199; // デューティ
short ee; // 差分用

/* パルスチェック */

if (pulse > 890) {

/* A/D変換　3回サンプリング */

adst = 1; // A/D変換開始
while(ir_adic == 0); // 変換終了待ち
an += ad;

adst = 1; // A/D変換開始
while(ir_adic == 0); // 変換終了待ち
an += ad;

adst = 1; // A/D変換開始
while(ir_adic == 0); // 変換終了待ち
an += ad;

an = an >> 1; // 1.5倍してpulseとスケールを合わせる

/* 制御量の計算 */

ee = e;
e = -780;
e += pulse; // 偏差の計算
e -= an;
ee = ee - e; // 差分の計算

} else {
e = 0;
}

if(e < -3) { // －側処理
d += (e * 3) - 100 + (ee * 40); // ゲイン３、パンチ１００
if(d < 0) {
tm0 = 0;
} else {
tm0 = d;
}
} else if(e > 3) { // ＋側処理
d += (e * 3) + 100 + (ee * 40); // ゲイン３、パンチ１００
if(d > 399) {
tm0 = 399;
} else {
tm0 = d;
}
} else { // デッドバンド処理
tm0 = 199;
}
}

/****************************************/
/* Ａ／Ｄ変換　初期設定 */
/****************************************/

ad_init(void)
{

/* Ａ／Ｄ変換　初期設定 */

md = 0; // 単発モード
adgsel0 = 1; // p1グループ選択
ch2 = 1; // AN10選択
ch1 = 1;
ch0 = 0;
bits = 1; // 10ビットモード
cks1 = 1; // f1を選択
cks0 = 0;
vcut = 1; // Vref接続
smp = 1; // サンプルホールドあり
adcap = 0; // A/D変換自動開始ビット　0:adstでスタート
}

/****************************************/
/* タイマＸ　初期化 */
/****************************************/

timer_x_init(void)
{

txs = 0; // カウント停止
while(txs == 1); // 注意事項（p242）より

txmod2 = 0; // パルス周期測定モード以外
txmod1 = 1; // パルス幅測定モード
txmod0 = 1;

r0edg = 1; // "H"レベル幅測定＆INT1が立下りエッジで割り込み

txocnt = 0; // パルス幅計測モードでは０設定
txedg = 0;
txund = 0;

prex = 0xff; // フルレンジ
tx = 0xff;

txck1 = 0; // カウントソース設定　f8 = 1MHz(1us)
txck0 = 1;

cntrsel = 0; // p1_7/CNTR00/INT10使用 UART0関連レジスタのため、UART0側で注意が必要

int1ic = 0x05; // INT1割り込み許可
// txic = 0x05; // timerX割り込み許可

txs = 1; // カウント開始
}

/****************************************/
/* ＩＮＴ１割り込み */
/****************************************/

#pragma interrupt int_1 // 割り込み処理関数指定
void int_1(void)
{

txs = 0; // カウント停止
while(txs == 1); // 注意事項（p242）より

pulse = tx; // パルス幅取得
pulse = (pulse << 8) + prex;
pulse = 0xffff - pulse;

// uart_put_short_dec((unsigned short)pulse); /* パルス幅送信 */
// uart_putc((unsigned char)0x0d); /* CR */
// uart_putc((unsigned char)0x0a); /* LF */

prex = 0xff; // カウンタリセット
tx = 0xff;

txs = 1; // カウント開始
}

/****************************************/
/* タイマＸ割り込み */
/****************************************/

#pragma interrupt timer_x // 割り込み処理関数指定
void timer_x(void)
{

}


/************************************************************************/
/* */
/* 通信処理プログラム */
/* */
/* CPU TYPE :R8C/Tiny 15Group (8MHz) */
/* Copyright :西　憲一郎 */
/* */
/************************************************************************/
// 注）９ビット転送には対応していません

#define BUFF_SIZE 256
typedef struct {
unsigned short len;
unsigned short read;
unsigned short write;
unsigned char *buff;
} FIFO;
static unsigned char rxbuff[BUFF_SIZE];
static unsigned char txbuff[BUFF_SIZE];
static FIFO rxfifo;
static FIFO txfifo;

/****************************************/
/* ＵＡＲＴ０　初期化 */
/****************************************/

#define PCLOCK (8000000)
uart_init(
unsigned short bps, /* 1200～51200 */
unsigned char bit, /* 7 or 8 */
unsigned char parity, /* 0:なし 1:奇数 2:偶数 */
unsigned char stopBit ) /* 1 or 2 */
{
/* RX FIFO */
rxfifo.len = 0;
rxfifo.read = 0;
rxfifo.write = 0;
rxfifo.buff = rxbuff;

/* TX FIFO */
txfifo.len = 0;
txfifo.read = 0;
txfifo.write = 0;
txfifo.buff = txbuff;

if(bit == 7) {
smd2_u0mr = 1; // 調歩7bit（p140）
smd1_u0mr = 0;
smd0_u0mr = 0;
} else {
smd2_u0mr = 1; // 調歩8bit
smd1_u0mr = 0;
smd0_u0mr = 1;
}

if(stopBit == 2) {
stps_u0mr = 1; // 2ストップビット
} else {
stps_u0mr = 0; // 1ストップビット
}

if(parity == 0) {
prye_u0mr = 0; // パリティ禁止
} else {
prye_u0mr = 1; // パリティ許可
if(parity == 1) {
pry_u0mr = 0; // 奇数パリティ
} else {
pry_u0mr = 1; // 偶数パリティ
}
}

ckdir_u0mr = 0; // 内部クロック

u0c0 = 0x00; // f1で8MHz、CMOS出力、LSBファースト
u0c1 = 0x07; // 受信許可、送信許可
ucon = 0x00; // 送信バッファ空で割り込み、連続受信禁止

u0brg = ((PCLOCK / 16) / bps) - 1; // bps設定

/* 割り込み開始 */
s0ric = 0x04; // 受信割り込み（レベル４）（p74）
s0tic = 0x04; // 送信割り込み（レベル４）
}

/****************************************/
/* 数値出力 */
/****************************************/

short uart_put_char_hex(
unsigned char data )
{
char hex;

hex = data>>4;
hex &= 0x0F;
if ( hex<10 ) {
hex += 0x30;
} else {
hex += 55;
}
uart_putc(hex);

hex = data;
hex &= 0x0F;
if ( hex<10 ) {
hex += 0x30;
} else {
hex += 55;
}
uart_putc(hex);

}

short uart_put_short_dec(
unsigned short data )
{
char dec;

dec = data/10000;
data = data%10000;
dec += 0x30;
uart_putc(dec);

dec = data/1000;
data = data%1000;
dec += 0x30;
uart_putc(dec);

dec = data/100;
data = data%100;
dec += 0x30;
uart_putc(dec);

dec = data/10;
data = data%10;
dec += 0x30;
uart_putc(dec);

dec = data;
dec += 0x30;
uart_putc(dec);
}

/****************************************/
/* １文字格納 */
/****************************************/

short uart_putc(
unsigned char data )
{
short ret;
short i;

ret = -1;

s0tic = 0x00; // 送信割り込み禁止

if(ti_u0c1 == 1) { // 送信バッファレジスタが空なら直接書き込む
ret = data;
u0tb = ret; // データ送信
for( i=0; i<10; i++){} // タイミング調整
} else {
if ( BUFF_SIZE > txfifo.len ) {
txfifo.buff[txfifo.write] = data; // バッファへ書き込み

txfifo.len++;
txfifo.write++;
if ( BUFF_SIZE <= txfifo.write ) {
txfifo.write = 0;
}
ret = data;
}
else {
}
}

s0tic = 0x04; // 送信割り込み許可

return (ret);
}

/****************************************/
/* １文字取得 */
/****************************************/

short uart_getc()
{
short data;

data = -1;

s0ric = 0x00; // 受信割り込み禁止

if ( 0 != rxfifo.len ) {
data = rxfifo.buff[rxfifo.read]; // バッファから読み出し

rxfifo.len--;
rxfifo.read++;
if ( BUFF_SIZE <= rxfifo.read ) {
rxfifo.read = 0;
}
}

s0ric = 0x04; // 受信割り込み許可

return (data);
}

/****************************************/
/* 受信割り込み */
/****************************************/

#pragma interrupt uart0_r // 受信割り込み処理関数指定
void uart0_r(void)
{
unsigned short data; // u0rbレジスタは必ず16ビット単位で読み出す

data = u0rb; // データ取得
data &= 0x00ff;

if ( BUFF_SIZE > rxfifo.len ) {
rxfifo.buff[rxfifo.write] = data; // バッファへ書き込み

rxfifo.len++;
rxfifo.write++;
if ( BUFF_SIZE <= rxfifo.write ) {
rxfifo.write = 0;
}
}
}

/****************************************/
/* 送信割り込み */
/****************************************/

#pragma interrupt uart0_t // 割り込み処理関数指定
void uart0_t(void)
{
unsigned short data;

if ( 0 < txfifo.len ) {
data = txfifo.buff[txfifo.read]; // バッファから読み出し

txfifo.len--;
txfifo.read++;
if ( BUFF_SIZE <= txfifo.read ) {
txfifo.read = 0;
}
u0tb = data; // データ送信
}
else {
}
}