SH7125用 ロータリーエンコーダ、モータドライバドライバ
さて、前回のエントリで新CPUのSH7125で、シリアル通信のドライバを書いたわけだが、今日は、２相ロータリーエンコーダのドライバとモータドライバのドライバと、システムタイマのドライバを書いた。
SH7125は16bitタイマが6chある。H8/3069よりかなり高機能で最初にハードウェアマニュアルをみたときはびっくりした。H8のときは、ジェネラルレジスタA, Bの２つでPWMとか生成するわけだが、ジェネラルレジスタがA,B,C,Dと4つもあったり、バッファ動作可能だったり、ブラシレスモータ用の3相PWM用の機能があったり（マニュアルによると、「チャネル0、3、4を連動して、相補PWM、リセットPWMを用いたAC同期モータ（ブラシレスDCモータ）駆動モードが設定可能で、2種（チョッピング、レベル）の波形出力が選択可能」）、さすがH8よりSuperH。高機能です。もちろん位相計数モードも2chあります。そのためにH8/3069からSH7125に変えたわけですが。
まず、タイマのチャンネルアサインですが、
| ch0 | システムタイマ |
| ch1 | ロータリーエンコーダL |
| ch2 | ロータリーエンコーダR |
| ch3 | モータードライバLR |
としました。ジェネラルレジスタがABCDの４つあるおかげでch3だけで左右のモータードライバが制御できます。
ロータリーエンコーダ
ピンアサインとしては、TCLKAとTCLKBにロータリーエンコーダLのA相、B相を入力。TCLKCとTCLKDにロータリーエンコーダRのA相、B相を入力。ハードウェアでアップダウンカウントをしてくれるのでソースはとっても簡単。実装もあっさりでした。
モータードライバ
モータードライバは、メカロボショップで買ったSabertoothを使っています。こいつの信号線S1、S2にPWM信号を送れば正転、逆転できます。H8/3096のときに書いたドライバコードを移植なので簡単。あっさり動作。
システムタイマ
ロボットの制御中に、現在時刻が欲しい。というのがあるので、マイコンが起動してからの時間をミリ秒単位で得られる機能が必要です。全部ハードウェアのカウンタでいけないかと検討しましたがよい案がみつからず、とりあえず1msごとに割り込みをかけてグローバル変数をインクリメントするように実装しました。25Mhzのクロックなので25000クロックごとに割り込みをかけました。実装は、割り込み優先度設定を忘れてちょっとだけハマりました。
うごいているところ
やー、去年はロータリーエンコーダが動いただけで喜んでいたが、今回はあっさり動いて喜ぶほどのものでないですね。
次は、PC側の指令を受けて制御するメインプログラムをH8/3069用から移植してオドメトリ精度を試験したい。
ソース
RotaryEncoder2.h
//RotaryEncoder2.h //ibis inc. Eiji Kamiya //Licence : New BSD //自作ロータリーエンコーダ 2相用 #ifndef __ROTARY_ENCODER_H__ #define __ROTARY_ENCODER_H__ #include "typedefine.h" //定数 //関数宣言 //ロータリーエンコーダ初期化 void InitializeRotaryEncoder(); //左ロータリエンコーダの値取得 uint16 GetRotaryEncoderL(); //右ロータリエンコーダの値取得 uint16 GetRotaryEncoderR(); #endif//__ROTARY_ENCODER_H__
RotaryEncoder2.c
//RotaryEncoder2.c //ibis inc. Eiji Kamiya //Licence : New BSD //SH7125 + 自作ロータリーエンコーダ2相 #include "typedefine.h" #include "iodefine.h" #include "RotaryEncoder2.h" //デバイス定数 //ch1 TCLKA pin29-cn2-7 //ch1 TCLKB pin28-ch2-8 //ch2 TCLKC pin27-ch2-9 //ch2 TCLKD pin26-ch2-10 //モジュール内グローバル変数 //ロータリエンコーダ初期化 void InitializeRotaryEncoder() { //ピンファンクションコントローラ PFC.PACRL2.BIT.PA6MD=1;//ポートAコントロールレジスタL2をTCLKA入力に設定 PFC.PACRL2.BIT.PA7MD=1;//ポートAコントロールレジスタL2をTCLKB入力に設定 PFC.PACRL3.BIT.PA8MD=1;//ポートAコントロールレジスタL3をTCLKC入力に設定 PFC.PACRL3.BIT.PA9MD=1;//ポートAコントロールレジスタL3をTCLKD入力に設定 //スタンバイコントロールレジスタ（省電力モード） STB.CR4.BIT._MTU2=0;//MTU2をON //タイマスタートレジスタ MTU2.TSTR.BYTE=MTU2.TSTR.BYTE & 0xf9;//ch1 ch2 stop //タイマモードレジスタ MTU21.TMDR.BIT.MD=4;//ch1 位相計数モード１ MTU22.TMDR.BIT.MD=4;//ch2 位相計数モード１ //タイマスタートレジスタ MTU2.TSTR.BYTE=MTU2.TSTR.BYTE | 0x06;//ch1 ch2 start } //左ロータリエンコーダの値取得 uint16 GetRotaryEncoderL() { return MTU21.TCNT; } //右ロータリエンコーダの値取得 uint16 GetRotaryEncoderR() { return MTU22.TCNT; }
MotorDriver.h
//MotorDrive.h //ibis inc. Eiji Kamiya //Licence : New BSD //SH7125 + SaberTooth用 #ifndef __MOTOR_DRIVER_H__ #define __MOTOR_DRIVER_H__ #include "typedefine.h" //定数 //モータID typedef enum { MOTOR_L=1, MOTOR_R=0, } MotorID; //関数宣言 //モータドライバ初期化 void InitializeMotorDriver(void); //モータ回転 void SetMotorPower(MotorID id,int16 power); #endif//__MOTOR_DRIVER_H__
MotorDriver.c
//MotorDrive.c //ibis inc. Eiji Kamiya //Licence : New BSD //SH7125 + SaberTooth用 #include "iodefine.h" #include "MotorDriver.h" //メカロボのSabertoothモータドライバ //16bitタイマーch3 TIOC3A(pin9-cn1-9 ) <-----> Motor Driver S1 //16bitタイマーch3 TIOC3C(pin7-cn1-11) <-----> Motor Driver S2 //S1,S2に1000us〜2000usの幅のパルスを与えて制御。1000usのとき逆転100%、1500usのとき停止、2000usの //とき正転100%。PWM周期は2500usとする。CPUボードのクロックは、25MHz。 //デバイス定数 #define PWM_FREQ 62500 //PWM周期（単位：クロック） 25,000,000Hz/400Hz=62500 (400Hz=2500us) //モータドライバ初期化 void InitializeMotorDriver(void) { //スタンバイコントロールレジスタ（省電力モード） STB.CR4.BIT._MTU2=0;//MTU2起動 //ピンファンクションコントローラ PFC.PECRL3.BIT.PE8MD=1;//PE8をMTU2へ PFC.PECRL3.BIT.PE10MD=1;//PE10をMTU2へ PFC.PEIORL.BIT.B8=1;//PE8を出力へ PFC.PEIORL.BIT.B10=1;//PE10を出力へ //タイマモードレジスタ MTU23.TMDR.BIT.MD=2;//PWMモード1 //タイマコントロールレジスタ MTU23.TCR.BIT.CCLR=1;//TGRAのマッチでタイマクリア MTU23.TCR.BIT.CKEG=0;//立ち上がりエッジでカウント MTU23.TCR.BIT.TPSC=0;//内部クロックφ/1 //タイマI/Oコントロールレジスタ MTU23.TIOR.BIT.IOA=1;//TIOC3Aは、初期0、コンペアマッチで0 MTU23.TIOR.BIT.IOB=2;//初期0、コンペアマッチで1 MTU23.TIOR.BIT.IOC=1;//TIOC3Cは、初期0、コンペアマッチで0 MTU23.TIOR.BIT.IOD=2;//初期0、コンペアマッチで1 //デューティー比設定 SetMotorPower(MOTOR_L,0); SetMotorPower(MOTOR_R,0); //PWMスタート MTU2.TSTR.BIT.CST3=1; } //モータ回転 // id : MOTER_L or MOTER_R // power : -1000〜+1000（単位：千分率） void SetMotorPower(MotorID id,int16 power) { uint16 duty;//デューティー比（千分率） uint16 cmp; if(power>1000) power=1000; if(power<-1000) power=-1000; //Sabertooth R/Cモードは、1000usで逆転100%、2000usで正転100%で、PWM_FREQが2500usなので //power=-1000のとき、1000us/2500us=40% //power= 0のとき、1500us/2500us=60% //power=+1000のとき、2000us/2500us=80% duty=600 + power/5; //60%±20% cmp=PWM_FREQ-(uint32)duty*(uint32)PWM_FREQ/1000; if(id==MOTOR_L){ MTU23.TGRA=PWM_FREQ; MTU23.TGRB=cmp; } else { MTU23.TGRC=PWM_FREQ; MTU23.TGRD=cmp; } }
Timer.h
//Timer.h //ibis inc. Eiji Kamiya //Licence : New BSD //32bitシステムタイマ #ifndef __TIMER_H__ #define __TIMER_H__ #include "typedefine.h" //システムタイマの初期化 //@param priority 優先度（1〜15） void InitializeTimer(int8 priority); //時刻の取得 //@return マイコンが起動してからの時刻（単位：ms） int32 GetTimeMillis(); //ビジーウェイト //@param millis 待ち時間（単位：ms） void Sleep(int32 millis); #endif//__TIMER_H__
Timer.c
//Timer.h //ibis inc. Eiji Kamiya //Licence : New BSD //SH7125用 32bitシステムタイマ //デバイス定義 //SH7125のMTU2のch0を使う #include "typedefine.h" #include "iodefine.h" #include "Timer.h" #define CLOCK 25000000u //タイマの内部クロックは25Mhz #define COUNT (CLOCK/1000u) //出力は1000hz(1ms)ごとなので、25000（uint16に収まる）カウント/1ms //グローバル変数 int32 g_time=0; //システムタイマの初期化 void InitializeTimer(int8 priority) { //スタンバイコントロールレジスタ（省電力モード） STB.CR4.BIT._MTU2=0;//MTU2起動 //割り込みコントローラ INTC.IPRD.BIT._MTU20G = priority;//MTU2の割り込みレベル設定 //タイマコントロールレジスタ MTU20.TCR.BIT.CCLR=1;//TGRAのマッチでタイマクリア MTU20.TCR.BIT.CKEG=0;//立ち上がりエッジでカウント MTU20.TCR.BIT.TPSC=0;//内部クロックφ/1 //タイマジェネラルレジスタ MTU20.TGRA=COUNT; //タイマインタラプトイネーブルレジスタ MTU20.TIER.BIT.TGIEA=1;//TGRAコンペアマッチ割り込みON //タイマスタート MTU2.TSTR.BIT.CST0=1; } //時刻の取得 //@return マイコンが起動してからの時刻（単位：ms） int32 GetTimeMillis() { return g_time; } //ビジーウェイト //@param millis 待ち時間（単位：ms） void Sleep(int32 millis) { int32 t=g_time; while(g_time-t<millis); } //タイマ割り込み //1msごとにコールされる //intprg.cのINT_MTU2_0_TGIA0に記述を追加すること //resetprog.cのSR_Initを0にすること void int_mtu2_0_tgia0() { MTU20.TSR.BIT.TGFA=0; g_time++; }
main.c
#include "Serial.h" #include "MotorDriver.h" #include "RotaryEncoder2.h" #include "Timer.h" //プロトタイプ宣言 //int32から文字列へ変換 char* Int32ToString(int32 n,char* out); //メイン void main(void) { int size; //受信したサイズ uint8 buf[64]; //受信バッファ int16 countL; //ロータリーエンコーダLカウンタ int16 countR; //ロータリーエンコーダRカウンタ int16 power=0; //モータ出力（千分率） int i; //初期化 InitializeSerial(15); InitializeRotaryEncoder(); InitializeMotorDriver(); InitializeTimer(14); SerialPuts("AKI-SH7125 firmware ver.0.01\r\n"); //メイン while(1){ //シリアル通信テスト size=GetRecieveBufferDataSize(); if(size>0){ size=SerialReadData(buf,sizeof(buf)); SerialWriteData(buf,size); //モータードライバテスト（+で加速、-で減速） for(i=0;i<size;i++){ if(buf[i]=='+'){ power+=10; } else if(buf[i]=='-'){ power-=10; } SetMotorPower(MOTOR_R,power); SerialPuts("power="); Int32ToString(power,(char*)buf); SerialPuts((char*)buf); SerialPuts("\r\n"); } } /* //ロータリーエンコーダテスト countL=GetRotaryEncoderL(); countR=GetRotaryEncoderR(); SerialPuts("L="); Int32ToString(countL,(char*)buf); SerialPuts((char*)buf); SerialPuts(" R="); Int32ToString(countR,(char*)buf); SerialPuts((char*)buf); SerialPuts("\r\n"); */ //タイマテスト Int32ToString(GetTimeMillis(),(char*)buf); SerialPuts((char*)buf); SerialPuts("\r\n"); Sleep(100); } } //int32から文字列へ変換 //@return 変換後文字列（outそのもの） char* Int32ToString(int32 n,char* out) { static const int table[]={1,1,10,100,1000,10000,100000,1000000,10000000,100000000,1000000000}; int count=0; int32 bak=n; char* p=out; if(n==0){//0のとき *(p++)='0'; *p='\0'; return out; } while(bak!=0){ bak/=10; count++; } if(n<0){//負のとき if(n==-2147483648){ strcpy(out,"-2147483648"); return out; } *(p++)='-'; n=-n; } for(;count>0;count--){ bak=n/table[count]; *(p++)='0'+bak; n-=bak*table[count]; } *p='\0'; return out; }
※Int32ToString()はちょっと冗長なので書き直したい。
シリアル通信のソースは、こちら。
2010/2/4追記：上記を綺麗にした最新ソースは、「Open Robot Framework」として公開しています。そちらのソースを参照してください。
