ファームウエア変更
画像処理を変更したので、ファームウエアも
変更します。
ファームウエアから見ると、FPGAには16本のレジスタが
あります。レジスタアドレスを指定して、パラメータを
入出力しやすいように、モータ、カメラデータとエンコーダ
に分けたラッパー関数を作成します。
モータ用関数
typedef unsigned char UBYTE ;
void put_motor_duty(UBYTE x);
void put_motor_dir(UBYTE x);
UBYTE get_motor_duty(UBYTE fr);
UBYTE get_motor_dir(UBYTE fr);
DUTY比設定は、関数put_motor_dutyを使います。
DUTY比は０〜99で７ビットで指定できるので
前輪、後輪の区別に８ビット目を利用します。
０で前輪、１で後輪のDUTY比指定とします。
現在のDUTY比は、関数get_motor_dutyで
取得します。前輪、後輪の指定が必要と
なるので、引数を一つにします。
方向指定は、関数put_motor_dirを利用します。
DUTY比設定同様、８ビット目を前輪、後輪の
選択指定に利用します。方向は、０、１、２で
前輪は、２（左）、１（右）、０（中央）で指定
します。
後輪は２（後退）、１（前進）、０（停止）と
仕様を決めます。
エンコーダカウントパルス取得関数
typedef unsigned shoft UWORD ;
UWORD get_pulse_count(void);
FPGAのレジスタは、パルスカウントの上位
下位を８ビットに分けて出力してくるので
16ビットで参照できるようにします。
センターライン位置取得関数
UBYTE get_top(void);
UBYTE get_middle(void);
UBYTE get_bottom(void);
３ラインの各ラインにおける、センターライン
位置を、TOP、MIDDLE、BOTTOMに分けて取得する
ようにします。
ライン幅取得関数
UBYTE get_top_width(void);
UBYTE get_middle_width(void);
UBYTE get_bottom_width(void);
各ラインにおける、センターラインの幅を
TOP、MIDDLE、BOTTOMに分けて取得します。
ライン位置指定関数
void put_line_number(UBYTE which,UBYTE x);
TOP、MIDDLE、BOTTOMのどれかをwhichで
ライン位置をxで指定します。
これらの関数は、ARMとFPGAとのデータ交換でできて
いるため、データ交換用の２つの関数のラッパーに
なるようにします。
２つのデータ交換関数は、次のように定義しました。
#define MASKFF 0xff
UBYTE get_fpga_reg(UBYTE ax)
{
volatile UBYTE adr ;
volatile UBYTE result ;
/* judge */
if ( ax > 15 ) { return 0 ; }
/* change inputs */
GP3DAT &= 0x00ffffff ;
/* address */
adr = 0x10+ax ;
/* set address and enable OE */
GP2DAT &= 0xff00ffff ;
GP2DAT |= (adr << 16) ;
/* get data */
result = GP3DAT & MASKFF ;
/* change outputs */
GP3DAT |= 0xff000000 ;
return result ;
}
#define S_TRG 22
void put_fpga_reg(UBYTE ax,UBYTE dx)
{
volatile UBYTE adr ;
/* judge */
if ( ax > 15 ) return ;
/* address */
adr = 0x20+ax ;
/* send address */
GP2DAT &= 0xff00ffff ;
GP2DAT |= (adr << 16) ;
/* send data */
GP3DAT &= 0xff00ffff ;
GP3DAT |= (dx << 16) ;
/* trigger */
GP2DAT |= (1 << S_TRG) ; /* trigger H */
adr = 0 ; /* delay */
GP2DAT &= ~(1 << S_TRG) ; /* trigger L */
}
２関数を土台にして、ラッパー関数を定義します。
void put_motor_duty(UBYTE x)
{
volatile UBYTE adr ;
volatile UBYTE dat ;
/* address */
adr = FP_ADR ;
if ( x & 0x80 ) { adr = RP_ADR ; }
/* data */
dat = x & 0x7f ;
/* send */
put_fpga_reg(adr,dat);
}
void put_motor_dir(UBYTE x)
{
volatile UBYTE adr ;
volatile UBYTE dat ;
/* front or rear */
adr = FD_ADR ;
if ( x & 0x80 ) { adr = RD_ADR ; }
/* data */
dat = x & 3 ;
/* send */
put_fpga_reg(adr,dat);
}
UBYTE get_motor_duty(UBYTE fr)
{
volatile UBYTE adr ;
/* front or rear */
adr = FP_ADR ;
if ( fr ) { adr = RP_ADR ; }
/* result */
return get_fpga_reg(adr);
}
UBYTE get_motor_dir(UBYTE fr)
{
volatile UBYTE adr ;
/* front or rear */
adr = FD_ADR ;
if ( fr ) { adr = RD_ADR ; }
/* result */
return get_fpga_reg(adr);
}
UWORD get_pulse_count(void)
{
volatile UBYTE dh ;
volatile UBYTE dl ;
/* get upper byte */
dh = get_fpga_reg(ROTU) ;
/* get lower byte */
dh = get_fpga_reg(ROTL) ;
/* result */
return( (dh << 8) | dl ) ;
}
UBYTE get_top(void)
{
return get_fpga_reg(LTOP);
}
UBYTE get_middle(void)
{
return get_fpga_reg(LMID);
}
UBYTE get_bottom(void)
{
return get_fpga_reg(LBOT);
}
UBYTE get_top_width(void)
{
return get_fpga_reg(DTOP);
}
UBYTE get_middle_width(void)
{
return get_fpga_reg(DMID);
}
UBYTE get_bottom_width(void)
{
return get_fpga_reg(DBOT);
}
関数put_fpga_regがあると、どのラインを画像処理の
対象に指定するかの関数put_line_numberは不要と判断
しました。階層構造は、下図となります。
マシンの保護とパワーセーブするため、指定距離の
走行およびスタートから指定時間経過したならば
モータを停止させます。
距離に相当するカウント値は関数get_pulse_countで
取得できます。スタート時のカウント値にオフセット
を加えて、１秒ごとに、指定距離まで到達したかを
チェックします。
スタート時のパルスカウント値から、停止距離相当の
カウント値を求めるコードは、以下。
#define MAX_RUN_PULSE 28000
target_distance = get_pulse_count() + MAX_RUN_PULSE ;
指定距離を走行したならば、モータを停止するには
次のコードで指定します。
if ( get_pulse_count() > target_distance ) {
put_motor_duty( 0 ) ;
put_motor_duty( 0x80 ) ;
}
マシンの電源を入れてから、どのくらい時間が経過
しているのかは、次のようにシステムタイマを利用
します。
0.1msごとのタイマー割込みを検出して、変数timcntを
インクリメントしていきます。
if ( (IRQSTA & RTOS_TIMER_BIT) == RTOS_TIMER_BIT ) {
/* clear timer0 interrupt flag */
T0CLRI = 0xff ;
/* increment */
timcnt++ ;
}
MCR大会では、１台のマシンに許されるコース占有時間は
１分程度なので、60000msが経過したならばモータを停止
するコードを定義します。
スタート時点のカウント値に、許容時間カウント値を
加えておきます。
#define MAX_RUN_TIME 600000
target_timcnt = timcnt + MAX_RUN_TIME ;
指定時間を走行したならば、モータを停止するには
次のコードで指定します。
if ( target_timcnt > timcnt ) {
put_motor_duty( 0 ) ;
put_motor_duty( 0x80 ) ;
}
実際は、計数フラグを用意して判定します。
flag = 0 ;
if ( target_timcnt > timcnt ) {
flag++ ;
}
if ( get_pulse_count() > target_distance ) {
flag++ ;
}
if ( flag ) {
/* stop motors */
put_motor_dir( FRONT_CENTER );
put_motor_dir( REAR_FORWARD );
put_motor_duty( 0 );
put_motor_duty( MASK80 | 0 );
}
カメラからの画像データ取得と一部の画像処理は
FPGAが担当するので、ファームウエアはRealTimeOS
を利用します。
各タスクの担当する内容は、以下とします。
SSTART (task0) スタートトリガー判定
BLIND_RUN (task1) 情報なし直進
NORMAL_RUN (task2) 通常走行
CRANK_RUN (task3) クランク走行
LANE_CHANGE (task4) レーンチェンジ走行
SENSING (task5) センサーデータ生成
MEASURE (task6) 距離、時間計測
SDEBUG (task7) デバッガ
LEDFLASH (task8) 状態対応LED表示
タスクの動作を、詳しく書いて机上デバッグ
していきます。
SSTART
スタートトリガーの判定と他タスク起動を担当。
スタートトリガーは、タイマー割込みで
シフトレジスタにスイッチの状態を読み
込み、チャタリング除去します。
これだけでは、IDLEかRUNかを判定できない
ので、動作状態を表す変数system_statusを
用意し、その値で走行か停止かを確定します。
状態変数system_statusの設定とともに、動作
タスクを起動します。また、最長走行距離と
動作時間を設定します。
状態変数を使うと、スイッチをスタートと
ストップのどちらにも利用できます。
タスク内部のコードは、以下です。
void tsk0_proc(void)
{
/* get data */
str_sft <<= 1 ;
str_sft &= MASK07 ;
if ( GP0DAT & MASK20 ) { str_sft |= ON ; }
/* judge */
if ( str_sft == 3 ) {
if ( system_status == IDLE ) {
/* IDLE -> RUN */
system_status = RUN ;
/* enable BLIND_RUN */
rsm_tsk(BLIND_RUN);
sus_tsk(LEDFLASH);
/* stop LED flashing */
turn_off_led();
/* set 1 minite */
target_timcnt = timcnt + MAX_RUN_TIME ;
/* set 100m */
target_distance = get_pulse_count() + MAX_RUN_PULSE ;
} else {
/* RUN -> IDLE */
system_status = IDLE ;
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
rsm_tsk(LEDFLASH);
/* stop */
fdir = FRONT_CENTER ;
rdir = REAR_FORWARD ;
fduty = 0 ;
rduty = (MASK80 | 0) ;
put_motor_dir( fdir ) ;
put_motor_dir( rdir ) ;
put_motor_duty( fduty ) ;
put_motor_duty( rduty ) ;
}
}
/* cycle 20ms */
wai_tsk( 2 );
}
スイッチのチャタリング除去のため、20ms
ごとに動作する、周期関数にしました。
BLIND_RUN
センサーの情報を利用しない、直進をします。
タスクSSTARTからREADYにされ、与えられた動作を
終了すると、タスクNORMAL_RUNをREADYにします。
自身は、SUSPEND状態になります。
内部は、単純なシーケンサにします。
500msごとに、後輪に与えるDUTY比を変え
タイヤの空転を回避します。タイヤが空
回りすると、思いもよりない方向に進む
ので、その対策です。またモータの焼き
つき防止の意味もあります。
タスク内部のコードは、以下。
void tsk1_proc(void)
{
/* show mode */
turn_on_led(LED_BLIND);
/* select CENTER */
put_motor_dir( FRONT_CENTER ) ;
put_motor_dir( REAR_FORWARD ) ;
/* sequencer */
switch ( bstate ) {
/* duty 5% */
case 0 : rduty = (MASK80 | 5) ;
break ;
/* duty 15% */
case 1 : rduty = (MASK80 | 15) ;
break ;
/* duty 25% */
case 2 : rduty = (MASK80 | 25) ;
break ;
/* duty 45% */
case 3 : rduty = (MASK80 | 45) ;
break ;
/* duty 65% */
case 4 : rduty = (MASK80 | 65) ;
break ;
/* duty 75% */
case 5 : rduty = (MASK80 | 75) ;
break ;
/* others */
default : break ;
}
put_motor_duty( rduty );
/* increment */
bstate++ ;
/* judge */
if ( bstate < 6 ) {
wai_tsk(50) ; /* 500ms interval */
} else {
bstate = 0 ;
rsm_tsk(NORMAL_RUN) ;
slp_tsk();
turn_off_led();
}
}
NORMAL_RUN
コース上にセンターラインがある場合の走行を
担当するタスクです。
タスクBLIND_RUNからREADYにされ、センサー値で
タスクCRANK_RUN、LANE_CHANGEをSUSPENDからREADY
にします。
センサーデータは、TOP、MIDDLE、BOTTOMの３ライン
のセンター位置、幅をFPGAが出力するので、ファーム
ウエアで、８ビットに変換します。
クランク、レーンチェンジの判定は、BOTOMでない
２つのラインの状態で判定します。
タスク内部のコードは、以下。
void tsk2_proc(void)
{
volatile UBYTE topx ;
volatile UBYTE midx ;
volatile UBYTE sensorx ;
volatile UBYTE flag2 ;
/* show mode */
turn_on_led(LED_NORMAL);
/* default */
flag2 = OFF ;
topx = xsjp.topx ;
midx = xsjp.midx ;
sensorx = xsjp.botx ;
/* judge */
if ( sensorx == STATE_CENTER ) {
fdir = FRONT_CENTER ;
rdir = REAR_FORWARD ;
fduty = 0 ;
rduty = (MASK80 | 50);
}
if ( sensorx == STATE_TINY_RIGHT ) {
fdir = FRONT_RIGHT ;
rdir = REAR_FORWARD ;
fduty = 50 ;
rduty = (MASK80 | 75);
}
if ( sensorx == STATE_RIGHT ) {
fdir = FRONT_RIGHT ;
rdir = REAR_FORWARD ;
fduty = 75 ;
rduty = (MASK80 | 55);
}
if ( sensorx == STATE_BIG_RIGHT ) {
fdir = FRONT_RIGHT ;
rdir = REAR_FORWARD ;
fduty = 90 ;
rduty = (MASK80 | 45);
}
if ( sensorx == STATE_TINY_LEFT ) {
fdir = FRONT_LEFT ;
rdir = REAR_FORWARD ;
fduty = 50 ;
rduty = (MASK80 | 75);
}
if ( sensorx == STATE_LEFT ) {
fdir = FRONT_LEFT ;
rdir = REAR_FORWARD ;
fduty = 70 ;
rduty = (MASK80 | 55);
}
if ( sensorx == STATE_BIG_LEFT ) {
fdir = FRONT_LEFT ;
rdir = REAR_FORWARD ;
fduty = 90 ;
rduty = (MASK80 | 45);
}
/* set motor duty ratio */
put_motor_dir( fdir );
put_motor_dir( rdir );
put_motor_duty( fduty );
put_motor_duty( rduty );
/* judge CRANK mark */
if ( topx == STATE_ALL_WHITE ||
midx == STATE_ALL_WHITE ) {
white_state = WHITE_STATE_ALL ;
rsm_tsk( CRANK_RUN );
flag2 = ON ;
}
/* judge LANE LEFT CHANGE mark */
if ( topx == STATE_LEFT_WHITE ||
midx == STATE_LEFT_WHITE ) {
white_state = WHITE_STATE_LEFT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* judge LANE RIGHT CHANGE mark */
if ( topx == STATE_RIGHT_WHITE ||
midx == STATE_RIGHT_WHITE ) {
white_state = WHITE_STATE_RIGHT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* delay */
if ( flag2 == ON ) {
slp_tsk() ;
turn_off_led();
} else {
wai_tsk(1) ; /* 10ms interval */
}
}
クランク、レーンチェンジのマークは３パターン
あるので、変数white_stateにより、CRANK_RUNと
LANE_CHANGEでの動作を判定できるようにします。
CRANK_RUN
クランクマークを検出してから、右あるいは左に
あるクランクを通過するまでを、担当します。
タスクNORMA_RUNからREADYにされ、センサー値で
タスクNORMALRUNをSUSPENDからREADYにします。
右あるいは左に方向転換するので、シーケンサで
動きを制御します。
シーケンスは、以下とします。
０ステート 設定時間分、直進と指定（１ステートに遷移）
１ステート 設定時間経過していいれば、２ステートに遷移
２ステート 右あるいは左に白線が偏っていれば、３ステートに遷移
３ステート センサーが示す方向に回転開始、４ステートに遷移
４ステート センサーライン検出まで回転継続、検出すると５ステートに遷移
５ステート タスクNORMAL_RUNをREADYに、自身はSUSPENDに
タスク内部のコードは、以下。
void tsk3_proc(void)
{
volatile UBYTE topx ;
volatile UBYTE midx ;
/* show mode */
turn_on_led(LED_CRANK);
/* get sensor data */
topx = xsjp.topx ;
midx = xsjp.midx ;
/* sequencer */
switch ( cstate ) {
/* blind run until minimum distance */
case 0 : cstate = 1 ;
ccnt = 30 ;
put_motor_dir( FRONT_CENTER );
put_motor_dir( REAR_FORWARD );
put_motor_duty( MASK80 | 75 );
break ;
/* delay */
case 1 : if ( ccnt == 0 ) {
cstate = 2 ;
} else {
ccnt-- ;
}
break ;
/* until CRANK location */
case 2 : if ( topx == STATE_LEFT_WHITE ||
midx == STATE_LEFT_WHITE ) {
cstate = 3 ;
white_state = WHITE_STATE_LEFT ;
}
if ( topx == STATE_RIGHT_WHITE ||
midx == STATE_RIGHT_WHITE ) {
cstate = 3 ;
white_state = WHITE_STATE_RIGHT ;
}
break ;
/* turn */
case 3 : cstate = 4 ;
if ( white_state == WHITE_STATE_LEFT ) {
put_motor_dir( FRONT_LEFT );
put_motor_duty( 50 );
}
if ( white_state == WHITE_STATE_RIGHT ) {
put_motor_dir( FRONT_RIGHT );
put_motor_duty( 50 );
}
put_motor_dir( REAR_FORWARD );
put_motor_duty( MASK80 | 50 );
break ;
/* judge center */
case 4 : if ( topx == STATE_CENTER || midx == STATE_CENTER ) {
cstate = 5 ;
put_motor_dir( FRONT_CENTER );
}
break ;
/* return first state */
case 5 : break ;
/* */
default : break ;
}
/* delay */
if ( cstate < 5 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
cstate = 0 ;
put_motor_dir( FRONT_CENTER );
put_motor_dir( REAR_FORWARD );
put_motor_duty( MASK80 | 75 );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
turn_off_led();
rsm_tsk( NORMAL_RUN );
}
}
LANE_CHANGE
レーンチェンジマークを検出してから、センターライン
なしの位置まで移動します。そこで、右あるいは左に
レーンを変え、次にセンターラインを検出するまでを
担当します。
タスクNORMA_RUNからREADYにされ、センサー値で
タスクNORMALRUNをSUSPENDからREADYにします。
右あるいは左に方向転換するので、シーケンサで
動きを制御します。
シーケンスは、以下とします。
０ステート センターラインがなくなるまで直進（センターラインなしで、１ステートに遷移）
１ステート 変数white_stateの値により、右か左に方向変更（２ステートに遷移）
２ステート 設定時間経過まで方向を右か左にし、時間経過後３ステートに遷移
３ステート 設定時間経過まで直進、その後４ステートに遷移
４ステート 直進、５ステートに遷移
５ステート センターラインを検出するまで直進、検出後６ステートに遷移
６ステート 右あるいは左にセンターラインが来たら、７ステートに遷移
７ステート 設定時間経過まで回転、その後８ステートに遷移
８ステート タスクNORMAL_RUNをREADYに、自身はSUSPENDに
タスク内部のコードは、以下。
void tsk4_proc(void)
{
volatile UBYTE xtop ;
volatile UBYTE xmid ;
/* show mode */
turn_on_led(LED_LANE);
/* get sensor data */
xtop = xsjp.topx ;
xmid = xsjp.midx ;
/* sequencer */
switch ( lstate ) {
/* forward until ALL_BLACK */
case 0 : if ( xtop == STATE_ALL_BLACK ||
xmid == STATE_ALL_BLACK ) {
lstate = 1 ;
}
break ;
/* judge turn */
case 1 : lstate = 2 ;
lcnt = 10 ;
fdir = FRONT_RIGHT ;
if ( white_state == WHITE_STATE_LEFT ) {
fdir = FRONT_LEFT ;
}
put_motor_dir( fdir );
put_motor_duty( 80 );
break ;
/* turn delay */
case 2 : if ( lcnt == 0 ) {
lstate = 3 ;
put_motor_dir( FRONT_CENTER );
} else {
lcnt-- ;
}
break ;
/* blind run */
case 3 : lstate = 4 ;
lcnt = 10 ;
put_motor_dir( REAR_FORWARD ) ;
put_motor_duty( MASK80 | 75 ) ;
break ;
/* blind run delay */
case 4 : if ( lcnt == 0 ) {
lstate = 5 ;
} else {
lcnt-- ;
}
break ;
/* judge center */
case 5 : if ( xtop == STATE_CENTER ||
xmid == STATE_CENTER ) {
lstate = 6 ;
}
break ;
/* judge turn (reverse) */
case 6 : lstate = 7 ;
lcnt = 10 ;
fdir = FRONT_LEFT ;
if ( white_state == WHITE_STATE_LEFT ) {
fdir = FRONT_RIGHT ;
}
put_motor_dir( fdir );
put_motor_duty( 80 );
break ;
/* turn (reverse) delay */
case 7 : if ( lcnt == 0 ) {
lstate = 8 ;
put_motor_dir( FRONT_CENTER );
} else {
lcnt-- ;
}
break ;
/* return first state */
case 8 : break ;
/* */
default : break ;
}
/* delay */
if ( lstate < 8 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
lstate = 0 ;
put_motor_dir( FRONT_CENTER );
put_motor_dir( REAR_FORWARD );
put_motor_duty( MASK80 | 75 );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
turn_off_led();
rsm_tsk( NORMAL_RUN );
}
}
SENSING
FPGAからの画像処理データを取得します。
カメラは、フレームレートが30なので１秒に
30回画像更新されます。30ms毎に、FPGAから
センサー関係パラメータを取得します。
FPGAからは、センターライン位置、幅を取得
できるので、これらから８ビットセンサーを
生成します。
位置、幅を構造体変数に入れ、専用関数で
８ビットセンサーを生成します。
専用関数は、後で定義します。
タスク内部のコードは、以下。
void tsk5_proc(void)
{
/* get data */
*(sensor+0) = get_top() ;
*(sensor+1) = get_middle() ;
*(sensor+2) = get_bottom() ;
/* get width */
*(sensor+3) = get_top_width() ;
*(sensor+4) = get_middle_width() ;
*(sensor+5) = get_bottom_width() ;
if ( system_status == IDLE ) {
top_center = *(sensor+0) ;
middle_center = *(sensor+1) ;
bottom_center = *(sensor+2) ;
top_width = *(sensor+3) ;
middle_width = *(sensor+4) ;
bottom_width = *(sensor+5) ;
}
/* judge */
xsjp.topx = *(sensor+0) ;
xsjp.midx = *(sensor+1) ;
xsjp.botx = *(sensor+2) ;
xsjp.topw = *(sensor+3) ;
xsjp.midw = *(sensor+4) ;
xsjp.botw = *(sensor+5) ;
perform_sensing(xsjp);
/* send trigger */
send_cam_trigger();
/* */
wai_tsk( 3 ) ; /* 30ms */
}
センサーデータを位置、幅の２種に分けて
グローバル変数に入れています。スタート
する前に、３ラインの位置、幅を記憶して
おき、走行中の位置、幅で８ビットデータ
を生成します。
MEASURE
スタートしてからの移動距離、経過時間を
カウントしていきます。カウント値が所定
の値を超えたときに、モータを停止します。
距離カウントはエンコーダパルスの積算値で
算出し、システムタイマのカウント値により
経過時間を判断します。
距離カウンタがあれば、周期タスクによる
時間差から、移動スピードを算出できます。
移動スピードから、登坂、下坂の判定が可能
ですが、必要になったときに入れます。
タスク内部のコードは、以下。
void tsk6_proc(void)
{
volatile UBYTE flag ;
/* update */
pre_rcnt = cur_rcnt ;
/* get */
cur_rcnt = get_pulse_count();
/* differencial pulse count */
dif_rcnt = cur_rcnt - pre_rcnt ;
/* judge */
flag = 0 ;
if ( target_timcnt > timcnt ) {
flag++ ;
}
if ( get_pulse_count() > target_distance ) {
flag++ ;
}
if ( flag ) {
/* stop RUN */
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
/* stop motors */
put_motor_dir( FRONT_CENTER );
put_motor_dir( REAR_FORWARD );
put_motor_duty( 0 );
put_motor_duty( MASK80 | 0 );
}
/* interval */
wai_tsk( 10 ) ; /* 100ms */
}
SDEBUG
ハードウエアをシリアルインタフェース
から与えるコマンドでテストします。
モータ関係の設定、画像処理によるセンターライン
の位置、幅、カメラ初期化等に加え、エンコーダ
パルス値を表示します。
他に、カメラ内部のレジスタ値がどうなって
いるのかを表示します。
どんなコマンドを持っているのかは、ヘルプ(?)を
入力することで、わかるようにしておきます。
タスク内部のコードは、以下。
void tsk7_proc(void)
{
volatile UBYTE msg[6] ;
volatile UWORD epulse ;
volatile UBYTE tmp7 ;
volatile UBYTE loop7 ;
/* default */
tmp7 = 0 ;
/* judge flag */
if ( uflag == OFF ) return ;
/* new line */
crlf();
/* clear flag */
uflag = OFF ;
/* judge */
cmd = *(sbuf+0) ;
if ( cmd == '?' ) { show_help(); }
/* initialize CAMERA */
if ( cmd == 'I' ) {
push_tsk_state();
turn_off_led();
init_cam( *(sbuf+1)-'0' ) ;
pop_tsk_state();
}
/* set front parameters */
if ( cmd == 'F' || cmd == 'R' ) {
/* duty */
duty = get_hex( *(sbuf+1) ) ;
duty *= 10 ;
duty += get_hex( *(sbuf+2) ) ;
/* direction */
direction = get_hex( *(sbuf+3) ) ;
/* rear or front */
if ( cmd == 'R' ) {
rdir = direction ;
rduty = duty ;
} else {
fdir = direction ;
fduty = duty ;
}
/* impress */
put_motor_dir( fdir ) ;
put_motor_dir( rdir ) ;
put_motor_dir( fduty ) ;
put_motor_dir( MASK80 | rduty ) ;
}
/* get encoder pulse */
if ( cmd == 'P' ) {
epulse = get_pulse_count();
/* separate */
*(msg+0) = epulse / 10000 ; epulse %= 10000 ;
*(msg+1) = epulse / 1000 ; epulse %= 1000 ;
*(msg+2) = epulse / 100 ; epulse %= 100 ;
*(msg+3) = epulse / 10 ;
*(msg+4) = epulse % 10 ;
*(msg+5) = '\0' ;
/* conversion */
for ( loop7 = 0 ; loop7 < 5 ; loop7++ ) {
*(msg+loop7) = asc_hex[ *(msg+loop7) ] ;
}
/* show */
rs_puts( msg ) ;
/* new line */
crlf() ;
}
/* show duty */
if ( cmd == 'S' ) { show_duty(); }
/* show sensor data */
if ( cmd == 'D' ) {
/* sensor */
for ( epulse = 0 ; epulse < 3 ; epulse++ ) {
/* top */
tmp7 = get_top() ;
/* middle */
if ( epulse == 1 ) { tmp7 = get_middle() ; }
/* bottom */
if ( epulse == 2 ) { tmp7 = get_bottom() ; }
/* separate */
*(msg+0) = 'S' ;
*(msg+1) = ' ' ;
*(msg+2) = tmp7 / 100 ; tmp7 %= 100 ;
*(msg+3) = tmp7 / 10 ;
*(msg+4) = tmp7 % 10 ;
*(msg+5) = '\0' ;
/* conversion */
for ( loop7 = 2 ; loop7 < 5 ; loop7++ ) {
*(msg+loop7) = asc_hex[ *(msg+loop7) ] ;
}
rs_puts( msg ) ;
crlf();
}
/* width */
for ( epulse = 0 ; epulse < 3 ; epulse++ ) {
/* top */
tmp7 = get_top_width() ;
/* middle */
if ( epulse == 1 ) { tmp7 = get_middle_width() ; }
/* bottom */
if ( epulse == 2 ) { tmp7 = get_bottom_width() ; }
/* separate */
*(msg+0) = 'W' ;
*(msg+1) = ' ' ;
*(msg+2) = tmp7 / 100 ; tmp7 %= 100 ;
*(msg+3) = tmp7 / 10 ;
*(msg+4) = tmp7 % 10 ;
*(msg+5) = '\0' ;
/* conversion */
for ( loop7 = 2 ; loop7 < 5 ; loop7++ ) {
*(msg+loop7) = asc_hex[ *(msg+loop7) ] ;
}
rs_puts( msg ) ;
crlf();
}
}
/* show eeprom data */
if ( cmd == 'e' ) {
/* show */
for ( loop7 = 0 ; loop7 < 64 ; loop7++ ) {
/* get data */
eeprom_dat = eeprom_get( loop7 ) ;
/* show */
show_value( (eeprom_dat >> 8) & MASKFF );
show_value( eeprom_dat & MASKFF );
rs_putchar( ' ' );
/* new line */
if ( (loop7 % 8) == 7 ) { crlf() ; }
}
crlf();
}
/* store eeprom data */
if ( cmd == 'E' ) {
/* generate address */
eeprom_adr = 0 ;
eeprom_adr += get_hex( *(sbuf+1) ) ;
eeprom_adr <<= 4 ;
eeprom_adr += get_hex( *(sbuf+2) ) ;
/* generate data */
eeprom_dat = 0 ;
eeprom_dat += get_hex( *(sbuf+3) ) ;
eeprom_dat <<= 4 ;
eeprom_dat += get_hex( *(sbuf+4) ) ;
eeprom_dat <<= 4 ;
eeprom_dat += get_hex( *(sbuf+5) ) ;
eeprom_dat <<= 4 ;
eeprom_dat += get_hex( *(sbuf+6) ) ;
/* judge and store */
if ( eeprom_adr < 64 ) { eeprom_put(eeprom_adr,eeprom_dat) ; }
else { rs_puts("Address Error !"); }
crlf();
}
/* show register values */
if ( cmd == 'V' ) {
push_tsk_state();
turn_off_led();
show_reg() ;
pop_tsk_state();
}
}
LEDFLASH
IDLE状態にあるときに、LEDを点滅します。
走行中の状態は、ひとつのLEDの点灯で表示します。
IDLE状態のときに、点滅します。
30msごとに、４つのLEDに異なるビット
パターンを出力します。
LEDの点滅には、シーケンサを利用します。
カウンタを一つ使い、０〜５を繰り返して
点灯位置が左右に振れるように見せます。
タスク内部のコードは、以下。
void tsk8_proc(void)
{
/* impress */
turn_on_led( cyclic[fstate] );
/* update state */
fstate++ ;
/* judge */
if ( fstate == 6 ) { fstate = 0 ; }
/* interval 300ms */
wai_tsk(30);
}
カメラパラメータ処理
OV7670を最適な状態で動かすためには
設定パラメータを何度か変更します。
何度もコンパイル、リンクするのは面倒
なのでEEPROMにパラメータを保存します。
EEPROMには、AT93C46を使うことに。
64x16ビットなので、64アドレスに
１ワード（16ビット）が割付けられて
います。
ARMのGPIOで残っているのは、P4なので
次のように接続します。
AT93C46を扱うために、３つの関数を定義します。
- eeprom_ewen
- eeprom_put
- eeprom_get
タイミングチャートを利用して、関数を定義します。
void eeprom_ewen(void)
{
UWORD tmp ;
UBYTE i ;
/* SB , OP code , dummy */
tmp = 0x130 ;
/* enable chip select */
send_bitx(MASK80 | EEPROM_CS_BIT) ;
/* send */
for ( i = 0 ; i < 9 ; i++ ) {
/* impress DI */
send_bitx(EEPROM_DI_BIT) ;
if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; }
/* SK : H */
send_bitx(MASK80 | EEPROM_SK_BIT) ;
/* shift */
tmp <<= 1 ;
/* SK : L */
send_bitx(EEPROM_SK_BIT) ;
}
/* clear DI */
send_bitx(EEPROM_DI_BIT) ;
/* disable chip select */
send_bitx(EEPROM_CS_BIT) ;
}
データを保存する場合、次のタイミングチャートを利用します。
関数は、以下。
void eeprom_put(UBYTE ax,UWORD dx)
{
UWORD tmp ;
UBYTE i ;
/* judge */
if ( ax > 63 ) return ;
/* SB , OP code , dummy */
tmp = 0x140 | (ax & MASK3F) ;
/* enable chip select */
send_bitx(MASK80 | EEPROM_CS_BIT) ;
/* send address */
for ( i = 0 ; i < 9 ; i++ ) {
/* impress DI */
send_bitx(EEPROM_DI_BIT) ;
if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; }
/* SK : H */
send_bitx(MASK80 | EEPROM_SK_BIT) ;
/* shift */
tmp <<= 1 ;
/* SK : L */
send_bitx(EEPROM_SK_BIT) ;
}
/* send data */
for ( i = 0 ; i < 16 ; i++ ) {
/* impress DI */
send_bitx(EEPROM_DI_BIT) ;
if ( dx & 0x8000 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; }
/* SK : H */
send_bitx(MASK80 | EEPROM_SK_BIT) ;
/* shift */
dx <<= 1 ;
/* SK : L */
send_bitx(EEPROM_SK_BIT) ;
}
/* disable chip select */
send_bitx(EEPROM_CS_BIT) ;
/* delay */
delay_ms(10) ;
}
データを取出すには、SKに同期してからDOの値
を１ビットごとに取得します。
関数は、以下。
UWORD eeprom_get(UBYTE ax)
{
UWORD result ;
UWORD tmp ;
UBYTE i ;
/* judge */
if ( ax > 63 ) { return 0 ; }
/* SB , OP code , dummy */
tmp = 0x180 | (ax & MASK3F) ;
/* enable chip select */
send_bitx(MASK80 | EEPROM_CS_BIT) ;
/* send address */
for ( i = 0 ; i < 9 ; i++ ) {
/* impress DI */
send_bitx(EEPROM_DI_BIT) ;
if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; }
/* SK : H */
send_bitx(MASK80 | EEPROM_SK_BIT) ;
/* shift */
tmp <<= 1 ;
/* SK : L */
send_bitx(EEPROM_SK_BIT) ;
}
/* default */
result = 0 ;
/* get data */
for ( i = 0 ; i < 16 ; i++ ) {
/* shift */
result <<= 1 ;
/* SK : H */
send_bitx(MASK80 | EEPROM_SK_BIT) ;
/* get data */
if ( GP4DAT & 1 ) { result |= ON ; }
/* SK : L */
send_bitx(EEPROM_SK_BIT) ;
}
/* disable chip select */
send_bitx(EEPROM_CS_BIT) ;
return result ;
}
カメラのパラメータは、最終で次の関数を実行する
ことにしました。
put_sccb_data(0x12,0x80);
delay_ms(200); //200ms
rs_puts("Complete reset"); crlf();
put_sccb_data(0x11,0x80) ; /* internal CLKRC */
put_sccb_data(0x3b,0x0a) ; /* disable night mode */
put_sccb_data(0x3a,0x04) ; /* normal graphic */
put_sccb_data(0x12,0x10) ; /* select YUV */
put_sccb_data(0x8c,0x00) ; /* select xRGB */
put_sccb_data(0x40,0xd0) ; /* COM15 [00] - [FF] */
put_sccb_data(0x17,0x16) ; /* HSTART */
put_sccb_data(0x18,0x04) ; /* HSTOP */
put_sccb_data(0x32,0x24) ; /* HREF */
put_sccb_data(0x19,0x02) ; /* VSTART */
put_sccb_data(0x1a,0x7a) ; /* VSTOP */
put_sccb_data(0x03,0x0a) ; /* VREF */
put_sccb_data(0x15,0x02) ; /* HREF */
put_sccb_data(0x0c,0x04) ; /* disable DCW */
put_sccb_data(0x3e,0x1a) ; /* DCW PCLK */
/* put_sccb_data(0x72,0x22) ; */
/* put_sccb_data(0x73,0xf2) ; */
put_sccb_data(0x92,0xfe) ; /* DM_LNL */
put_sccb_data(0x93,0x01) ; /* DM_LNH */
rs_puts("Exit initialize"); crlf();
18回、パラメータを出力しているだけなので
EEPROMのアドレス０〜１７までの18ワードに
データを保存しておき、それらを転送すると
パラメータ設定が終了するようにします。
EEPROMの１ワード設定値は、次のようにしました。
0x00 : 0x1280
0x01 : 0x1180
0x02 : 0x3b0a
0x03 : 0x3a04
0x04 : 0x1210
0x05 : 0x8c00
0x06 : 0x40d0
0x07 : 0x1716
0x08 : 0x1804
0x09 : 0x3224
0x0a : 0x1902
0x0b : 0x1a7a
0x0c : 0x030a
0x0d : 0x1502
0x0e : 0x0c04
0x0f : 0x3e1a
0x10 : 0x92fe
0x11 : 0x9301
カメラの初期化は、EEPROMの中に格納された値を
使えばよいので、次のように記述できます。
for ( i = 0 ; i < 18 ; i++ ) {
/* get 1 word */
tmpx = eeprom_get(i);
/* separate */
ax = (tmpx >> 8) & 0xff ;
dx = tmpx & 0xff ;
/* store */
put_sccb_data(ax,dx);
/* delay */
if ( i == 0 ) {
delay_ms(200);
rs_puts("Complete reset");
crlf();
}
}
rs_puts("Exit initialize");
crlf();
実際にカメラにSCCB経由でパラメータを設定し
正しく値が設定されているのかを、調べると
次のようになりました。
EEPROMに入れてあるアドレス、データが確実に
カメラのレジスタ値と同じになっていることを
確認できました。
ファームウエアソースコード これまでの内容を実装したソースコードは、以下。 #include <ADuC7026.h> #define OFF 0 #define ON OFF+1 #define NO 0 #define YES NO+1 /* data definitions */ typedef unsigned char UBYTE ; typedef signed char SBYTE ; typedef unsigned short UWORD ; typedef signed short SWORD ; typedef unsigned long ULONG ; typedef signed long SLONG ; void IRQ_Handler(void) __irq; void init_usr(void); #define MASKFF 0xFF #define MASK0F 0x0F #define MASK80 0x80 #define MASK40 0x40 #define MASK20 0x20 #define MASK10 0x10 #define MASK08 0x08 #define MASK04 0x04 #define MASK02 0x02 #define MASK03 0x03 #define MASK01 0x01 #define MASKF0 0xF0 #define MASK07 0x07 #define MASK3F 0x3f #define MASK7F 0x7f #define SCCB_ID_WR 0x42 #define SCCB_ID_RD 0x43 #define S_SCL 21 #define S_SDA 20 #define S_CTRG 23 #define S_TRG 22 #define TSK_ID_MAX 10 #define TSK_ID0 0 #define TSK_ID1 1 #define TSK_ID2 2 #define TSK_ID3 3 #define TSK_ID4 4 #define TSK_ID5 5 #define TSK_ID6 6 #define TSK_ID7 7 #define TSK_ID8 8 #define TSK_ID9 9 typedef struct { void (*tsk)(void); UWORD wcount ; } TCBP ; TCBP tcb[TSK_ID_MAX]; #define TTS_SUSPEND 0 #define TTS_WAIT TTS_SUSPEND+1 #define TTS_READY TTS_SUSPEND+2 volatile UWORD ready ; volatile UWORD suspend; volatile UWORD waitq ; volatile UWORD run_tsk; volatile UBYTE tflag ; volatile UWORD tsk_state[3] ; /*-----------------------*/ /* system call prototype */ /*-----------------------*/ 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); void timer_handler(void); void push_tsk_state(void); void pop_tsk_state(void); void rs_putchar(UBYTE x); void crlf(void); void rs_puts(UBYTE *x); UBYTE get_hex(UBYTE x); void show_help(void); void init_cam(void); void put_sccb_start(void); void put_sccb_stop(void); void put_sccb_data(UBYTE ax,UBYTE dx); UBYTE get_sccb_data(UBYTE ax); UBYTE load_cam(UBYTE adr); void show_reg(void); void send_cam(UBYTE adr,UBYTE dat); void delay_sccb(UBYTE x); void delay_ms(UWORD x); void show_value(UBYTE x); /*---------------*/ /* FPGA handling */ /*---------------*/ typedef struct { UBYTE topx ; UBYTE midx ; UBYTE botx ; UBYTE topw ; UBYTE midw ; UBYTE botw ; } SJP ; SJP xsjp ; void put_fpga_reg(UBYTE ax,UBYTE dx); UBYTE get_fpga_reg(UBYTE ax); void put_motor_duty(UBYTE x); void put_motor_dir(UBYTE x); UBYTE get_motor_duty(UBYTE ax); void show_duty(void); void show_data(UBYTE x); void send_cam_trigger(void); void turn_off_led(void); void turn_on_led(UBYTE x); UBYTE perform_judge(UBYTE xw,UBYTE xl,UBYTE yc,UBYTE yw); void perform_sensing(SJP x); UWORD get_pulse_count(void); UBYTE get_top(void); UBYTE get_middle(void); UBYTE get_bottom(void); UBYTE get_top_width(void); UBYTE get_middle_width(void); UBYTE get_bottom_width(void); void eeprom_ewen(void); void eeprom_put(UBYTE ax,UWORD dx); UWORD eeprom_get(UBYTE ax); void send_bitx(UBYTE x); /* global variables */ volatile UBYTE uflag ; volatile UBYTE sbuf[8] ; volatile UBYTE sindex ; volatile UBYTE cmd ; volatile UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','a','b','c','d','e','f'}; volatile ULONG timcnt ; volatile ULONG target_timcnt ; volatile UWORD target_distance ; volatile UWORD vscnt ; volatile UBYTE vtrg ; volatile UBYTE ptrg ; volatile UBYTE str_sft ; volatile UBYTE str_trg ; volatile UBYTE fduty ; volatile UBYTE rduty ; volatile UBYTE fdir ; volatile UBYTE rdir ; volatile UBYTE tsensor; volatile UBYTE msensor; volatile UBYTE bsensor; volatile UBYTE eeprom_adr ; volatile UWORD eeprom_dat ; #define ILAST 30000 #define S_MODE_BIT 18 #define FRONT_CENTER 0x00 #define FRONT_RIGHT 0x01 #define FRONT_LEFT 0x02 #define REAR_STOP 0x80 #define REAR_FORWARD 0x81 #define REAR_REVERSE 0x82 #define STATE_WAIT 0 #define STATE_NORMAL 1 #define STATE_CRANK 2 #define STATE_LANEC 3 #define SSTART TSK_ID0 #define BLIND_RUN TSK_ID1 #define NORMAL_RUN TSK_ID2 #define CRANK_RUN TSK_ID3 #define LANE_CHANGE TSK_ID4 #define SENSING TSK_ID5 #define MEASURE TSK_ID6 #define SDEBUG TSK_ID7 #define LEDFLASH TSK_ID8 #define ONE_SHOT TSK_ID9 #define WHITE_STATE_NONE 0 #define WHITE_STATE_RIGHT 1 #define WHITE_STATE_LEFT 2 #define WHITE_STATE_ALL 3 #define ALL_BLACK 0x00 #define ALL_WHITE 0xff #define STATE_ALL_BLACK 0 #define STATE_BIG_RIGHT 1 #define STATE_RIGHT 2 #define STATE_TINY_RIGHT 3 #define STATE_CENTER 4 #define STATE_TINY_LEFT 5 #define STATE_LEFT 6 #define STATE_BIG_LEFT 7 #define STATE_RIGHT_WHITE 8 #define STATE_LEFT_WHITE 9 #define STATE_ALL_WHITE 10 #define FD_ADR 0 #define FP_ADR 1 #define RD_ADR 2 #define RP_ADR 3 #define DTOP 4 #define DMID 5 #define DBOT 6 #define CDATX 7 #define TOPS 8 #define MIDS 9 #define BOTS 10 #define LTOP 11 #define LMID 12 #define LBOT 13 #define ROTU 14 #define ROTL 15 #define LED_BLIND 0 #define LED_NORMAL 1 #define LED_CRANK 2 #define LED_LANE 3 #define LED_NONE 4 #define EEPROM_CS_BIT 19 #define EEPROM_SK_BIT 18 #define EEPROM_DI_BIT 17 volatile system_status ; volatile UBYTE bstate ; volatile UBYTE cstate ; volatile UBYTE ccnt ; volatile UBYTE lstate ; volatile UBYTE lcnt ; volatile UBYTE direction ; volatile UBYTE white_state ; volatile UWORD cur_rcnt ; volatile UWORD pre_rcnt ; volatile UWORD dif_rcnt ; volatile UBYTE cyclic[6] = {LED_BLIND,LED_NORMAL,LED_CRANK,LED_LANE,LED_CRANK,LED_NORMAL}; volatile UBYTE fstate ; void tsk0_proc(void); void tsk1_proc(void); void tsk2_proc(void); void tsk3_proc(void); void tsk4_proc(void); void tsk5_proc(void); void tsk6_proc(void); void tsk7_proc(void); void tsk8_proc(void); void tsk9_proc(void); #define IDLE 0 #define RUN IDLE+1 #define MAX_RUN_PULSE 28000 #define MAX_RUN_TIME 60000 volatile UBYTE duty ; volatile UBYTE top_center ; volatile UBYTE middle_center ; volatile UBYTE bottom_center ; volatile UBYTE top_width ; volatile UBYTE middle_width ; volatile UBYTE bottom_width ; volatile ULONG last_timcnt ; int main(void) { volatile TCBP pcur_tsk ; /* initialize user */ init_usr(); /* */ init_os(); /* regist task */ cre_tsk(SSTART ,tsk0_proc); cre_tsk(BLIND_RUN ,tsk1_proc); cre_tsk(NORMAL_RUN ,tsk2_proc); cre_tsk(CRANK_RUN ,tsk3_proc); cre_tsk(LANE_CHANGE,tsk4_proc); cre_tsk(SENSING ,tsk5_proc); cre_tsk(MEASURE ,tsk6_proc); cre_tsk(SDEBUG ,tsk7_proc); cre_tsk(LEDFLASH ,tsk8_proc); cre_tsk(ONE_SHOT ,tsk9_proc); /* initialize task states */ sta_tsk(SSTART ,TTS_READY); sta_tsk(BLIND_RUN ,TTS_SUSPEND); sta_tsk(NORMAL_RUN ,TTS_SUSPEND); sta_tsk(CRANK_RUN ,TTS_SUSPEND); sta_tsk(LANE_CHANGE,TTS_SUSPEND); sta_tsk(SENSING ,TTS_READY); sta_tsk(MEASURE ,TTS_SUSPEND); sta_tsk(SDEBUG ,TTS_READY); sta_tsk(LEDFLASH ,TTS_READY); sta_tsk(ONE_SHOT ,TTS_READY); /* show message */ rs_puts("Hello"); crlf(); /* initialize camera */ init_cam() ; /* sensor trigger */ send_cam_trigger(); /* endless loop */ while (ON) { /* get target task state */ pcur_tsk = tcb[run_tsk] ; /* perform task */ if ( is_tsk_ready( run_tsk ) == YES ) { (*(pcur_tsk.tsk))(); } /* change next task */ run_tsk++; if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; } /* timer handling */ if ( tflag ) { /* clear flag */ tflag = OFF ; /* update */ timer_handler(); } } /* dummy return */ return (0); } void IRQ_Handler(void) __irq { volatile UBYTE ch ; /* judge UART receive interruption */ if ( (IRQSTA & UART_BIT) == UART_BIT ) { /* judge */ if ( COMSTA0 & 1 ) { /* clear flag */ ch = COMRX ; /* echo back */ rs_putchar(ch) ; /* store */ *(sbuf+sindex) = ch ; sindex++ ; /* judge */ if ( ch == '\r' ) { sindex = 0 ; uflag = ON ; } } } /* judge timer0 interruption (1ms) */ if ( (IRQSTA & RTOS_TIMER_BIT) == RTOS_TIMER_BIT ) { /* clear timer0 interrupt flag */ T0CLRI = 0xff ; /* increment */ timcnt++ ; /* judge */ if ( (timcnt & MASK0F) == 10 ) { tflag = ON ; } if ( (timcnt & 0x7ff) == 1000 ) { GP4DAT ^= (1 << 23); } } } void init_usr(void) { /* select clock 10.44MHz initialized in start up routine */ PLLKEY1 = 0xaa ; PLLCON = 0x01 ; PLLKEY2 = 0x55 ; /* power control initialized in start up routine */ /* clear flag */ uflag = OFF ; str_trg = OFF ; vtrg = OFF ; ptrg = OFF ; /* clear counter */ sindex = 0 ; vscnt = 0 ; str_sft = 0 ; white_state = WHITE_STATE_NONE ; bstate = 0 ; /* blinde run */ cstate = 0 ; /* crank run */ ccnt = 0 ; lstate = 0 ; /* lane change */ lcnt = 0 ; fstate = 0 ; /* flashing LED */ /* initialize UART */ { /* set baud rate 19200 bps CD = 2 */ COMCON0 = 0x80 ; /* select COMDIV1 and COMDIV0 */ COMDIV0 = 0x11 ; COMDIV1 = 0x00 ; /* set conditions */ COMCON0 = 0x03 ; /* select COMRX and COMTX , 8bit data , 1 stop bit , no parity */ /* enable interrupt */ COMIEN0 = 0x01 ; /* ERBFI */ } /* P0 */ { /* use GPIO */ GP0CON = 0x00000000 ; /* */ GP0DAT = 0xDF1F0000 ; } /* P1 */ { /* use UART */ GP1CON = 0x00000011 ; /* */ GP1DAT = 0xfef00000 ; } /* P2 */ { /* all bits outputs */ GP2DAT = 0xff000000 ; } /* P3 */ { /* all bits outputs */ GP3DAT = 0xff000000 ; } /* P4 */ { GP4DAT = 0xfe000000 ; } /* initialize timer 0 (10kHz) */ { T0LD = 10440 ; /* (10.44MHz / 1) / 1kHz */ T0CON = 0xc0 ; /* enable , cyclic , 1/1 */ } timcnt = 0 ; fduty = 0 ; rduty = 0 ; fdir = FRONT_CENTER ; rdir = REAR_FORWARD - MASK80 ; /* clear rotary counter */ cur_rcnt = 0 ; pre_rcnt = 0 ; dif_rcnt = cur_rcnt - pre_rcnt ; /* target line */ put_fpga_reg(LTOP,10) ; put_fpga_reg(LMID,30) ; put_fpga_reg(LBOT,50) ; /* pseudo center */ top_center = 80 ; middle_center = 80 ; bottom_center = 80 ; top_width = 10 ; middle_width = 10 ; bottom_width = 10 ; /* enable Read / Write */ eeprom_ewen(); /* stop motor */ put_motor_dir( 0x00 ); put_motor_duty( 0x00 ); put_motor_dir( REAR_FORWARD ); put_motor_duty( MASK80 | 0x00 ); /* enable timer 0 and UART interrupt */ IRQEN = RTOS_TIMER_BIT | UART_BIT ; } /* UART putchar */ void rs_putchar(UBYTE x) { /* ? transmmit buffer empty */ while( (COMSTA0 & 0x40) == 0 ) ; /* set value */ COMTX = x ; } /* carriage return and line feed */ void crlf(void) { rs_putchar('\r'); rs_putchar('\n'); } /* UART puts */ void rs_puts(UBYTE *x) { while ( *x != '\0' ) { rs_putchar( *x ) ; x++ ; } } /* convert ASCII to number */ UBYTE get_hex(UBYTE x) { UBYTE result ; /* default */ result = 0 ; /* judge */ if ( '0' <= x && x <= '9' ) { result = x - '0' ; } if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; } if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; } return result ; } /* show help */ void show_help(void) { rs_puts("? help") ; crlf(); rs_puts("I initialize camera") ; crlf(); rs_puts("D show duty") ; crlf(); rs_puts("F set front duty and direction") ; crlf(); rs_puts("R set rear duty and direction") ; crlf(); rs_puts("P pulse count") ; crlf(); rs_puts("S show sensor parameters") ; crlf(); rs_puts("V show registers") ; crlf(); rs_puts("E put data to EEPROM") ; crlf(); rs_puts("e show contents of EEPROM") ; crlf(); rs_puts("L put target line") ; crlf(); rs_puts("l show target line") ; crlf(); } void show_reg(void) { UBYTE i ; UBYTE tmp ; UBYTE tmpx ; UBYTE msg[3] ; /* horizontal ruler */ for ( i = 0 ; i < 4 ; i++ ) { rs_putchar(' ') ; } /* 0 -> F */ for ( i = 0 ; i < 16 ; i++ ) { rs_putchar('+'); rs_putchar( asc_hex[i] ) ; rs_putchar(' '); } /* new line */ crlf(); /* show */ for ( i = 0 ; i < 160 ; i++ ) { /* vertical ruler */ if ( (i & MASK0F) == 0 ) { rs_putchar('+'); rs_putchar( asc_hex[ i >> 4 ] ) ; rs_putchar('0'); rs_putchar(' '); } /* get register data */ tmp = load_cam(i) ; /* separate */ *(msg+0) = ((tmp >> 4) & MASK0F) ; *(msg+1) = (tmp & MASK0F) ; /* show */ rs_putchar( asc_hex[*(msg+0)] ) ; rs_putchar( asc_hex[*(msg+1)] ) ; rs_putchar(' '); /* new line */ tmpx = i & MASK0F ; if ( tmpx == 15 ) { crlf(); } } /* new line */ crlf(); /* FPGA internal registers */ for ( i = 0 ; i < 16 ; i++ ) { /* get data */ tmp = get_fpga_reg( i ) ; tmpx = tmp ; /* separate */ *(msg+0) = tmp / 100 ; tmp %= 100 ; *(msg+1) = tmp / 10 ; *(msg+2) = tmp % 10 ; /* conversion */ *(msg+0) = asc_hex[ *(msg+0) ] ; *(msg+1) = asc_hex[ *(msg+1) ] ; *(msg+2) = asc_hex[ *(msg+2) ] ; /* show */ if ( tmpx < 100 ) { *(msg+0) = ' ' ; } rs_putchar( *(msg+0) ) ; rs_putchar( *(msg+1) ) ; rs_putchar( *(msg+2) ) ; rs_putchar(' '); /* new line */ tmpx = i & MASK07 ; if ( tmpx == 7 ) { crlf(); } } /* new line */ crlf(); } void put_sccb_start(void) { /* both high level */ GP4DAT |= ((1 << S_SCL) | (1 << S_SDA)) ; /* SCL = ON ; SDA = ON */ /* 0 -> SDA */ GP4DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */ /* 0 -> SCL */ GP4DAT &= ~(1 << S_SCL) ; /* SCL = OFF ; SDA = OFF */ } void put_sccb_stop(void) { /* both low level */ GP4DAT &= 0xffcfffff ; /* SCL = OFF ; SDA = OFF */ /* 1 -> SCL */ GP4DAT |= (1 << S_SCL) ; /* SCL = ON ; SDA = OFF */ /* both high level */ GP4DAT |= (1 << S_SDA) ; /* SCL = ON ; SDA = ON */ } void put_sccb_data(UBYTE ax,UBYTE dx) { UWORD tmp ; UBYTE i ; UBYTE j ; /* start condition */ put_sccb_start(); /* */ for ( j = 0 ; j < 3 ; j++ ) { /* set condition word */ tmp = SCCB_ID_WR ; if ( j == 1 ) { tmp = ax ; } if ( j == 2 ) { tmp = dx ; } /* transfer */ for ( i = 0 ; i < 8 ; i++ ) { /* impress data */ GP4DAT &= ~(1 << S_SDA) ; if ( tmp & MASK80 ) { GP4DAT |= (1 << S_SDA) ; } /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* shift */ tmp <<= 1 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; } /* change input */ GP4DAT &= 0xefffffff ; /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* dummy load */ tmp = GP4DAT & MASK10 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; /* change output */ GP4DAT |= 0x10000000 ; } /* stop condition */ put_sccb_stop(); } UBYTE get_sccb_data(UBYTE ax) { UBYTE result ; UBYTE i ; UBYTE j ; UBYTE tmp ; /* start condition */ put_sccb_start() ; /* send address */ for ( j = 0 ; j < 2 ; j++ ) { /* set parameter */ tmp = SCCB_ID_WR ; if ( j == 1 ) { tmp = ax ; } /* transfer */ for ( i = 0 ; i < 8 ; i++ ) { /* send 1 bit */ GP4DAT &= ~(1 << S_SDA) ; if ( tmp & MASK80 ) { GP4DAT |= (1 << S_SDA) ; } /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* shift */ tmp <<= 1 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; } /* dummy read */ { /* change input */ GP4DAT &= 0xefffffff ; /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* dummy input */ tmp = GP4DAT & MASK10 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; /* change output */ GP4DAT |= 0x10000000 ; } } /* stop condition */ put_sccb_stop() ; /* start condition */ put_sccb_start() ; /* send code */ { /* set parameter */ tmp = SCCB_ID_RD ; /* transfer */ for ( i = 0 ; i < 8 ; i++ ) { /* send 1 bit */ GP4DAT &= ~(1 << S_SDA) ; if ( tmp & MASK80 ) { GP4DAT |= (1 << S_SDA) ; } /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* shift */ tmp <<= 1 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; } /* dummy read */ { /* change input */ GP4DAT &= 0xefffffff ; /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* dummy input */ tmp = GP4DAT & MASK10 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; /* change output */ GP4DAT |= 0x10000000 ; } } /* get data */ { result = 0 ; /* change input */ GP4DAT &= 0xefffffff ; /* get */ for ( i = 0 ; i < 8 ; i++ ) { /* shift */ result <<= 1 ; /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* data */ if ( GP4DAT & MASK10 ) { result |= ON ; } /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; } /* dummy read */ { /* SCL : H */ GP4DAT |= (1 << S_SCL) ; /* dummy input */ tmp = GP4DAT & MASK10 ; /* SCL : L */ GP4DAT &= ~(1 << S_SCL) ; /* change output */ GP4DAT |= 0x10000000 ; } } /* stop condition */ put_sccb_stop() ; return result ; } void send_cam(UBYTE adr,UBYTE dat) { put_sccb_data(adr,dat); } UBYTE load_cam(UBYTE adr) { return get_sccb_data(adr) ; } void delay_ms(UWORD x) { ULONG last ; /* calculate */ last = timcnt + 10 * x ; /* wait */ while ( timcnt < last ) ; } void init_cam(void) { UBYTE i ; UWORD tmpx ; UBYTE ax ; UBYTE dx ; /* loop */ for ( i = 0 ; i < 18 ; i++ ) { /* get 1 word */ tmpx = eeprom_get(i); /* separate */ ax = (tmpx >> 8) & 0xff ; dx = tmpx & 0xff ; /* store */ put_sccb_data(ax,dx); /* delay */ if ( i == 0 ) { delay_ms(200); } } } void put_fpga_reg(UBYTE ax,UBYTE dx) { volatile UBYTE adr ; /* generate address */ adr = (ax & MASK0F) ; adr |= 0x20 ; /* send address */ GP2DAT &= 0xff00ffff ; /* clear */ GP2DAT |= (adr << 16) ; /* add parameter */ /* send data */ GP3DAT &= 0xff00ffff ; GP3DAT |= (dx << 16) ; /* trigger */ GP2DAT |= (1 << S_TRG) ; /* trigger H */ ax <<= 1 ; /* delay */ GP2DAT &= ~(1 << S_TRG) ; /* trigger L */ ax <<= 1 ; /* delay */ } UBYTE get_fpga_reg(UBYTE ax) { volatile UBYTE adr ; volatile UBYTE result ; /* generate address */ adr = (ax & MASK0F) ; adr |= 0x10 ; /* change inputs */ GP3DAT &= 0x00ffffff ; /* set address and enable OE */ GP2DAT &= 0xff00ffff ; GP2DAT |= (adr << 16) ; /* get data */ result = GP3DAT & MASKFF ; /* change outputs */ GP3DAT |= 0xff000000 ; return result ; } UBYTE get_motor_duty(UBYTE ax) { return get_fpga_reg( ax ); } void put_motor_duty(UBYTE x) { volatile UBYTE adr ; volatile UBYTE dat ; /* address */ adr = FP_ADR ; /* register address 0x01 */ if ( x & MASK80 ) { adr = RP_ADR ; } /* register address 0x03 */ /* data */ dat = x & MASK7F ; /* send */ put_fpga_reg(adr,dat); } void put_motor_dir(UBYTE x) { volatile UBYTE adr ; volatile UBYTE dat ; /* front or rear */ adr = FD_ADR ; /* register address = 0x00 */ if ( x & MASK80 ) { adr = RD_ADR ; } /* register address = 0x02 */ /* data */ dat = x & MASK03 ; /* send */ put_fpga_reg(adr,dat); } void send_cam_trigger(void) { GP2DAT |= (1 << S_CTRG); GP2DAT &= ~(1 << S_CTRG); } void show_duty(void) { UBYTE i ; UBYTE msg[7] ; /* clear buffer */ for ( i = 0 ; i < 7 ; i++ ) { *(msg+i) = '\0' ; } *(msg+1) = ' ' ; *(msg+4) = ' ' ; /* front */ *(msg+0) = 'F' ; *(msg+2) = (fduty / 10) + '0' ; *(msg+3) = (fduty % 10) + '0' ; *(msg+5) = fdir + '0' ; rs_puts( msg ); crlf(); /* rear */ *(msg+0) = 'R' ; *(msg+2) = (rduty / 10) + '0' ; *(msg+3) = (rduty % 10) + '0' ; *(msg+5) = rdir + '0' ; rs_puts( msg ); crlf(); /* FPGA front */ i = get_motor_duty(FP_ADR) ; *(msg+0) = 'F' ; *(msg+2) = (i / 10) + '0' ; *(msg+3) = (i % 10) + '0' ; *(msg+5) = fdir + '0' ; rs_puts( msg ); crlf(); /* FPGA rear */ i = get_motor_duty(RP_ADR) ; *(msg+0) = 'R' ; *(msg+2) = (i / 10) + '0' ; *(msg+3) = (i % 10) + '0' ; *(msg+5) = rdir + '0' ; rs_puts( msg ); crlf(); } void show_data(UBYTE x) { int i ; UBYTE tmp ; /* */ tmp = x ; /* show */ for ( i = 7 ; i > -1 ; i-- ) { rs_putchar( ((tmp >> i) & 1) + '0'); } crlf(); } void show_value(UBYTE x) { rs_putchar( asc_hex[ (x >> 4) & MASK0F ] ) ; rs_putchar( asc_hex[ x & MASK0F ] ) ; } /* turn off all LEDs */ void turn_off_led(void) { GP1DAT |= 0x00f00000 ; } /* turn on target LED */ void turn_on_led(UBYTE x) { /* judge */ if ( x > LED_LANE ) { return ; } /* turn off all LEDs */ turn_off_led(); /* turn on target LED */ GP1DAT &= ~(1 << (x+20)); } UBYTE perform_judge(UBYTE xw,UBYTE xl,UBYTE yc,UBYTE yw) { volatile UBYTE result ; /* default */ if ( xw == 0 ) { result = STATE_ALL_BLACK ; } else { if ( xw == MASKFF ) { result = STATE_ALL_WHITE ; } else { if ( xw <= yw ) { if ( xl < yc ) { result = STATE_TINY_LEFT ; if ( xl < yc - 10 ) { result = STATE_LEFT ; } if ( xl < yc - 20 ) { result = STATE_BIG_LEFT ; } } if ( xl == yc ) { result = STATE_CENTER ; } if ( xl > yc ) { result = STATE_TINY_RIGHT ; if ( xl > yc + 10 ) { result = STATE_RIGHT ; } if ( xl > yc + 20 ) { result = STATE_BIG_RIGHT ; } } } else { if ( xl < 10 ) { result = STATE_LEFT_WHITE ; } if ( xl > 150 ) { result = STATE_RIGHT_WHITE ; } } } } return result ; } void perform_sensing(SJP x) { UBYTE xwidth ; UBYTE xloc ; /* judge TOP */ xwidth = x.topx ; xloc = x.topw ; tsensor = perform_judge(xwidth,xloc,top_center,top_width); /* judge MIDDLE */ xwidth = x.midx ; xloc = x.midw ; msensor = perform_judge(xwidth,xloc,middle_center,middle_width); /* judge BOTTOM */ xwidth = x.botx ; xloc = x.botw ; bsensor = perform_judge(xwidth,xloc,bottom_center,bottom_width); } UWORD get_pulse_count(void) { volatile UWORD result ; /* get upper byte */ result = get_fpga_reg(ROTU) ; result <<= 8 ; /* get lower byte */ result |= get_fpga_reg(ROTL) ; /* result */ return( result ) ; } UBYTE get_top(void) { return get_fpga_reg(LTOP); } UBYTE get_middle(void) { return get_fpga_reg(LMID); } UBYTE get_bottom(void) { return get_fpga_reg(LBOT); } UBYTE get_top_width(void) { return get_fpga_reg(DTOP); } UBYTE get_middle_width(void) { return get_fpga_reg(DMID); } UBYTE get_bottom_width(void) { return get_fpga_reg(DBOT); } /* start trigger handling */ void tsk0_proc(void) { /* get data */ str_sft <<= 1 ; str_sft &= MASK07 ; if ( GP0DAT & MASK20 ) { str_sft |= ON ; } /* judge */ if ( str_sft == 3 ) { if ( system_status == IDLE ) { /* IDLE -> RUN */ system_status = RUN ; rsm_tsk(BLIND_RUN); rsm_tsk(MEASURE); rsm_tsk(SENSING); sus_tsk(LEDFLASH); sus_tsk(ONE_SHOT); turn_off_led(); /* set 1 minite */ target_timcnt = timcnt + MAX_RUN_TIME ; /* clear distance */ put_fpga_reg( ROTL , 0 ); put_fpga_reg( ROTU , 0 ); /* set 100m */ target_distance = get_pulse_count() + MAX_RUN_PULSE ; } else { /* RUN -> IDLE */ system_status = IDLE ; sus_tsk(NORMAL_RUN); sus_tsk(CRANK_RUN); sus_tsk(LANE_CHANGE); sus_tsk(SENSING); sus_tsk(MEASURE); rsm_tsk(LEDFLASH); rsm_tsk(ONE_SHOT); /* stop */ fdir = FRONT_CENTER ; rdir = REAR_FORWARD ; fduty = 0 ; rduty = (MASK80 | 0) ; put_motor_dir( fdir ) ; put_motor_dir( rdir ) ; put_motor_duty( fduty ) ; put_motor_duty( rduty ) ; } } /* cycle 20ms */ wai_tsk( 2 ); } /* BLIND_RUN */ void tsk1_proc(void) { /* show mode */ turn_on_led(LED_BLIND); /* select CENTER */ put_motor_dir( FRONT_CENTER ) ; put_motor_dir( REAR_FORWARD ) ; /* sequencer */ switch ( bstate ) { /* duty 5% */ case 0 : rduty = (MASK80 | 5) ; break ; /* duty 15% */ case 1 : rduty = (MASK80 | 15) ; break ; /* duty 25% */ case 2 : rduty = (MASK80 | 25) ; break ; /* duty 45% */ case 3 : rduty = (MASK80 | 45) ; break ; /* duty 65% */ case 4 : rduty = (MASK80 | 65) ; break ; /* duty 75% */ case 5 : rduty = (MASK80 | 75) ; break ; /* others */ default : break ; } put_motor_duty( rduty ); /* increment */ bstate++ ; /* judge */ if ( bstate < 6 ) { wai_tsk(50) ; /* 500ms interval */ } else { bstate = 0 ; /* rsm_tsk(SENSING) ; */ rsm_tsk(NORMAL_RUN) ; slp_tsk(); turn_off_led(); } } /* NORMAL_RUN */ void tsk2_proc(void) { volatile UBYTE topx ; volatile UBYTE midx ; volatile UBYTE sensorx ; volatile UBYTE flag2 ; /* show mode */ turn_on_led(LED_NORMAL); /* default */ flag2 = OFF ; topx = tsensor ; midx = msensor ; sensorx = bsensor ; /* judge */ if ( sensorx == STATE_CENTER ) { fdir = FRONT_CENTER ; rdir = REAR_FORWARD ; fduty = 0 ; rduty = (MASK80 | 50); } if ( sensorx == STATE_TINY_RIGHT ) { fdir = FRONT_RIGHT ; rdir = REAR_FORWARD ; fduty = 50 ; rduty = (MASK80 | 75); } if ( sensorx == STATE_RIGHT ) { fdir = FRONT_RIGHT ; rdir = REAR_FORWARD ; fduty = 75 ; rduty = (MASK80 | 55); } if ( sensorx == STATE_BIG_RIGHT ) { fdir = FRONT_RIGHT ; rdir = REAR_FORWARD ; fduty = 90 ; rduty = (MASK80 | 45); } if ( sensorx == STATE_TINY_LEFT ) { fdir = FRONT_LEFT ; rdir = REAR_FORWARD ; fduty = 50 ; rduty = (MASK80 | 75); } if ( sensorx == STATE_LEFT ) { fdir = FRONT_LEFT ; rdir = REAR_FORWARD ; fduty = 70 ; rduty = (MASK80 | 55); } if ( sensorx == STATE_BIG_LEFT ) { fdir = FRONT_LEFT ; rdir = REAR_FORWARD ; fduty = 90 ; rduty = (MASK80 | 45); } /* set motor duty ratio */ put_motor_dir( fdir ); put_motor_dir( rdir ); put_motor_duty( fduty ); put_motor_duty( rduty ); /* judge CRANK mark */ if ( topx == STATE_ALL_WHITE || midx == STATE_ALL_WHITE ) { white_state = WHITE_STATE_ALL ; rsm_tsk( CRANK_RUN ); flag2 = ON ; } /* judge LANE LEFT CHANGE mark */ if ( topx == STATE_LEFT_WHITE || midx == STATE_LEFT_WHITE ) { white_state = WHITE_STATE_LEFT ; rsm_tsk( LANE_CHANGE ); flag2 = ON ; } /* judge LANE RIGHT CHANGE mark */ if ( topx == STATE_RIGHT_WHITE || midx == STATE_RIGHT_WHITE ) { white_state = WHITE_STATE_RIGHT ; rsm_tsk( LANE_CHANGE ); flag2 = ON ; } /* delay */ if ( flag2 == ON ) { slp_tsk() ; turn_off_led(); } else { wai_tsk(1) ; /* 10ms interval */ } } /* CRANK_RUN */ void tsk3_proc(void) { volatile UBYTE topx ; volatile UBYTE midx ; /* show mode */ turn_on_led(LED_CRANK); /* */ topx = tsensor ; midx = msensor ; /* sequencer */ switch ( cstate ) { /* blind run until minimum distance */ case 0 : cstate = 1 ; ccnt = 30 ; put_motor_dir( FRONT_CENTER ); put_motor_dir( REAR_FORWARD ); put_motor_duty( MASK80 | 75 ); break ; /* delay */ case 1 : if ( ccnt == 0 ) { cstate = 2 ; } else { ccnt-- ; } break ; /* until CRANK location */ case 2 : if ( topx == STATE_LEFT_WHITE || midx == STATE_LEFT_WHITE ) { cstate = 3 ; white_state = WHITE_STATE_LEFT ; } if ( topx == STATE_RIGHT_WHITE || midx == STATE_RIGHT_WHITE ) { cstate = 3 ; white_state = WHITE_STATE_RIGHT ; } break ; /* turn */ case 3 : cstate = 4 ; if ( white_state == WHITE_STATE_LEFT ) { put_motor_dir( FRONT_LEFT ); put_motor_duty( 50 ); } if ( white_state == WHITE_STATE_RIGHT ) { put_motor_dir( FRONT_RIGHT ); put_motor_duty( 50 ); } put_motor_dir( REAR_FORWARD ); put_motor_duty( MASK80 | 50 ); break ; /* judge center */ case 4 : if ( topx == STATE_CENTER || midx == STATE_CENTER ) { cstate = 5 ; put_motor_dir( FRONT_CENTER ); } break ; /* return first state */ case 5 : break ; /* */ default : break ; } /* delay */ if ( cstate < 5 ) { wai_tsk(1) ; /* 10ms interval */ } else { cstate = 0 ; /* direction and duty ratio */ fdir = FRONT_CENTER ; rdir = REAR_FORWARD ; fduty = 0 ; rduty = MASK80 | 75 ; /* set motor duty ratio */ put_motor_dir( fdir ); put_motor_dir( rdir ); put_motor_duty( fduty ); put_motor_duty( rduty ); /* */ white_state = WHITE_STATE_NONE ; slp_tsk() ; turn_off_led(); rsm_tsk( NORMAL_RUN ); } } /* LANE_CHANGE */ void tsk4_proc(void) { volatile UBYTE xtop ; volatile UBYTE xmid ; /* show mode */ turn_on_led(LED_LANE); /* */ xtop = tsensor ; xmid = msensor ; /* sequencer */ switch ( lstate ) { /* forward until ALL_BLACK */ case 0 : if ( xtop == STATE_ALL_BLACK || xmid == STATE_ALL_BLACK ) { lstate = 1 ; } break ; /* judge turn */ case 1 : lstate = 2 ; lcnt = 10 ; fdir = FRONT_RIGHT ; if ( white_state == WHITE_STATE_LEFT ) { fdir = FRONT_LEFT ; } put_motor_dir( fdir ); put_motor_duty( 80 ); break ; /* turn delay */ case 2 : if ( lcnt == 0 ) { lstate = 3 ; put_motor_dir( FRONT_CENTER ); } else { lcnt-- ; } break ; /* blind run */ case 3 : lstate = 4 ; lcnt = 10 ; put_motor_dir( REAR_FORWARD ) ; put_motor_duty( MASK80 | 75 ) ; break ; /* blind run delay */ case 4 : if ( lcnt == 0 ) { lstate = 5 ; } else { lcnt-- ; } break ; /* judge center */ case 5 : if ( xtop == STATE_CENTER || xmid == STATE_CENTER ) { lstate = 6 ; } break ; /* judge turn (reverse) */ case 6 : lstate = 7 ; lcnt = 10 ; fdir = FRONT_LEFT ; if ( white_state == WHITE_STATE_LEFT ) { fdir = FRONT_RIGHT ; } put_motor_dir( fdir ); put_motor_duty( 80 ); break ; /* turn (reverse) delay */ case 7 : if ( lcnt == 0 ) { lstate = 8 ; put_motor_dir( FRONT_CENTER ); } else { lcnt-- ; } break ; /* return first state */ case 8 : break ; /* */ default : break ; } /* delay */ if ( lstate < 8 ) { wai_tsk(1) ; /* 10ms interval */ } else { lstate = 0 ; /* direction and duty ratio */ fdir = FRONT_CENTER ; rdir = REAR_FORWARD ; fduty = 0 ; rduty = MASK80 | 75 ; /* set motor duty ratio */ put_motor_dir( fdir ); put_motor_dir( rdir ); put_motor_duty( fduty ); put_motor_duty( rduty ); /* */ white_state = WHITE_STATE_NONE ; slp_tsk() ; turn_off_led(); rsm_tsk( NORMAL_RUN ); } } /* SENSING */ void tsk5_proc(void) { /* get data */ xsjp.topx = get_top() ; xsjp.midx = get_middle(); xsjp.botx = get_bottom(); /* get width */ xsjp.topw = get_top_width() ; xsjp.midw = get_middle_width(); xsjp.botw = get_bottom_width(); /* generate sensor data */ perform_sensing(xsjp); /* */ wai_tsk(10) ; /* 100ms */ } /* MEASURE */ void tsk6_proc(void) { volatile UBYTE flag6 ; /* update */ pre_rcnt = cur_rcnt ; /* get */ cur_rcnt = get_pulse_count(); /* differencial pulse count */ dif_rcnt = cur_rcnt - pre_rcnt ; /* judge */ flag6 = 0 ; if ( timcnt > target_timcnt ) { flag6++ ; } if ( get_pulse_count() > target_distance ) { flag6++ ; } if ( flag6 ) { /* stop RUN */ sus_tsk(NORMAL_RUN); sus_tsk(CRANK_RUN); sus_tsk(LANE_CHANGE); rsm_tsk(LEDFLASH); /* stop motors */ put_motor_dir( FRONT_CENTER ); put_motor_dir( REAR_STOP ); put_motor_duty( 0 ); put_motor_duty( MASK80 | 0 ); } /* interval */ if ( flag6 ) { slp_tsk(); rs_puts("Timer Over !"); crlf(); } else { wai_tsk( 100 ) ; /* 1000ms */ } } /* SDEBUG */ void tsk7_proc(void) { volatile UBYTE msg[6] ; volatile UWORD epulse ; volatile UBYTE tmp7 ; volatile UBYTE loop7 ; /* default */ tmp7 = 0 ; /* judge flag */ if ( uflag == OFF ) return ; /* new line */ crlf(); /* clear flag */ uflag = OFF ; /* judge */ cmd = *(sbuf+0) ; if ( cmd == '?' ) { show_help(); } /* initialize CAMERA */ if ( cmd == 'I' ) { push_tsk_state(); turn_off_led(); init_cam() ; pop_tsk_state(); } /* set front parameters */ if ( cmd == 'F' || cmd == 'R' ) { /* duty */ duty = get_hex( *(sbuf+1) ) ; duty *= 10 ; duty += get_hex( *(sbuf+2) ) ; /* direction */ direction = get_hex( *(sbuf+3) ) ; /* front */ if ( cmd == 'F' ) { fduty = duty ; fdir = direction ; /* impress */ put_motor_duty( fduty ) ; put_motor_dir( fdir ) ; } /* rear */ if ( cmd == 'R' ) { rdir = direction ; rduty = duty ; /* impress */ put_motor_duty( MASK80 | rduty ) ; put_motor_dir( REAR_FORWARD ) ; } } /* get encoder pulse */ if ( cmd == 'P' ) { epulse = get_pulse_count(); /* separate */ *(msg+0) = epulse / 10000 ; epulse %= 10000 ; *(msg+1) = epulse / 1000 ; epulse %= 1000 ; *(msg+2) = epulse / 100 ; epulse %= 100 ; *(msg+3) = epulse / 10 ; *(msg+4) = epulse % 10 ; *(msg+5) = '\0' ; /* conversion */ for ( loop7 = 0 ; loop7 < 5 ; loop7++ ) { *(msg+loop7) = asc_hex[ *(msg+loop7) ] ; } /* show */ rs_puts( msg ) ; /* new line */ crlf() ; } /* show duty */ if ( cmd == 'D' ) { show_duty(); } /* show sensor data */ if ( cmd == 'S' ) { /* sensor */ for ( epulse = 0 ; epulse < 3 ; epulse++ ) { /* top */ tmp7 = get_top() ; /* middle */ if ( epulse == 1 ) { tmp7 = get_middle() ; } /* bottom */ if ( epulse == 2 ) { tmp7 = get_bottom() ; } /* separate */ *(msg+0) = 'S' ; *(msg+1) = ' ' ; *(msg+2) = tmp7 / 100 ; tmp7 %= 100 ; *(msg+3) = tmp7 / 10 ; *(msg+4) = tmp7 % 10 ; *(msg+5) = '\0' ; /* conversion */ for ( loop7 = 2 ; loop7 < 5 ; loop7++ ) { *(msg+loop7) = asc_hex[ *(msg+loop7) ] ; } rs_puts( msg ) ; crlf(); } /* width */ for ( epulse = 0 ; epulse < 3 ; epulse++ ) { /* top */ tmp7 = get_top_width() ; /* middle */ if ( epulse == 1 ) { tmp7 = get_middle_width() ; } /* bottom */ if ( epulse == 2 ) { tmp7 = get_bottom_width() ; } /* separate */ *(msg+0) = 'W' ; *(msg+1) = ' ' ; *(msg+2) = tmp7 / 100 ; tmp7 %= 100 ; *(msg+3) = tmp7 / 10 ; *(msg+4) = tmp7 % 10 ; *(msg+5) = '\0' ; /* conversion */ for ( loop7 = 2 ; loop7 < 5 ; loop7++ ) { *(msg+loop7) = asc_hex[ *(msg+loop7) ] ; } rs_puts( msg ) ; crlf(); } } /* show eeprom data */ if ( cmd == 'e' ) { /* show */ for ( loop7 = 0 ; loop7 < 64 ; loop7++ ) { /* get data */ eeprom_dat = eeprom_get( loop7 ) ; /* show */ show_value( (eeprom_dat >> 8) & MASKFF ); show_value( eeprom_dat & MASKFF ); rs_putchar( ' ' ); /* new line */ if ( (loop7 % 8) == 7 ) { crlf() ; } } crlf(); } /* store eeprom data */ if ( cmd == 'E' ) { /* generate address */ eeprom_adr = 0 ; eeprom_adr += get_hex( *(sbuf+1) ) ; eeprom_adr <<= 4 ; eeprom_adr += get_hex( *(sbuf+2) ) ; /* generate data */ eeprom_dat = 0 ; eeprom_dat += get_hex( *(sbuf+3) ) ; eeprom_dat <<= 4 ; eeprom_dat += get_hex( *(sbuf+4) ) ; eeprom_dat <<= 4 ; eeprom_dat += get_hex( *(sbuf+5) ) ; eeprom_dat <<= 4 ; eeprom_dat += get_hex( *(sbuf+6) ) ; /* judge and store */ if ( eeprom_adr < 64 ) { eeprom_put(eeprom_adr,eeprom_dat) ; } else { rs_puts("Address Error !"); } crlf(); } /* show register values */ if ( cmd == 'V' ) { push_tsk_state(); turn_off_led(); show_reg() ; pop_tsk_state(); } /* put target line */ if ( cmd == 'L' ) { /* get value */ tmp7 = get_hex( *(sbuf+1) ) ; tmp7 *= 10 ; tmp7 += get_hex( *(sbuf+2) ) ; /* get location */ loop7 = *(sbuf+3) ; /* store */ if ( loop7 == 'T' ) { put_fpga_reg(LTOP,tmp7) ; } if ( loop7 == 'M' ) { put_fpga_reg(LMID,tmp7) ; } if ( loop7 == 'B' ) { put_fpga_reg(LBOT,tmp7) ; } } /* show target line */ if ( cmd == 'l' ) { *(msg+1) = ' ' ; *(msg+4) = '\0' ; for ( loop7 = 0 ; loop7 < 3 ; loop7++ ) { /* top */ tmp7 = get_fpga_reg(LTOP) ; *(msg+0) = 'T' ; /* middle */ if ( loop7 == 1 ) { tmp7 = get_fpga_reg(LMID) ; *(msg+0) = 'M' ; } /* bottom */ if ( loop7 == 2 ) { tmp7 = get_fpga_reg(LBOT) ; *(msg+0) = 'B' ; } /* show */ *(msg+2) = asc_hex[ tmp7 / 10 ] ; *(msg+3) = asc_hex[ tmp7 % 10 ] ; rs_puts( msg ) ; crlf() ; } } } /* LED flash */ void tsk8_proc(void) { /* impress */ turn_on_led( cyclic[fstate] ); /* update state */ fstate++ ; /* judge */ if ( fstate == 6 ) { fstate = 0 ; } /* interval 300ms */ wai_tsk(30); } /* get first data */ void tsk9_proc(void) { /* store */ top_center = get_top() ; middle_center = get_middle(); bottom_center = get_bottom(); top_width = get_top_width() ; middle_width = get_middle_width(); bottom_width = get_bottom_width(); /* interval 100ms */ wai_tsk(100); } /*------------------*/ /* system call body */ /*------------------*/ void init_os(void) { ready = 0 ; suspend = 0 ; waitq = 0 ; tflag = OFF ; } void cre_tsk(UBYTE tid,void (*tsk)(void)) { tcb[tid].tsk = tsk; tcb[tid].wcount = 0; } void sta_tsk(UBYTE tid,UBYTE sta) { volatile UWORD tmp ; tmp = (1 << tid); if ( sta == TTS_READY ) { ready |= tmp; } if ( sta == TTS_SUSPEND ) { suspend |= tmp; } if ( sta == TTS_WAIT ) { waitq |= tmp; } } void rsm_tsk(UBYTE tid) { volatile UWORD tmp ; tmp = (1 << tid); ready |= tmp; suspend &= ~tmp; waitq &= ~tmp; } void sus_tsk(UBYTE tid) { volatile UWORD tmp ; tmp = (1 << tid); ready &= ~tmp; suspend |= tmp; waitq &= ~tmp; } void slp_tsk(void) { sus_tsk(run_tsk); } void wai_tsk(UWORD x) { volatile UWORD tmp ; tmp = (1 << run_tsk); ready &= ~tmp; suspend &= ~tmp; waitq |= tmp; tcb[run_tsk].wcount = x ; } UBYTE is_tsk_ready(UBYTE tid) { return( (ready >> tid) & 1 ) ; } void timer_handler(void) { volatile UWORD xtmp; volatile UBYTE loop; /* call timer handling */ xtmp = waitq ; for ( loop = 0 ; loop < TSK_ID_MAX ; loop++ ) { if ( xtmp & ON ) { tcb[loop].wcount-- ; if ( tcb[loop].wcount == 0 ) { rsm_tsk(loop); } } xtmp >>= 1 ; } } void push_tsk_state(void) { /* store */ *(tsk_state+0) = ready ; *(tsk_state+1) = suspend ; *(tsk_state+2) = waitq ; /* resume */ ready = (1 << SDEBUG) ; suspend = 0 ; waitq = 0 ; } void pop_tsk_state(void) { /* resume */ ready = *(tsk_state+0) ; suspend = *(tsk_state+1) ; waitq = *(tsk_state+2) ; } void eeprom_ewen(void) { UWORD tmp ; UBYTE i ; /* SB , OP code , dummy */ tmp = 0x130 ; /* enable chip select */ send_bitx(MASK80 | EEPROM_CS_BIT) ; /* send */ for ( i = 0 ; i < 9 ; i++ ) { /* impress DI */ send_bitx(EEPROM_DI_BIT) ; if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; } /* SK : H */ send_bitx(MASK80 | EEPROM_SK_BIT) ; /* shift */ tmp <<= 1 ; /* SK : L */ send_bitx(EEPROM_SK_BIT) ; } /* clear DI */ send_bitx(EEPROM_DI_BIT) ; /* disable chip select */ send_bitx(EEPROM_CS_BIT) ; } void eeprom_put(UBYTE ax,UWORD dx) { UWORD tmp ; UBYTE i ; /* judge */ if ( ax > 63 ) return ; /* SB , OP code , dummy */ tmp = 0x140 | (ax & MASK3F) ; /* enable chip select */ send_bitx(MASK80 | EEPROM_CS_BIT) ; /* send address */ for ( i = 0 ; i < 9 ; i++ ) { /* impress DI */ send_bitx(EEPROM_DI_BIT) ; if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; } /* SK : H */ send_bitx(MASK80 | EEPROM_SK_BIT) ; /* shift */ tmp <<= 1 ; /* SK : L */ send_bitx(EEPROM_SK_BIT) ; } /* send data */ for ( i = 0 ; i < 16 ; i++ ) { /* impress DI */ send_bitx(EEPROM_DI_BIT) ; if ( dx & 0x8000 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; } /* SK : H */ send_bitx(MASK80 | EEPROM_SK_BIT) ; /* shift */ dx <<= 1 ; /* SK : L */ send_bitx(EEPROM_SK_BIT) ; } /* disable chip select */ send_bitx(EEPROM_CS_BIT) ; /* delay */ delay_ms(10) ; } UWORD eeprom_get(UBYTE ax) { UWORD result ; UWORD tmp ; UBYTE i ; /* judge */ if ( ax > 63 ) { return 0 ; } /* SB , OP code , dummy */ tmp = 0x180 | (ax & MASK3F) ; /* enable chip select */ send_bitx(MASK80 | EEPROM_CS_BIT) ; /* send address */ for ( i = 0 ; i < 9 ; i++ ) { /* impress DI */ send_bitx(EEPROM_DI_BIT) ; if ( tmp & 0x100 ) { send_bitx(MASK80 | EEPROM_DI_BIT) ; } /* SK : H */ send_bitx(MASK80 | EEPROM_SK_BIT) ; /* shift */ tmp <<= 1 ; /* SK : L */ send_bitx(EEPROM_SK_BIT) ; } /* default */ result = 0 ; /* get data */ for ( i = 0 ; i < 16 ; i++ ) { /* shift */ result <<= 1 ; /* SK : H */ send_bitx(MASK80 | EEPROM_SK_BIT) ; /* get data */ if ( GP4DAT & 1 ) { result |= ON ; } /* SK : L */ send_bitx(EEPROM_SK_BIT) ; } /* disable chip select */ send_bitx(EEPROM_CS_BIT) ; return result ; } void send_bitx(UBYTE x) { ULONG tmp; UBYTE loc ; /* get now value */ tmp = GP4DAT ; /* get location */ loc = x & MASK3F ; /* clear */ tmp &= ~(1 << loc); /* judge */ if ( x & MASK80 ) { tmp |= (1 << loc); } /* put value */ GP4DAT = tmp ; } 実走行とハードウエアのテストのための関数を 入れているため、非常に長くなっています。 これだけ長くても、９kbyte程度です。 ピンアサインは、次のように変更しています。 P0.7 = -- P0.6 = -- P0.5 = start trigger P0.4 = -- P0.3 = -- P0.2 = -- P0.1 = -- P0.0 = -- P1.7 = stateLED3(LANE_CHANGE) P1.6 = stateLED2(CRANK_RUN) P1.5 = stateLED1(NORMAL_RUN) P1.4 = stateLED0(BLIND_RUN) P1.3 = -- P1.2 = -- P1.1 = SOUT(serial) P1.0 = SIN (serial) P2.7 = CENA P2.6 = TRG P2.5 = WR P2.4 = OE P2.3 = LSEL3 P2.2 = LSEL2 P2.1 = LSEL1 P2.0 = LSEL0 P3.7 = BIO(7) P3.6 = BIO(6) P3.5 = BIO(5) P3.4 = BIO(4) P3.3 = BIO(3) P3.2 = BIO(2) P3.1 = BIO(1) P3.0 = BIO(0) P4.7 = monitor LED P4.6 = ----- P4.5 = SCCB_SCL P4.4 = SCCB_SDA P4.3 = EEPROM_CS P4.2 = EEPROM_SK P4.1 = EEPROM_DI P4.0 = EEPROM_DO これを叩き台として、走行精度を上げていきます。
目次 前 次