応用例Ｃ ＭＣＲマシン制御
ルネサステクノロジーが後援するMCR(Micom Car Rally)に、JICA研修員を 参加させてきました。 研修員が簡単にMCRマシンを作れるように、制御基板とファームウエアを 用意しました。それが、下図です。上図は、モータ制御回路です。左右２個のモータを利用します。
上図は、センサーと左右モータのPWMのDUTY比表示回路です。
上図は、マシン状態とモード指定する回路です。
上図は、路面センサー回路です。 これらの回路を、指定マイコン(H8/3048Fone)で制御すると コース上をマシンが走行します。 コース全容は、以下です。
動作をタスクに分けて、MCRマシンを動かします。
仕様検討
MCRの規定に従って、動作仕様を検討します。 ルールは、次のように規定されています。 スタートの合図で、ゲートが開くのをセンサーで判断するか ボタンを押してスタートさせます。ゴールするか、審判から の指示があるまで、マシンには触れられません。 このルールから、スタートボタンを押してから、動作を開始 する仕様にします。 スタートボタンを押してから、路面センサーからの情報を取得し その情報を利用し、モータの回転を制御すれば走行できます。 大まかな動作をタスクで記述してみます。各タスクがどういう処理をすればよいかを考えます。 スタートボタンによるトリガー待ち 回路図から、PB4からの入力値が０であれば、他の タスクを起動して、自分自身はSUSPEND状態に遷移 します。 最初は、READY状態にしておきます。 路面センサーから情報取得 PORT_7からセンサーの情報を入力し、保存します。 この情報をLEDアレイに出力して、人間にわかるよう にします。 スタートボタンを押す前にも、センサー感度を調整 するために、常に動かしておきます。 常にREADY状態にします。 センサー情報からモータスピード計算 センサー情報から、モータスピードを計算しますが 路面状態は、直線、ワインディング、左右クランク、 平行ライン、片側平行ラインで場合分けして、値を 決定します。 最初は、SUSPEND状態にしておきます。 モータスピード設定 他のタスクで、モータスピードを計算してあるので その値を内部にあるPWM回路に与えます。 最初は、SUSPEND状態にしておきます。 モード更新 コースは、直線、曲線、カーブ、クランク、レーン チェンジが組み合わされています。マシンがどの状態 （モード）にあるのかを、決定します。 モードを決定するには、センサー情報を過去５回ほど に渡り、履歴として残します。この履歴から、車体が どちらの方向に動いているかスピードがどの程度かを 判断して、モードはどこになっているかを決定します。 最初は、SUSPEND状態にしておきます。 １から５までのタスクとなったので、タスク０からタスク４ として、動作を定義します。 コンフィグレータに必要な情報を与え、スケルトンのソース コードを作成します。
- スタートボタンによるトリガー待ち （タスク０）
- 路面センサーから情報取得 （タスク１）
- センサー情報からモータスピード計算（タスク２）
- モータスピード設定 （タスク３）
- モード更新 （タスク４）
タスク動作定義
タスク０を定義します。 スタートボタン(PB4)の入力値を判断します。 １なら、何もしないで終了します。 （RUNからREADY状態に戻します。） ０なら、タスク２〜４を起動後、SLEEPします。 （RUNからSUSPEND状態にします。） コメントで処理を定義します。 void tsk0_proc(void) { /* スタートボタンが１なら何もしない */ /* タスク２〜４を起動 */ } スタートボタンが押されていないときの処理を記述します。 #define START_BUTTON PB.DR.BIT.B4 void tsk0_proc(void) { /* スタートボタンが１なら何もしない */ if ( START_BUTTON == ON ) return ; /* タスク２〜４を起動 */ if ( START_BUTTON == OFF ) { rsm_tsk( TSK_ID2 ); rsm_tsk( TSK_ID3 ); rsm_tsk( TSK_ID4 ); slp_tsk(); } } システムコールを使い、タスクを起動します。 #define START_BUTTON PB.DR.BIT.B4 void tsk0_proc(void) { /* スタートボタンが１なら何もしない */ if ( START_BUTTON == ON ) return ; /* タスク２〜４を起動 */ rsm_tsk( TSK_ID2 ); rsm_tsk( TSK_ID3 ); rsm_tsk( TSK_ID4 ); slp_tsk(); } まとめます。 #define START_BUTTON PB.DR.BIT.B4 void tsk0_proc(void) { /* no push */ if ( START_BUTTON == ON ) return ; /* push */ rsm_tsk( TSK_ID2 ); rsm_tsk( TSK_ID3 ); rsm_tsk( TSK_ID4 ); slp_tsk(); } タスク１を定義します。 ポート７からセンサーの情報を入力し、保存します。 この情報をLEDアレイに出力して、人間にわかるよう にします。 スタートボタンを押す前にも、センサー感度を調整 するために、常に動かしておきます。 コメントで処理を定義します。 void tsk1_proc(void) { /* センサー情報入力 */ /* センサー情報保存 */ /* センサー情報をLEDアレイに出力 */ } センサー情報をポート７から入力します。 #define SENSORS P7.DR.BYTE UBYTE sensor_information ; void tsk1_proc(void) { /* センサー情報入力 */ sensor_information = SENSORS ; /* センサー情報保存 */ /* センサー情報をLEDアレイに出力 */ } センサー情報を保存します。 データ保存用関数store_sensorは、定義済みと仮定します。 #define SENSORS P7.DR.BYTE UBYTE sensor_information ; void tsk1_proc(void) { /* センサー情報入力 */ sensor_information = SENSORS ; /* センサー情報保存 */ store_sensor( sensor_information ) ; /* センサー情報をLEDアレイに出力 */ } センサー情報を、ＬＥＤアレイに反映させます。 #define SENSORS P7.DR.BYTE #define ISENSOR PA.DR.BYTE UBYTE sensor_information ; void tsk1_proc(void) { /* センサー情報入力 */ sensor_information = SENSORS ; /* センサー情報保存 */ store_sensor( sensor_information ) ; /* センサー情報をLEDアレイに出力 */ ISENSOR = ~sensor_information ; } まとめます。 #define SENSORS P7.DR.BYTE #define ISENSOR PA.DR.BYTE UBYTE sensor_information ; void tsk1_proc(void) { /* get sensor infomations */ sensor_information = SENSORS ; /* store sensor informations */ store_sensor( sensor_information ) ; /* show sensor states */ ISENSOR = ~sensor_information ; } タスク２を定義します。 センサー情報から、モータスピードを計算しますが 路面状態は、直線、ワインディング、左右クランク、 平行ライン、片側平行ライン等で場合分けして、値を 決定します。 多くのモードを用意すると、対応処理の記述が面倒に なるので、５〜７程度にしておきます。 今回は、７モードに分けます。
- STANDBY
- NORMAL
- CRANK
- DOUBLE_LINE
- SINGLE_LINE
- LANE_CHANGE
- OUT_OF_LANE
- STANDBY : スタートの合図を待っている
- NORMAL : 直線、ワインディング、カーブを走行
- CRANK : 左右に９０°曲がる
- DOUBLE_LINE : CRANKの目印検出
- SINGLE_LINE : LANE_CHANGEの目印検出
- LANE_CHANGE : 左右の車線変更
- OUT_OF_LANE : コースアウト
状態遷移図を眺めながら、状態を遷移させる条件を考えます。
実際の状態遷移は、タスク４が担当します。
コメントで処理を定義します。
void tsk2_proc(void)
{
/* STANDBY */
/* NORMAL */
/* CRANK */
/* DOUBLE_LINE */
/* SINGLE_LINE */
/* LANE_CHANGE */
/* OUT_OF_LANE */
}
switch文で、場合分けします。
UBYTE mode ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY : break ;
case NORMAL : break ;
case CRANK : break ;
case DOUBLE_LINE : break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
STANDBYモードでは、停止しています。
両モータのPWMのDUTY比は、０にします。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL : break ;
case CRANK : break ;
case DOUBLE_LINE : break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
NORMALモードでは、直線、ワインディング、カーブを
走行しています。この状態では、車体の中心にライン
中央の白線が来るように制御します。
路面センサーの情報を利用し、左右のDUTY比を決定します。
関数normal_procを定義して、その関数に処理を一任します。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_DUTY_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK : break ;
case DOUBLE_LINE : break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
CRANKモードでは、左か右のどちらかに車体を回転させながら
移動します。方向判断のために、フラグRLFLAGを使いますが、
RLFLAGの設定は、タスク４に一任します。
CRANKモードは、より詳細な動きを定義する必要があります。
関数crank_procを定義して、処理を一任します。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE : break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
DOUBLE_LINEモードは、２本の平行白線を検出後、右クランク
あるいは左クランクに到達するまで、スロー走行させます。
２本の平行白線に、どういう角度で車体が進入したかで
細かな制御が必要になります。関数wline_procを定義し
処理を一任します。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE :
wline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
SINGLE_LINEモードは、左右が欠けた２本の平行白線を検出後
右レーンチェンジあるいは左レーンチェンジに到達するまで
スロー走行させます。
欠けた平行白線に、どういう角度で車体が進入したかで
細かな制御が必要になります。関数sline_procを定義し
処理を一任します。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE :
wline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case SINGLE_LINE :
sline_proc(sensor_information);
left__duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case LANE_CHANGE : break ;
case OUT_OF_LANE : break ;
}
}
LANE_CHANGEモードは、誘導に利用するセンターラインがない
状態で左あるいは右に走行します。どちらの方向に進むかは
タスク４がフラグRLFLAGに入れた情報で判断します。
フラグとセンサー情報の組み合わせで、DUTY比を決定します。
関数lane_procを定義し、処理を一任します。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE :
wline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case SINGLE_LINE :
sline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case LANE_CHANGE :
lane_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case OUT_OF_LANE : break ;
}
}
OUT_OF_LANEモードは、コースアウトしているので、マシンが
あらぬ方向に行かないように、モータの回転を止めます。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case STANDBY :
left_duty = 0 ;
right_duty = 0 ;
break ;
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE :
wline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case SINGLE_LINE :
sline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case LANE_CHANGE :
lane_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case OUT_OF_LANE :
left_duty = 0 ;
right_duty = 0 ;
break ;
}
}
STANDBYとOUT_OF_LANEモードは、同じことをしているので
一つにまとめてしまいます。
UBYTE mode ;
UBYTE left_duty ;
UBYTE right_duty ;
void tsk2_proc(void)
{
switch ( mode ) {
case NORMAL :
normal_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case CRANK :
crank_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case DOUBLE_LINE :
wline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case SINGLE_LINE :
sline_proc(sensor_information);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case LANE_CHANGE :
lane_proc(sensor_information,RLFLAG);
left_duty = get_duty_ratio( LEFT ) ;
right_duty = get_duty_ratio( RIGHT ) ;
break ;
case STANDBY :
case OUT_OF_LANE :
default :
left_duty = 0 ;
right_duty = 0 ;
break ;
}
}
タスク３を定義します。
他のタスクで、モータスピードを計算してあるので
その値を内部にあるPWM回路に与えます。
また、DUTY比の値を４つの範囲に分割して、ＬＥＤ表示
します。
コメントで動作を記述します。
void tsk3_proc(void)
{
/* 左右のモータスピードを取得 */
/* 左右のPWM値計算 */
/* 左右のPWM値設定 */
/* 左右のDUTY比表示 */
}
モータスピードを取得します。
void tsk3_proc(void)
{
UWORD left ;
UWORD right ;
/* 左右のモータスピードを取得 */
left = left_duty ;
right = right_duty ;
/* 左右のPWM値計算 */
/* 左右のPWM値設定 */
}
PWMのDUTY比に相当する値を計算し直します。
関数update_motorとcalc_duty_rangeは、定義されていると仮定します。
#define DLEDS PB.DR.BYTE
void tsk3_proc(void)
{
UWORD left ;
UWORD right ;
UBYTE led_left ;
UBYTE led_right ;
/* 左右のモータスピードを取得 */
left = left_duty ;
right = right_duty ;
/* 左右のPWM値計算 */
left *= 30 ;
right *= 30 ;
update_motor(left,right);
/* 左右のPWM値設定 */
led_left = calc_duty_range(left_duty ) ;
led_right = calc_duty_range(right_duty) ;
DLEDS = ~((led_left << 4) | led_right) ;
}
まとめます。
#define DLEDS PB.DR.BYTE
void tsk3_proc(void)
{
UWORD left ;
UWORD right ;
UBYTE led_left ;
UBYTE led_right ;
/* 左右のモータスピードを取得 */
left = left_duty ;
right = right_duty ;
/* 左右のPWM値計算 */
left *= 30 ;
right *= 30 ;
update_motor(left,right);
/* 左右のPWM値設定 */
led_left = calc_duty_range(left_duty ) ;
led_right = calc_duty_range(right_duty) ;
DLEDS = ~((led_left << 4) | led_right) ;
}
タスク４を定義します。
センサー情報を３個保存してあります。
この履歴から、車体がどちらの方向に動いているか
スピードがどの程度かを判断し、モードを決定します。
コメントで大まかな動作を記述します。
void tsk4_proc(void)
{
/* NORMALモードの判定 */
/* CRANKモードの判定 */
/* DOUBLE_LINEモードの判定 */
/* SINGLE_LINEモードの判定 */
/* LANE_CHANGEモードの判定 */
/* STANDBYモードの判定 */
/* OUT_OF_LANEモードの判定 */
}
switch文を利用し、モードごとの判定に分割します。
void tsk4_proc(void)
{
switch ( mode ) {
case NORMAL : break ;
case CRANK : break ;
case DOUBLE_LINE : break ;
case SINGLE_LINE : break ;
case LANE_CHANGE : break ;
case STANDBY : break ;
case OUT_OF_LANE : break ;
}
}
モードを変更するかは、下位関数に一任します。
ただし、STANDBY、OUT_OF_LANEは、モードをキープします。
void tsk4_proc(void)
{
UBYTE next_mode ;
switch ( mode ) {
case NORMAL :
next_mode = judge_normal() ;
break ;
case CRANK :
next_mode = judge_crank() ;
break ;
case DOUBLE_LINE :
next_mode = judge_double_line() ;
break ;
case SINGLE_LINE :
next_mode = judge_single_line() ;
break ;
case LANE_CHANGE :
next_mode = judge_lane_change() ;
break ;
case STANDBY :
case OUT_OF_LANE :
next_mode = mode ;
break ;
defualt :
break ;
}
mode = next_mode ;
}
平行白線を検出したかどうかを、このタスク中でＬＥＤに反映させます。
void tsk4_proc(void)
{
UBYTE next_mode ;
/* default */
CROSS_WHITE = LED_OFF ;
/* judge */
switch ( mode ) {
case NORMAL :
next_mode = judge_normal() ;
break ;
case CRANK :
next_mode = judge_crank() ;
break ;
case DOUBLE_LINE :
next_mode = judge_double_line() ;
break ;
case SINGLE_LINE :
next_mode = judge_single_line() ;
break ;
case LANE_CHANGE :
next_mode = judge_lane_change() ;
break ;
case STANDBY :
case OUT_OF_LANE :
next_mode = mode ;
break ;
defualt :
break ;
}
mode = next_mode ;
if ( mode == DOUBLE_LINE || mode == SINGLE_LINE ) {
CROSS_WHITE = LED_ON ;
}
}
まとめます。
void tsk4_proc(void)
{
UBYTE next_mode ;
/* default */
CROSS_WHITE = LED_OFF ;
/* judge */
switch ( mode ) {
case NORMAL :
next_mode = judge_normal() ;
break ;
case CRANK :
next_mode = judge_crank() ;
break ;
case DOUBLE_LINE :
next_mode = judge_double_line() ;
break ;
case SINGLE_LINE :
next_mode = judge_single_line() ;
break ;
case LANE_CHANGE :
next_mode = judge_lane_change() ;
break ;
case STANDBY :
case OUT_OF_LANE :
next_mode = mode ;
break ;
defualt :
break ;
}
mode = next_mode ;
if ( mode == DOUBLE_LINE || mode == SINGLE_LINE ) {
CROSS_WHITE = LED_ON ;
}
}
変数、フラグ定義
タスクで利用しているフラグを、タスク関数から拾い出します。 タスク０で利用している変数、フラグを拾います。 変数 なし フラグ なし タスク１で利用している変数、フラグを拾います。 変数 sensor_information フラグ なし タスク２で利用している変数、フラグを拾います。 変数 mode left_duty right_duty フラグ RLFLAG タスク３で利用している変数、フラグを拾います。 変数 なし フラグ なし タスク４で利用している変数、フラグを拾います。 変数 led_count フラグ LFLAG フラグは、１ビットのデータ処理に利用するので、新たにデータ型 を定義して利用します。 typedef union { struct { unsigned B7:1; unsigned B6:1; unsigned B5:1; unsigned B4:1; unsigned B3:1; unsigned B2:1; unsigned B1:1; unsigned B0:1; } BIT ; unsigned char DR ; } FLAGSP ; FLAGSP x_flags ; #define RLFLAG x_flags.BIT.B0 変数を定義します。 typedef unsigned char UBYTE ; typedef unsigned short UWORD ; UWORD time_count ; UBYTE led_count ; UBYTE count ;サブ関数定義
各タスクで利用する関数を、拾いあげて定義します。 タスク０で利用している関数を拾います。 関数 なし タスク１で利用している関数を拾います。 関数 store_sensor ８ビットの情報を、単純にレジスタファイルに入力します。 レジスタファイルを定義します。 UBYTE sensor_data[3] ; 古い情報から最新の情報を、保存するとしてシフトレジスタ 構成します。 void store_sensor(UBYTE x) { /* shift */ *(sensor_data+0) = *(sensor_data+1) ; *(sensor_data+1) = *(sensor_data+2) ; /* store */ *(sensor_data+2) = x ; } タスク２で利用している関数を拾います。 関数 normal_proc crank_proc wline_proc sline_proc lane_proc get_duty_ratio update_motor 関数get_duty_ratioは、他の関数が確定した値を取り出せる仕様にします。 入力パラメータは、左か右を指定して、値を返します。 #define RIGHT 0 #define LEFT RIGHT+1 UBYTE xright_duty ; UBYTE xleft_duty ; UBYTE get_duty_ratio(UBYTE which) { UBYTE result ; /* default */ result = xright_duty ; /* judge */ if ( which ) { result = xleft_duty ; } /* */ return result ; } ノーマルモードに入っている場合は、センサー情報 のみをパラメータとして指定します。 関数の雛形を定義します。 void normal_proc(UBYTE sinfo); ノーマルの状態遷移図を作成し、移動方法を考えます。
void normal_proc(UBYTE sinfo)
{
UBYTE dleft ;
UBYTE dright ;
UBYTE pre_state ;
/* get previous state */
pre_state = *(sensor_data+3) ;
/* center */
if ( sinfo == 0x18 ) {
dleft = 35 ;
dright = 35 ;
}
/* right1 */
if ( sinfo == 0x38 ) {
dleft = 45 ;
dright = 25 ;
}
/* right2 */
if ( sinfo == 0x30 ) {
dleft = 45 ;
dright = 20 ;
}
/* right3 */
if ( sinfo == 0x70 ) {
dleft = 45 ;
dright = 15 ;
}
/* right4 */
if ( sinfo == 0x60 ) {
dleft = 45 ;
dright = 10 ;
}
/* right5 */
if ( sinfo == 0xe0 ) {
dleft = 45 ;
dright = 5 ;
}
/* left1 */
if ( sinfo == 0x1c ) {
dleft = 25 ;
dright = 25 ;
}
/*left2*/
if ( sinfo == 0x0c ) {
dleft = 20 ;
dright = 35 ;
}
/*left3*/
if ( sinfo == 0x0e ) {
dleft = 15 ;
dright = 35 ;
}
/*left4*/
if ( sinfo == 0x06 ) {
dleft = 10 ;
dright = 35 ;
}
/*left5*/
if ( sinfo == 0x07 ) {
dleft = 5 ;
dright = 35 ;
}
/* same state */
if ( sinfo == 0xc1 ) {
/* previous state */
if ( pre_state == 0x81 ) {
dleft = 0 ;
dright = 40 ;
}
if ( pre_state == 0xc0 ) {
dleft = 40 ;
dright = 0 ;
}
}
if ( sinfo == 0x81 ) {
/* previous state */
if ( pre_state == 0x83 ) {
dleft = 0 ;
dright = 40 ;
}
if ( pre_state == 0xc1 ) {
dleft = 40 ;
dright = 0 ;
}
}
if ( sinfo == 0x83 ) {
/* previous state */
if ( pre_state == 0x03 ) {
dleft = 0 ;
dright = 40 ;
}
if ( pre_state == 0x81 ) {
dleft = 40 ;
dright = 0 ;
}
}
if ( sinfo == 0xc0 ) {
/* previous state */
if ( pre_state == 0xc1 ) {
dleft = 0 ;
dright = 40 ;
}
if ( pre_state == 0xe0 ) {
dleft = 40 ;
dright = 0 ;
}
}
if ( sinfo == 0x03 ) {
/* previous state */
if ( pre_state == 0x07 ) {
dleft = 0 ;
dright = 40 ;
}
if ( pre_state == 0x83 ) {
dleft = 40 ;
dright = 0 ;
}
}
/* update */
xleft_duty = dleft ;
xright_duty = dright ;
}
※この状態では、まだコースアウトしてしまいます。
実際に走行させて、パターンを増やします。
クランクモードに入っている場合は、センサー情報と左右
のどちらに車線変更するかをパラメータ指定します。
関数の雛形を定義します。
void crank_proc(UBYTE sinfo,UBYTE which);
クランクの状態遷移図を作成し、移動方法を考えます。
void crank_proc(UBYTE sinfo,UBYTE which)
{
UBYTE dleft ;
UBYTE dright ;
/* center */
if ( sinfo == 0x18 ) {
dleft = 50 ;
dright = 50 ;
}
/* bit right */
if ( sinfo == 0x0c ) {
dleft = 30 ;
dright = 50 ;
}
/* right */
if ( sinfo == 0x06 ) {
dleft = 20 ;
dright = 60 ;
}
/* bit left */
if ( sinfo == 0x30 ) {
dleft = 50 ;
dright = 30 ;
}
/* left */
if ( sinfo == 0x60 ) {
dleft = 60 ;
dright = 20 ;
}
/* update */
xleft_duty = dleft ;
xright_duty = dright ;
}
ダブルラインモードに入っている場合は、センサー情報
のみをパラメータとして指定します。
関数の雛形を定義します。
void wline_proc(UBYTE sinfo);
ダブルラインの状態遷移図を作成し、移動方法を考えます。
void wline_proc(UBYTE sinfo)
{
UBYTE dleft ;
UBYTE dright ;
/* center */
if ( sinfo == 0x18 ) {
dleft = 50 ;
dright = 50 ;
}
/* bit right */
if ( sinfo == 0x0c ) {
dleft = 30 ;
dright = 50 ;
}
/* right */
if ( sinfo == 0x06 ) {
dleft = 20 ;
dright = 60 ;
}
/* bit left */
if ( sinfo == 0x30 ) {
dleft = 50 ;
dright = 30 ;
}
/* left */
if ( sinfo == 0x60 ) {
dleft = 60 ;
dright = 20 ;
}
/* update */
xleft_duty = dleft ;
xright_duty = dright ;
}
シングルラインモードに入っている場合は、センサー情報
のみをパラメータとして指定します。
関数の雛形を定義します。
void sline_proc(UBYTE sinfo);
シングルラインの状態遷移図を作成し、移動方法を考えます。
void sline_proc(UBYTE sinfo)
{
UBYTE dleft ;
UBYTE dright ;
/* center */
if ( sinfo == 0x18 ) {
dleft = 50 ;
dright = 50 ;
}
/* bit right */
if ( sinfo == 0x0c ) {
dleft = 30 ;
dright = 50 ;
}
/* right */
if ( sinfo == 0x06 ) {
dleft = 20 ;
dright = 60 ;
}
/* bit left */
if ( sinfo == 0x30 ) {
dleft = 50 ;
dright = 30 ;
}
/* left */
if ( sinfo == 0x60 ) {
dleft = 60 ;
dright = 20 ;
}
/* update */
xleft_duty = dleft ;
xright_duty = dright ;
}
レーンチェンジモードに入っている場合は、センサー情報
と左右のどちらに車線変更するかをパラメータ指定します。
関数の雛形を定義します。
void lane_proc(UBYTE sinfo,UBYTE which);
レーンチェンジの状態遷移図を作成し、移動方法を考えます。
void lane_proc(UBYTE sinfo,UBYTE which)
{
UBYTE dleft ;
UBYTE dright ;
/* blind move */
if ( sinfo == 0x00 ) {
dleft = 25 ;
dright = 25 ;
}
/* center */
if ( sinfo == 0x18 ) {
if ( which == LEFT_DIR ) { /* LEFT */
dleft = 25 ;
dright = 45 ;
} else { /* RIGHT */
dleft = 45 ;
dright = 25 ;
}
}
if ( sinfo == 0x18 ) {
if ( which == LEFT_DIR ) { /* LEFT */
dleft = 25 ;
dright = 45 ;
} else { /* RIGHT */
dleft = 45 ;
dright = 25 ;
}
}
/* bit right */
if ( sinfo == 0x8c ) {
if ( which == LEFT_DIR ) { /* LEFT */
dleft = 25 ;
dright = 55 ;
} else { /* RIGHT */
dleft = 55 ;
dright = 25 ;
}
}
if ( sinfo == 0xcc ) {
dleft = 30 ;
dright = 50 ;
if ( which == LEFT_DIR ) { /* LEFT */
dleft = 15 ;
dright = 55 ;
} else { /* RIGHT */
dleft = 55 ;
dright = 15 ;
}
}
/* right */
if ( sinfo == 0x06 ) {
if ( which == LEFT_DIR ) { /* LEFT */
dleft = 25 ;
dright = 55 ;
} else { /* RIGHT */
dleft = 55 ;
dright = 25 ;
}
}
/* bit left */
if ( sinfo == 0x30 ) {
dleft = 50 ;
dright = 30 ;
}
/* left */
if ( sinfo == 0x60 ) {
dleft = 60 ;
dright = 20 ;
}
/* update */
xleft_duty = dleft ;
xright_duty = dright ;
}
モータのDUTY比を変更すればよいので、H8/3048Fに内蔵されている
ITU(Integrated Timer Unit)の３、４を利用します。
void update_motor(UWORD dl,UWORD dr)
{
ITU3.BRB = dl;
ITU4.BRA = dr;
}
ITUを利用するには、モード設定と初期化が必要なので
ここで、定義しておきます。
モードは、カウンタを停止し、ＰＷＭモードにします。
void init_pwm(void)
{
ITU.TSTR.BIT.STR3 = 0; /* stop the counter 3 */
ITU.TOER.BYTE = 0; /* disable B5 and B4 as clock outputs */
ITU3.TCR.BYTE = 0xa0; /* set mode */
ITU3.TCNT = 0x00; /* stop timer counter */
ITU.TFCR.BYTE = 0xf6; /* disable set complimentary PWM mode */
ITU3.GRA = 3000 ; /* MAX PWM ratio */
ITU4.GRA = 0 ; /* LEFT MOTOR ratio */
ITU3.GRB = 0 ; /* RIGHT MOTOR ratio */
}
モータの回転開始を指定する関数を定義します。
void start_pwm(void)
{
ITU.TOER.BYTE = 0xff; /* */
ITU.TSTR.BIT.STR3 = 1; /* start ITU3 */
ITU3.BRB = 300 ; /* right DUTY ratio */
ITU4.BRA = 300 ; /* left DUTY ratio */
}
タスク３で利用している関数を拾います。
関数 calc_duty_range
DUTY比の表示は、８個のＬＥＤだけなので、左右に４個ずつ
割当てて表示することにします。
ＬＥＤ４個に５状態を表示するとし、次のように
範囲と点灯状態を指定します。
0 - 24 => 0001 (2^0ビット点灯)
25 - 49 => 0010 (2^1ビット点灯)
50 - 74 => 0100 (2^2ビット点灯)
75 - 99 => 1000 (2^3ビット点灯)
範囲を判定して、ビットを立てます。
UBYTE calc_duty_range(UBYTE x)
{
UBYTE result ;
/* judge */
if ( x < 24 ) { result = (1 << 0); }
if ( 24 < x && x < 50 ) { result = (1 << 1); }
if ( 50 < x && x < 75 ) { result = (1 << 2); }
if ( x > 75 ) { result = (1 << 3); }
/* */
return result ;
}
タスク４で利用している関数を拾います。
関数 judge_normal judge_crank judge_double_line
judge_single_line judge_lane_change
関数judge_lane_changeが返す値は、NORMAL、LANE_CHANGE、OUT_OF_LANE
のどれかになります。関数のカタチを整えます。
UBYTE judge_lane_change(void)
{
UBYTE result ;
return result ;
}
センサー情報は３個あるので、それらから次の状態を決定します。
５個の状態を、排他的論理和を利用して、どちらの方法に移動して
いるかを判断します。
UBYTE judge_lane_change(void)
{
UBYTE result ;
UBYTE info ;
/* shuffle */
info = *(sensor_data+0) ^ *(sensor_data+1) ^ *(sensor_data+2) ;
/* */
return result ;
}
センサー情報は３個あるので、それらから次の状態を決定します。
LANE_CHANGEからNORMALへの状態遷移は、左右のセンサーで白を検出
した場合です。
UBYTE judge_lane_change(void)
{
UBYTE result ;
return result ;
}
関数judge_crankが返す値は、NORMAL、CRANKのいずれか
になります。関数のカタチを整えます。
UBYTE judge_crank(void)
{
UBYTE result ;
return result ;
}
センサー情報は５個あるので、それらから次の状態を決定します。
CRANKからNORMALへの状態遷移は、左右のセンサーで白を検出
した場合です。
UBYTE judge_crank(void)
{
UBYTE result ;
return result ;
}
タスクの初期状態定義
タスクの関数を定義したので、初期状態を考えます。- スタートボタンによるトリガー待ち タスク０
- 路面センサーから情報取得 タスク１
- センサー情報からモータスピード計算 タスク２
- モータスピード設定 タスク３
- モード更新 タスク４
I/O初期化
回路図から、ポート４、Ａ、Ｂの初期値と方向を決めます。
ポート４には、ＬＥＤが接続されています。
方向は出力で、最初はＬＥＤを消灯させます。
#define P4DR P4.DR.BYTE
#define P4DDR P4.DDR
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
ポートＡにも、ＬＥＤが接続されています。
方向は出力で、最初はＬＥＤを消灯させます。
#define PADR PA.DR.BYTE
#define PADDR PA.DDR
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
ポートＢには、ＬＥＤ、ボタンスイッチ、モータ制御
回路が接続されます。
ビット７、６には、ＬＥＤが接続されるので出力に
設定します。最初は、消灯すればよいので１を出力
します。
#define PBDR PB.DR.BYTE
#define PBDDR PB.DDR
PBDR = (0x03 << 6) ; /* turn off LEDs */
PBDDR = (0x03 << 6) ; /* 7,6 :outputs */
ビット５、４には、ボタンスイッチが接続されるので
入力に設定します。
#define PBDR PB.DR.BYTE
#define PBDDR PB.DDR
PBDR = (0x03 << 6) ; /* turn off LEDs */
PBDDR = (0x03 << 6) | (0x00 << 4) ;
/* 7,6 : outputs 5,4 : inputs */
ビット２、０には、モータの回転を制御するための
PWM波形を出力するので、出力に設定します。また
最初は、モータ回転は、停止させておきます。
入力に設定します。
#define PBDR PB.DR.BYTE
#define PBDDR PB.DDR
PBDR = (0x03 << 6) | 0x00 ;
/* turn off LEDs , stop motoros */
PBDDR = (0x03 << 6) | (0x00 << 4) ;
/* 5,4 : inputs Others : outputs */
ひとつの関数にまとめます。
void user_initialize(void)
{
/* PORT 4 */
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xcf ; /* turn off LEDs , stop motoros */
PBDDR = 0xcf ; /* 5,4 : inputs Others : outputs */
}
I/Oには、ITUがあるので、初期化の関数が定義されて
いるとして、付加します。
void user_initialize(void)
{
/* PORT 4 */
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xcf ; /* turn off LEDs , stop motoros */
PBDDR = 0xcf ; /* 5,4 : inputs Others : outputs */
/* initialize timer0(ITU0) */
init_timer0();
}
モータ制御のためにPWMを利用します。初期化の関数が定義されて
いるとして、付加します。
void user_initialize(void)
{
/* PORT 4 */
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xcf ; /* turn off LEDs , stop motoros */
PBDDR = 0xcf ; /* 5,4 : inputs Others : outputs */
/* initialize timer0(ITU0) */
init_timer0();
/* initialize PWM module */
init_pwm();
}
システムの状態を変数modeに設定します。
void user_initialize(void)
{
/* PORT 4 */
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xcf ; /* turn off LEDs , stop motoros */
PBDDR = 0xcf ; /* 5,4 : inputs Others : outputs */
/* initialize timer0(ITU0) */
init_timer0();
/* initialize PWM module */
init_pwm();
/* initialize MODE */
mode = STANDBY ;
}
USOの10msのタイミングをとるための変数os_cntを初期化します。
void user_initialize(void)
{
/* PORT 4 */
P4DR = 0xff ; /* turn off all LEDs */
P4DDR = 0xff ; /* all outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xcf ; /* turn off LEDs , stop motoros */
PBDDR = 0xcf ; /* 5,4 : inputs Others : outputs */
/* initialize timer0(ITU0) */
init_timer0();
/* initialize PWM module */
init_pwm();
/* initialize MODE */
mode = STANDBY ;
/* initialize RTOS */
os_cnt = 0 ;
}
割込み定義
USOを利用するためには、10msごとのタイマー割込みが必要です。 タイマー割込みが発生したときに、USOのコンフィグレータが 生成した関数timer_handlerを呼びだします。 タイマー割込みの周期が10msであれば、そのまま呼出せば 充分です。1msや2msの場合は、カウンタを用意し、割込み 回数をカウントします。 手元にあったH8/3048Fは、1msの割込みだけを実現できたので カウンタを用意して、割込みを１０回カウントしてから関数 timer_handlerを呼び出しました。 #pragma interrupt int_imia0 void int_imia0(void) { UBYTE dummy ; /* clear flag */ dummy = ITU0.TSR.BIT.IMFA ; ITU0.TSR.BIT.IMFA = OFF ; /* os handling */ os_cnt++ ; if ( os_cnt == 10 ) { os_cnt = 0 ; timer_handler(); } } H8/3048FのITUは多機能であるため、データシートを一読した だけでは理解できませんでした。 英文データシート掲載のサンプルコードを参照し、実機 テストでやっと理解できました。 次の手順で、初期化します。- ITU動作停止
- 出力レジスタ設定
- I/O制御レジスタ設定
- 制御レジスタ設定
- 割込み指定（割込みを使う場合のみ）
- カウンタ、レジスタ値に初期値設定
- ITU動作開始
タスク内容見直し
H8/3048Fボードにプログラムを転送し、スタートボタンスイッチを 押しても、走行しないので、原因を探ってみました。 モータ制御回路につけているＬＥＤが消灯しているので、ファーム ウエアから、制御信号が出ていないことが分かりました。 タスク０の中に、モードを判定してモータ回転開始を指定する 記述がなかったので、付け加えます。 void tsk0_proc(void) { /* no push */ if ( START_BUTTON == ON ) return ; /* push */ rsm_tsk( TSK_ID2 ); rsm_tsk( TSK_ID3 ); rsm_tsk( TSK_ID4 ); /* start PWM */ start_pwm(); /* change mode */ mode = NORMAL ; slp_tsk(); }全ソースコード
#define SYS_CLOCK_14_7456 #include "3048.h" typedef unsigned char UBYTE ; typedef unsigned short UWORD ; typedef unsigned long ULONG ; typedef signed short SWORD ; typedef struct { void (*tsk)(void); UWORD wcount ; } TCBP ; #define TTS_SUSPEND 0 #define TTS_WAIT TTS_SUSPEND+1 #define TTS_READY TTS_SUSPEND+2 #define OFF 0 #define ON OFF+1 #define NO 0 #define YES NO+1 #define MASKFF 0xff #define MASK07 0x07 #define MASK03 0x03 #define P4DR P4.DR.BYTE #define P4DDR P4.DDR #define P7DR P7.DR.BYTE #define PADR PA.DR.BYTE #define PADDR PA.DDR #define PBDR PB.DR.BYTE #define PBDDR PB.DDR #define SETRG PB.DR.BIT.B4 #define SENSORS P7.DR.BYTE #define ISENSOR PA.DR.BYTE #define DLEDS PB.DR.BYTE #define CROSS_WHITE PB.DR.BIT.B7 volatile UBYTE sensor_information ; #define TSK_ID_MAX 5 #define TSK_ID0 0 #define TSK_ID1 1 #define TSK_ID2 2 #define TSK_ID3 3 #define TSK_ID4 4 volatile TCBP tcb[TSK_ID_MAX]; #define LED_OFF 1 #define LED_ON 0 #define MULTV 30 #define SECNT_MAX 10 #define START_BUTTON PB.DR.BIT.B4 #define STANDBY 0 #define NORMAL 1 #define CRANK 2 #define DOUBLE_LINE 3 #define SINGLE_LINE 4 #define LANE_CHANGE 5 #define OUT_OF_LANE 6 #define RLFLAG 1 #define RIGHT 0 #define LEFT RIGHT+1 #define RIGHT_DIR 1 #define LEFT_DIR RIGHT_DIR+1 /* variables */ volatile UBYTE run_tsk ; volatile UWORD ready ; volatile UWORD vldtsk ; volatile UWORD suspend ; volatile UBYTE sflag ; /* sensor flag */ volatile UBYTE seflag ; /* start exit flag */ volatile UBYTE secnt ; /* start exit flag update interval counter */ volatile UBYTE mode ; volatile UBYTE left_duty ; volatile UBYTE right_duty ; volatile UBYTE sensor_data[3] ; volatile UBYTE xright_duty ; volatile UBYTE xleft_duty ; /* USO functions */ void init_os(void); void cre_tsk(UBYTE tid,void (*tsk)(void)); void sta_tsk(UBYTE tid,UBYTE sta); void rsm_tsk(UBYTE tid); void sus_tsk(UBYTE tid); void slp_tsk(void); void wai_tsk(UWORD x); UBYTE is_tsk_ready(UBYTE tid); /* task functions */ void tsk0_proc(void); void tsk1_proc(void); void tsk2_proc(void); void tsk3_proc(void); void tsk4_proc(void); /* functions */ void init_user(void); void set_duty(UBYTE lx,UBYTE rx); void store_sensor(UBYTE x); UBYTE get_duty_ratio(UBYTE which); void normal_proc(UBYTE sinfo); void crank_proc(UBYTE sinfo,UBYTE which); void wline_proc(UBYTE sinfo); void sline_proc(UBYTE sinfo); void lane_proc(UBYTE sinfo,UBYTE which); void update_motor(UWORD dl,UWORD dr); void init_pwm(void); void start_pwm(void); UBYTE calc_duty_range(UBYTE x); UBYTE judge_normal(void); UBYTE judge_crank(void); UBYTE judge_double_line(void); UBYTE judge_single_line(void); UBYTE judge_lane_change(void); int main(void) { TCBP pcur_tsk ; /* disable interrupt */ DI; /* initialize */ init_user(); /* initialize RTOS */ init_os(); /* regist */ cre_tsk(TSK_ID0,tsk0_proc); cre_tsk(TSK_ID1,tsk1_proc); cre_tsk(TSK_ID2,tsk2_proc); cre_tsk(TSK_ID3,tsk3_proc); cre_tsk(TSK_ID4,tsk4_proc); /* initialize task state */ sta_tsk(TSK_ID0,TTS_READY); sta_tsk(TSK_ID1,TTS_SUSPEND); sta_tsk(TSK_ID2,TTS_SUSPEND); sta_tsk(TSK_ID3,TTS_SUSPEND); sta_tsk(TSK_ID4,TTS_SUSPEND); /* enable interrupt */ EI; /* endless loop */ while ( ON ) { pcur_tsk = tcb[run_tsk] ; if ( is_tsk_ready( run_tsk ) == YES ) { (*(pcur_tsk.tsk))(); } run_tsk++; if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; } } return 0 ; } void init_user(void) { /* initialize I/O PORT 4 */ P4DR = 0x00 ; P4DDR = MASKFF ; /* output */ /* initialize I/O PORT A */ PADR = MASKFF ; /* turn off all LED */ PADDR = MASKFF ; /* output */ /* initialize I/O PORT B */ PBDR = 0xCF ; PBDDR = 0xCF ; /* initialize ITU0 compare match A */ { ITU0.TCNT = 0 ; /* clear counter */ ITU0.GRA = 18394 ; /* initialize counter */ ITU0.GRB = 20000 ; /* initialize counter */ /* select compare match A clear and prescaler /8 */ ITU0.TCR.BYTE = (1 << 5) | (3 << 0) ; /* enable compare match A interruption */ ITU0.TIER.BIT.IMIEA = ON ; } /* start ITU0 */ ITU.TSTR.BIT.STR0 = ON ; /* initialize PWM */ init_pwm(); } void set_duty(UBYTE lx,UBYTE rx) { ITU3.BRB = MULTV * lx; ITU4.BRA = MULTV * rx; } /* ITU0 interrupt */ /* 1ms */ void int_imia0 (void) { UBYTE dummy ; /* read interrupt flag */ dummy = ITU0.TSR.BYTE ; /* clear interrupt flag */ ITU0.TSR.BIT.IMFA = OFF ; /* set flag */ sflag = ON ; /* count up */ secnt++ ; /* judge */ if ( secnt == SECNT_MAX ) { secnt = 0 ; seflag = ON ; } } /* USO functions */ void init_os(void) { ready = vldtsk = suspend = 0 ; } void cre_tsk(UBYTE tid,void (*tsk)(void)) { if ( tid >= TSK_ID_MAX ) return ; vldtsk |= (1 << tid) ; tcb[tid].tsk = tsk; tcb[tid].wcount = 0; } void sta_tsk(UBYTE tid,UBYTE sta) { if ( tid >= TSK_ID_MAX ) return ; if ( sta == TTS_READY ) { ready |= (1 << tid); suspend &= ~(1 << tid); } if ( sta == TTS_WAIT ) { ready &= ~(1 << tid); suspend &= ~(1 << tid); } if ( sta == TTS_SUSPEND ) { ready &= ~(1 << tid); suspend |= (1 << tid); } } void rsm_tsk(UBYTE tid) { if ( tid >= TSK_ID_MAX ) return ; ready |= (1 << tid); suspend &= ~(1 << tid); } void sus_tsk(UBYTE tid) { if ( tid >= TSK_ID_MAX ) return ; ready &= ~(1 << tid); suspend |= (1 << tid); } void slp_tsk(void) { sus_tsk(run_tsk); } void wai_tsk(UWORD x) { ready &= ~(1 << run_tsk); suspend &= ~(1 << run_tsk); tcb[run_tsk].wcount = x ; } UBYTE is_tsk_ready(UBYTE tid) { return 0 ; } /* task functions */ void tsk0_proc(void) { /* no push */ if ( START_BUTTON == ON ) return ; /* push */ rsm_tsk( TSK_ID2 ); rsm_tsk( TSK_ID3 ); rsm_tsk( TSK_ID4 ); /* start PWM */ start_pwm(); /* change mode */ mode = NORMAL ; slp_tsk(); } void tsk1_proc(void) { /* get sensor infomations */ sensor_information = SENSORS ; /* store sensor informations */ store_sensor( sensor_information ) ; /* show sensor states */ ISENSOR = ~sensor_information ; } void tsk2_proc(void) { switch ( mode ) { case NORMAL : normal_proc(sensor_information); left_duty = get_duty_ratio( LEFT ) ; right_duty = get_duty_ratio( RIGHT ) ; break ; case CRANK : crank_proc(sensor_information,RLFLAG); left_duty = get_duty_ratio( LEFT ) ; right_duty = get_duty_ratio( RIGHT ) ; break ; case DOUBLE_LINE : wline_proc(sensor_information); left_duty = get_duty_ratio( LEFT ) ; right_duty = get_duty_ratio( RIGHT ) ; break ; case SINGLE_LINE : sline_proc(sensor_information); left_duty = get_duty_ratio( LEFT ) ; right_duty = get_duty_ratio( RIGHT ) ; break ; case LANE_CHANGE : lane_proc(sensor_information,RLFLAG); left_duty = get_duty_ratio( LEFT ) ; right_duty = get_duty_ratio( RIGHT ) ; break ; case STANDBY : case OUT_OF_LANE : default : left_duty = 0 ; right_duty = 0 ; break ; } } void tsk3_proc(void) { UWORD left ; UWORD right ; UBYTE led_left ; UBYTE led_right ; /* 左右のモータスピードを取得 */ left = left_duty ; right = right_duty ; /* 左右のPWM値計算 */ left *= 30 ; right *= 30 ; update_motor(left,right); /* 左右のPWM値設定 */ led_left = calc_duty_range(left_duty ) ; led_right = calc_duty_range(right_duty) ; DLEDS = ~((led_left << 4) | led_right) ; } void tsk4_proc(void) { UBYTE next_mode ; /* default */ CROSS_WHITE = LED_OFF ; /* judge */ switch ( mode ) { case NORMAL : next_mode = judge_normal() ; break ; case CRANK : next_mode = judge_crank() ; break ; case DOUBLE_LINE : next_mode = judge_double_line() ; break ; case SINGLE_LINE : next_mode = judge_single_line() ; break ; case LANE_CHANGE : next_mode = judge_lane_change() ; break ; case STANDBY : case OUT_OF_LANE : next_mode = mode ; break ; defualt : break ; } mode = next_mode ; if ( mode == DOUBLE_LINE || mode == SINGLE_LINE ) { CROSS_WHITE = LED_ON ; } } void store_sensor(UBYTE x) { /* shift */ *(sensor_data+0) = *(sensor_data+1) ; *(sensor_data+1) = *(sensor_data+2) ; /* store */ *(sensor_data+2) = x ; } UBYTE get_duty_ratio(UBYTE which) { UBYTE result ; /* default */ result = xright_duty ; /* judge */ if ( which ) { result = xleft_duty ; } /* */ return result ; } void normal_proc(UBYTE sinfo) { UBYTE dleft ; UBYTE dright ; UBYTE pre_state ; /* get previous state */ pre_state = *(sensor_data+3) ; /* center */ if ( sinfo == 0x18 ) { dleft = 35 ; dright = 35 ; } /* right1 */ if ( sinfo == 0x38 ) { dleft = 45 ; dright = 25 ; } /* right2 */ if ( sinfo == 0x30 ) { dleft = 45 ; dright = 20 ; } /* right3 */ if ( sinfo == 0x70 ) { dleft = 45 ; dright = 15 ; } /* right4 */ if ( sinfo == 0x60 ) { dleft = 45 ; dright = 10 ; } /* right5 */ if ( sinfo == 0xe0 ) { dleft = 45 ; dright = 5 ; } /* left1 */ if ( sinfo == 0x1c ) { dleft = 25 ; dright = 25 ; } /*left2*/ if ( sinfo == 0x0c ) { dleft = 20 ; dright = 35 ; } /*left3*/ if ( sinfo == 0x0e ) { dleft = 15 ; dright = 35 ; } /*left4*/ if ( sinfo == 0x06 ) { dleft = 10 ; dright = 35 ; } /*left5*/ if ( sinfo == 0x07 ) { dleft = 5 ; dright = 35 ; } /* same state */ if ( sinfo == 0xc1 ) { /* previous state */ if ( pre_state == 0x81 ) { dleft = 0 ; dright = 40 ; } if ( pre_state == 0xc0 ) { dleft = 40 ; dright = 0 ; } } if ( sinfo == 0x81 ) { /* previous state */ if ( pre_state == 0x83 ) { dleft = 0 ; dright = 40 ; } if ( pre_state == 0xc1 ) { dleft = 40 ; dright = 0 ; } } if ( sinfo == 0x83 ) { /* previous state */ if ( pre_state == 0x03 ) { dleft = 0 ; dright = 40 ; } if ( pre_state == 0x81 ) { dleft = 40 ; dright = 0 ; } } if ( sinfo == 0xc0 ) { /* previous state */ if ( pre_state == 0xc1 ) { dleft = 0 ; dright = 40 ; } if ( pre_state == 0xe0 ) { dleft = 40 ; dright = 0 ; } } if ( sinfo == 0x03 ) { /* previous state */ if ( pre_state == 0x07 ) { dleft = 0 ; dright = 40 ; } if ( pre_state == 0x83 ) { dleft = 40 ; dright = 0 ; } } /* update */ xleft_duty = dleft ; xright_duty = dright ; } void crank_proc(UBYTE sinfo,UBYTE which) { UBYTE dleft ; UBYTE dright ; /* center */ if ( sinfo == 0x18 ) { dleft = 50 ; dright = 50 ; } /* bit right */ if ( sinfo == 0x0c ) { dleft = 30 ; dright = 50 ; } /* right */ if ( sinfo == 0x06 ) { dleft = 20 ; dright = 60 ; } /* bit left */ if ( sinfo == 0x30 ) { dleft = 50 ; dright = 30 ; } /* left */ if ( sinfo == 0x60 ) { dleft = 60 ; dright = 20 ; } /* update */ xleft_duty = dleft ; xright_duty = dright ; } void wline_proc(UBYTE sinfo) { UBYTE dleft ; UBYTE dright ; /* center */ if ( sinfo == 0x18 ) { dleft = 50 ; dright = 50 ; } /* bit right */ if ( sinfo == 0x0c ) { dleft = 30 ; dright = 50 ; } /* right */ if ( sinfo == 0x06 ) { dleft = 20 ; dright = 60 ; } /* bit left */ if ( sinfo == 0x30 ) { dleft = 50 ; dright = 30 ; } /* left */ if ( sinfo == 0x60 ) { dleft = 60 ; dright = 20 ; } /* update */ xleft_duty = dleft ; xright_duty = dright ; } void sline_proc(UBYTE sinfo) { UBYTE dleft ; UBYTE dright ; /* center */ if ( sinfo == 0x18 ) { dleft = 50 ; dright = 50 ; } /* bit right */ if ( sinfo == 0x0c ) { dleft = 30 ; dright = 50 ; } /* right */ if ( sinfo == 0x06 ) { dleft = 20 ; dright = 60 ; } /* bit left */ if ( sinfo == 0x30 ) { dleft = 50 ; dright = 30 ; } /* left */ if ( sinfo == 0x60 ) { dleft = 60 ; dright = 20 ; } /* update */ xleft_duty = dleft ; xright_duty = dright ; } void lane_proc(UBYTE sinfo,UBYTE which) { UBYTE dleft ; UBYTE dright ; /* blind move */ if ( sinfo == 0x00 ) { dleft = 25 ; dright = 25 ; } /* center */ if ( sinfo == 0x18 ) { if ( which == LEFT_DIR ) { /* LEFT */ dleft = 25 ; dright = 45 ; } else { /* RIGHT */ dleft = 45 ; dright = 25 ; } } if ( sinfo == 0x18 ) { if ( which == LEFT_DIR ) { /* LEFT */ dleft = 25 ; dright = 45 ; } else { /* RIGHT */ dleft = 45 ; dright = 25 ; } } /* bit right */ if ( sinfo == 0x8c ) { if ( which == LEFT_DIR ) { /* LEFT */ dleft = 25 ; dright = 55 ; } else { /* RIGHT */ dleft = 55 ; dright = 25 ; } } if ( sinfo == 0xcc ) { dleft = 30 ; dright = 50 ; if ( which == LEFT_DIR ) { /* LEFT */ dleft = 15 ; dright = 55 ; } else { /* RIGHT */ dleft = 55 ; dright = 15 ; } } /* right */ if ( sinfo == 0x06 ) { if ( which == LEFT_DIR ) { /* LEFT */ dleft = 25 ; dright = 55 ; } else { /* RIGHT */ dleft = 55 ; dright = 25 ; } } /* bit left */ if ( sinfo == 0x30 ) { dleft = 50 ; dright = 30 ; } /* left */ if ( sinfo == 0x60 ) { dleft = 60 ; dright = 20 ; } /* update */ xleft_duty = dleft ; xright_duty = dright ; } void update_motor(UWORD dl,UWORD dr) { ITU3.BRB = dl; ITU4.BRA = dr; } void init_pwm(void) { ITU.TSTR.BIT.STR3 = 0; /* stop the counter 3 */ ITU.TOER.BYTE = 0; /* disable B5 and B4 as clock outputs */ ITU3.TCR.BYTE = 0xa0; /* set mode */ ITU3.TCNT = 0x00; /* stop timer counter */ ITU.TFCR.BYTE = 0xf6; /* disable set complimentary PWM mode */ ITU3.GRA = 3000 ; /* MAX PWM ratio */ ITU4.GRA = 0 ; /* LEFT MOTOR ratio */ ITU3.GRB = 0 ; /* RIGHT MOTOR ratio */ } void start_pwm(void) { ITU.TOER.BYTE = 0xff; /* */ ITU.TSTR.BIT.STR3 = 1; /* start ITU3 */ ITU3.BRB = 300 ; /* right DUTY ratio */ ITU4.BRA = 300 ; /* left DUTY ratio */ } UBYTE calc_duty_range(UBYTE x) { UBYTE result ; /* judge */ if ( x < 24 ) { result = (1 << 0); } if ( 24 < x && x < 50 ) { result = (1 << 1); } if ( 50 < x && x < 75 ) { result = (1 << 2); } if ( x > 75 ) { result = (1 << 3); } /* */ return result ; } UBYTE judge_normal(void) { UBYTE result ; result = 0 ; /* */ return result ; } UBYTE judge_crank(void) { UBYTE result ; result = 0 ; /* */ return result ; } UBYTE judge_double_line(void) { UBYTE result ; result = 0 ; /* */ return result ; } UBYTE judge_single_line(void) { UBYTE result ; result = 0 ; /* */ return result ; } UBYTE judge_lane_change(void) { UBYTE result ; UBYTE info ; /* shuffle */ info = *(sensor_data+0) ^ *(sensor_data+1) ^ *(sensor_data+2) ; /* */ return result ; }目次 前