ファームウエア
カメラ関係の処理は、FPGAに一任したので マイコンにはRTOS(Real Time Operating System) を使い、楽することにしました。 RTOSは、自作のUSOを使います。 カメラを利用して、コースを確実に一周すること を目標にしているので、スピードがあるマシンを 作るのではなく、安全走行マシンとします。 2012年２月MCR全国大会コースを見て、走行 パターンを考えました。通常走行、クランク走行、レーンチェンジの ３パターンがあれば、よいと判断できます。 他に、スタート後センサーを使わず直進する ことができればよいとして、タスクを割当て ました。
- タスク０ スタート判定(SSTART)
- タスク１ スタート後の直進(BLIND_RUN)
- タスク２ 通常走行(NORMAL_RUN)
- タスク３ クランク走行(CRANK_RUN)
- タスク４ レーンチェンジ(LANE_CHANGE)
- タスク５ センサーデータ取得(SENSING)
- タスク６ 距離判定(MEASURE)
- タスク７ デバッグ処理(SDEBUG)
- タスク８ モード指定LEDフラッシュ(LEDFLASH)
走行関係タスクは、SUSPENDにしておき
他はREADYで動かします。READYのタスク
は、USOでは周期的に実行します。
走行関係タスクは、主従関係をもっています。
タスクが、別タスクをREADY、SUSPENDにする
場合分けは、以下としました。
- SSTART => BLIND_RUN (BLIND_RUN SUSPEND => READY)
- SSTART => BLIND_RUN (SSTART READY => READY)
- BLIND_RUN => NORMAL_RUN (NORMAL_RUN SUSPEND => READY)
- BLIND_RUN => NORMAL_RUN (BLIND_RUN READY => SUSPEND)
- NORMAL_RUN => CRANK_RUN (CRANK_RUN SUSPEND => READY)
- NORMAL_RUN => CRANK_RUN (NORMAL_RUN READY => SUSPEND)
- CRANK_RUN => NORMAL_RUN (NORMAL_RUN SUSPEND => READY)
- CRANK_RUN => NORMAL_RUN (CRANK_RUN READY => SUSPEND)
- NORMAL_RUN => LANE_CHANGE (LANE_CHANGE SUSPEND => READY)
- NORMAL_RUN => LANE_CHANGE (NORMAL_RUN READY => SUSPEND)
- LANE_CHANGE => NORMAL_RUN (LANE_CHANGE READY => SUSPEND)
- LANE_CHANGE => NORMAL_RUN (NORMAL_RUN SUSPEND => READY)
タスクSSTART
RTOSでは、タスクを周期的に動かすことが できるので、シフトレジスタを用意して スイッチで発生するチャタリングを除去 します。 スタート（ストップにも利用）トリガースイッチに シフトレジスタを割当てて、20msごとにスイッチの 状態を更新します。
スタートトリガーが来たことを確認したなら
タスクBLIND_RUNを、SUSPENDからREADYに
状態遷移させます。
状態遷移をさせるか否かは、現在のシステム
状態に関係するので、システム状態を示す値
を見てから、判断します。
ここまでの内容をまとめます。
/* get data */
str_sft <<= 1 ; /* shift */
str_sft &= MASK07 ; /* mask */
if ( GP0DAT & MASK20 ) { str_sft |= ON ; }
/* judge */
if ( str_sft == 3 ) {
if ( system_status == IDLE ) {
system_status = RUN ;
rsm_tsk(BLIND_RUN);
} else {
system_status = IDLE ;
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
/* stop */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = REAR_FORWARD ;
send_fpga( xmotor );
}
}
チャタリング除去のため、20msごとにタスクを
動かします。この処理を入れると、以下となり
ます。
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 ) {
system_status = RUN ;
rsm_tsk(BLIND_RUN);
} else {
system_status = IDLE ;
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
/* stop */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = REAR_FORWARD ;
send_fpga( xmotor );
}
}
/* cycle 20ms */
wai_tsk( 2 );
}
タスクSSTARTの中で、モータを動かすために
関数send_fpgaを利用しています。
この関数は、構造体変数を引数設定用に利用
しています。構造体は、次のように定義して
あります。
typedef struct {
UWORD xfront ;
UWORD xrear ;
} MOTORP ;
この構造体変数をグローバルで定義し
どのタスクからでも、使えるようにします。
MOTORP xmotor ;
タスクBLIND_RUN
スタートトリガーで少しの時間直進するよう 内部をステートマシンで構成します。 500msごとに、モータのDUTY比をあげていき 75%DUTY比になったなら、タスクNORMAL_RUNに 制御を渡します。 500msのインターバルは、システムコールwai_tskで 生成します。 wai_tsk(50) ; /* 500ms interval */ 75%DUTY比になったなら、READYからSUSPENDにし タスクNORMAL_RUNをSUSPENDからREADYにします。 ステートマシンでは、ステートを示す変数を使う ので、その変数をbstateとして、周期処理するか SUSPENDになるのかを判定します。 if ( bstate < 6 ) { wai_tsk(50) ; /* 500ms interval */ } else { bstate = 0 ; rsm_tsk(NORMAL_RUN); slp_tsk(); } 状態遷移のカラクリを決めたなら、ステートごとに モータのDUTY比を決めて、設定します。 ステートを管理する変数は、bstateとしました。 void tsk1_proc(void) { xmotor.xfront = FRONT_CENTER ; /* sequencer */ switch ( bstate ) { /* duty 5% */ case 0 : xmotor.xrear = (REAR_FORWARD | 5) ; break ; /* duty 15% */ case 1 : xmotor.xrear = (REAR_FORWARD | 15) ; break ; /* duty 25% */ case 2 : xmotor.xrear = (REAR_FORWARD | 25) ; break ; /* duty 45% */ case 3 : xmotor.xrear = (REAR_FORWARD | 45) ; break ; /* duty 65% */ case 4 : xmotor.xrear = (REAR_FORWARD | 65) ; break ; /* duty 75% */ case 5 : xmotor.xrear = (REAR_FORWARD | 75) ; break ; /* others */ default : break ; } send_fpga( xmotor ); /* increment */ bstate++ ; /* judge */ if ( bstate < 6 ) { wai_tsk(50) ; /* 500ms interval */ } else { bstate = 0 ; rsm_tsk(NORMAL_RUN); slp_tsk(); } } ステート遷移は、switch文実行後としてあり 各ステートで実行する内容に専念できるよう にしてあります。タスクNORMAL_RUN
タスクBLIND_RUNからREADYにされると センサーデータを判断し、モータの回転 を制御します。 白線、左白線、右白線を判断して タスクCRANK_RUN、LANE_CHANGEをREADY にします。
この判定後、タスクを切り替えるか、そのままに
するかを決めるので、タスクを実現するコードの
最後は、以下とします。
/* get CRANK mark */
if ( sensorx == ALL_WHITE ) {
white_state = WHITE_STATE_ALL ;
rsm_tsk( CRANK_RUN );
flag2 = ON ;
}
/* get LANE LEFT CHANGE mark */
if ( is_left_white(sensorx) == YES ) {
white_state = WHITE_STATE_LEFT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* get LANE RIGHT CHANGE mark */
if ( is_right_white(sensorx) == YES ) {
white_state = WHITE_STATE_RIGHT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* delay */
if ( flag2 == ON ) {
slp_tsk() ;
} else {
wai_tsk(1) ; /* 10ms interval */
}
白線を検出した場合は、フラグをセットし
READYを継続するかSUSPENDに遷移するかを
判断します。
LANE_CHANGEでは、右にコースをかえるか
左にかえるかを、グローバル変数white_state
に設定します。
センサーデータを８ビットとして、次のように
分類します。
- all_black
- all_white
- center
- tiny_right
- right
- big_right
- tiny_left
- left
- big_left
- right_white
- left_white
タスクCRANK_RUN
タスクNORMAL_RUNからREADYにされると センサーデータを判断し、モータの回転 を制御します。 制御の仕方は、コースを見て考えます。
白線からクランクに入るまでの最短距離は
ルールで規定されているので、その距離を
走行します。
このような処理は、ステートマシンを使う方が
単純なコードになるので、変数cstateを使った
switch文で記述します。
最短距離を走った後、左白線、右白線を見つける
まで直進を続けます。
左白線、右白線を見つけた後、回転方向を覚え
すべて黒、すべて白を見つけたなら回転します。
中央に白線が見えるまで回転します。
中央の白線を見つけたなら、ステアリングを
戻してNORMAL_RUNに復帰します。
一連の流れをステートマシンで記述します。
void tsk3_proc(void)
{
/* sequencer */
switch ( cstate ) {
/* blind run until minimum distance */
case 0 : cstate = 1 ;
ccnt = 30 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = ( REAR_FORWARD | 50 );
send_fpga( xmotor );
break ;
/* delay */
case 1 : if ( ccnt == 0 ) {
cstate = 2 ;
} else {
ccnt-- ;
}
break ;
/* until CRANK location */
case 2 : if ( is_left_white(sensorx) == YES ) {
cstate = 3 ;
white_state = WHITE_STATE_LEFT ;
}
if ( is_right_white(sensorx) == YES ) {
cstate = 3 ;
white_state = WHITE_STATE_RIGHT ;
}
break ;
/* judge turn location */
case 3 : if ( sensorx == ALL_WHITE || sensorx == ALL_BLACK ) {
cstate = 4 ;
}
break ;
/* turn */
case 4 : cstate = 5 ;
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 50);
}
if ( white_state == WHITE_STATE_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 50);
}
xmotor.xrear = (REAR_FORWARD | 30);
send_fpga( xmotor ) ;
break ;
/* judge center */
case 5 : if ( is_center(sensorx) == YES ) {
cstate = 6 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor ) ;
}
break ;
/* return first state */
case 6 : break ;
/* */
default : break ;
}
/* delay */
if ( cstate < 6 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
cstate = 0 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = ( REAR_FORWARD | 50 );
send_fpga( xmotor );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
rsm_tsk( NORMAL_RUN );
}
}
時間待ちは、変数ccntを用意し、値設定後
システムコールで、10ms単位に時間を刻み
ます。
タスクLANE_CHANGE
タスクNORMAL_RUNからREADYにされると センサーデータを判断し、モータの回転 を制御します。 制御の仕方は、コースを見て考えます。
左白線、右白線は、タスクNORMAL_RUNで判断
して、変数white_stateに情報で格納されて
います。
中央の白線がなくなるので、すべて黒を
判断します。
すべて黒を判断したら、変数white_stateの
情報を利用して、コースを変え、中央の白線
を見つけるように走ります。
中央の白線を見つけたら、変数white_stateの
情報を使い、逆にステアリングを切ってから
タスクNORMAL_RUNをREADYにします。
一連の流れをステートマシンで記述します。
void tsk4_proc(void)
{
/* sequencer */
switch ( lstate ) {
/* forward until ALL_BLACK */
case 0 : if ( sensorx == ALL_BLACK ) {
lstate = 1 ;
}
break ;
/* judge turn */
case 1 : lstate = 2 ;
lcnt = 10 ;
xmotor.xfront = ( FRONT_RIGHT | 80 );
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = ( FRONT_LEFT | 80 );
}
send_fpga( xmotor );
break ;
/* turn delay */
case 2 : if ( lcnt == 0 ) {
lstate = 3 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor );
} else {
lcnt-- ;
}
break ;
/* blind run */
case 3 : lstate = 4 ;
lcnt = 10 ;
xmotor.xrear = ( REAR_FORWARD | 50 ) ;
send_fpga( xmotor );
break ;
/* blind run delay */
case 4 : if ( lcnt == 0 ) {
lstate = 5 ;
} else {
lcnt-- ;
}
break ;
/* judge center */
case 5 : if ( is_center(sensorx) == YES ) {
lstate = 6 ;
}
break ;
/* judge turn (reverse) */
case 6 : lstate = 7 ;
lcnt = 10 ;
xmotor.xfront= ( FRONT_LEFT | 80 );
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = ( FRONT_RIGHT | 80 );
}
send_fpga( xmotor );
break ;
/* turn (reverse) delay */
case 7 : if ( lcnt == 0 ) {
lstate = 8 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor );
} else {
lcnt-- ;
}
break ;
/* return first state */
case 8 : break ;
/* */
default : break ;
}
/* delay */
if ( lstate < 8 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
lstate = 0 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = ( REAR_FORWARD | 50 );
send_fpga( xmotor );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
rsm_tsk( NORMAL_RUN );
}
}
タスクSENSING
FPGAに接続したカメラから、定期的にセンサー データを取得します。 周期処理タスクとし、30ms毎にセンサーデータを ５ライン分の５バイトデータをメモリに保存して いきます。 周期を30msにしたのは、カメラのフレームレートが 最大30なので、33msに一度画像データを更新すると して決めました。 単純なメモリ操作になるよう、以下のようにしました。 カメラは、シャッターを押さないと撮影しないので センサーデータを取得後、シャッターを押すトリガー を与えています。 void tsk5_proc(void) { /* get data */ *(sensor+0) = get_fpga(SENX0) ; *(sensor+1) = get_fpga(SENX1) ; *(sensor+2) = get_fpga(SENX2) ; *(sensor+3) = get_fpga(SENX3) ; *(sensor+4) = get_fpga(SENX4) ; /* load */ sensorx = *(sensor+4) ; /* send trigger */ send_cam_trigger(); /* interval */ wai_tsk( 3 ) ; /* 30ms */ }タスクMEASURE
マシンには、ロータリーエンコーダを接続しています。 回転パルスは、FPGAが拾い内部カウンタを インクリメントするので、カウンタの値を 定期的に読み込んでいきます。 現在とひとつ前のパルスカウント値を利用して 差分を求めると、スピードを計算できます。 現在のパルス値を利用して、走行をやめて 停止させます。MCRのコースは、これまでの 大会で最大70mなので、100m走行すると停止 させます。 100msごとに、ロータリーエンコーダの積算 パルス数を取得します。 void tsk6_proc(void) { volatile UWORD xmes ; /* update */ pre_rcnt = cur_rcnt ; /* get upper byte */ xmes = get_fpga(ROTU); xmes <<= 8 ; /* get lower byte */ xmes += get_fpga(ROTL); /* update */ cur_rcnt = xmes ; /* differencial pulse count */ dif_rcnt = cur_rcnt - pre_rcnt ; /* judge */ if ( cur_rcnt == MAX_RUN_PULSE ) { /* stop RUN */ sus_tsk(NORMAL_RUN); sus_tsk(CRANK_RUN); sus_tsk(LANE_CHANGE); /* stop motors */ xmotor.xfront = FRONT_CENTER ; xmotor.xrear = REAR_FORWARD ; send_fpga( xmotor ); } /* interval */ wai_tsk( 10 ) ; /* 100ms */ }タスクSDEBUG
ハードの動作テストをしたい場合、ファームウエア を入替えるのは面倒なので、デバッグ処理をタスク として用意しておきます。 デバッグ用に次のコマンドを用意しました。- ? help
- I initialize camera
- D show sensor data
- F set front duty and direction
- R set rear duty and direction
- S show duty ratio
- V show camera register values
タスクLEDFLASH
走行中に、マシンがどのモードで動いている のかを示すために、４個のLEDを使っています。 スタートトリガー待機中は、４LEDを左右に フラッシングしておきます。 周期的に点灯するLEDを切替え、フラッシング します。どのLEDを点灯するのかは、配列に格納 してステートマシンで決めます。 ステートは、インターバルごとに変数を ＋１することで変えていきます。 300msごとにタスクを動かすと考えて 次のように定義しました。 void tsk8_proc(void) { /* impress */ turn_on_led( cyclic[fstate] ); /* update state */ fstate++ ; /* judge */ if ( fstate == 6 ) { fstate = 0 ; } /* interval 300ms */ wai_tsk(30); }全ソースコード
#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 SCCB_ID_WR 0x42 #define SCCB_ID_RD 0x43 #define S_SCL 23 #define S_SDA 22 #define S_CTRG 23 #define S_TRG 22 #define S_DIR 20 #define S_OE 19 #define TSK_ID_MAX 9 #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 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 ; /*-----------------------*/ /* 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 rs_putchar(UBYTE x); void crlf(void); void rs_puts(UBYTE *x); UBYTE get_hex(UBYTE x); void show_help(void); void init_cam(UBYTE x); void put_sccb_start(void); void put_sccb_stop(void); void put_sccb_data(UBYTE x); UBYTE get_sccb_data(void); UBYTE load_cam(UBYTE adr); void show_reg(void); void show_sensor(UBYTE x); void send_cam(UBYTE adr,UBYTE dat); void delay_ms(UWORD x); void delay_s(UBYTE x); /*---------------*/ /* FPGA handling */ /*---------------*/ typedef struct { UWORD xfront ; UWORD xrear ; } MOTORP ; MOTORP xmotor ; void send_fpga(MOTORP x); UBYTE get_fpga(UBYTE lx); void show_duty(void); void show_data(UBYTE x); UBYTE get_sensor(UBYTE x); void send_cam_trigger(void); void put_mode(UBYTE x); UBYTE is_center(UBYTE x); UBYTE is_left_white(UBYTE x); UBYTE is_right_white(UBYTE x); UBYTE is_tiny_right(UBYTE x); UBYTE is_right(UBYTE x); UBYTE is_big_right(UBYTE x); UBYTE is_tiny_left(UBYTE x); UBYTE is_left(UBYTE x); UBYTE is_big_left(UBYTE x); void turn_off_led(void); void turn_on_led(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 UBYTE state ; volatile UWORD vscnt ; volatile UBYTE vtrg ; volatile UBYTE ptrg ; volatile UBYTE str_sft ; volatile UBYTE str_trg ; volatile UBYTE str_sft_p40 ; volatile UBYTE str_sft_p41 ; volatile UBYTE fduty ; volatile UBYTE rduty ; volatile UBYTE fdir ; volatile UBYTE rdir ; volatile ULONG icount ; volatile UBYTE mode ; #define ILAST 300000 #define S_MODE_BIT 18 #define REG_GAIN 0x00 /* Gain lower 8 bits (rest in vref) */ #define REG_BLUE 0x01 /* blue gain */ #define REG_RED 0x02 /* red gain */ #define REG_VREF 0x03 /* Pieces of GAIN, VSTART, VSTOP */ #define REG_COM1 0x04 /* Control 1 */ #define COM1_CCIR656 0x40 /* CCIR656 enable */ #define REG_BAVE 0x05 /* U/B Average level */ #define REG_GbAVE 0x06 /* Y/Gb Average level */ #define REG_AECHH 0x07 /* AEC MS 5 bits */ #define REG_RAVE 0x08 /* V/R Average level */ #define REG_COM2 0x09 /* Control 2 */ #define COM2_SSLEEP 0x10 /* Soft sleep mode */ #define REG_PID 0x0a /* Product ID MSB */ #define REG_VER 0x0b /* Product ID LSB */ #define REG_COM3 0x0c /* Control 3 */ #define COM3_SWAP 0x40 /* Byte swap */ #define COM3_SCALEEN 0x08 /* Enable scaling */ #define COM3_DCWEN 0x04 /* Enable downsamp/crop/window */ #define REG_COM4 0x0d /* Control 4 */ #define REG_COM5 0x0e /* All "reserved" */ #define REG_COM6 0x0f /* Control 6 */ #define REG_AECH 0x10 /* More bits of AEC value */ #define REG_CLKRC 0x11 /* Clocl control */ #define CLK_EXT 0x40 /* Use external clock directly */ #define CLK_SCALE 0x3f /* Mask for internal clock scale */ #define REG_COM7 0x12 /* Control 7 */ #define COM7_RESET 0x80 /* Register reset */ #define COM7_FMT_MASK 0x38 #define COM7_FMT_VGA 0x00 #define COM7_FMT_CIF 0x20 /* CIF format */ #define COM7_FMT_QVGA 0x10 /* QVGA format */ #define COM7_FMT_QCIF 0x08 /* QCIF format */ #define COM7_RGB 0x04 /* bits 0 and 2 - RGB format */ #define COM7_YUV 0x00 /* YUV */ #define COM7_BAYER 0x01 /* Bayer format */ #define COM7_PBAYER 0x05 /* "Processed bayer" */ #define REG_COM8 0x13 /* Control 8 */ #define COM8_FASTAEC 0x80 /* Enable fast AGC/AEC */ #define COM8_AECSTEP 0x40 /* Unlimited AEC step size */ #define COM8_BFILT 0x20 /* Band filter enable */ #define COM8_AGC 0x04 /* Auto gain enable */ #define COM8_AWB 0x02 /* White balance enable */ #define COM8_AEC 0x01 /* Auto exposure enable */ #define REG_COM9 0x14 /* Control 9 - gain ceiling */ #define REG_COM10 0x15 /* Control 10 */ #define COM10_HSYNC 0x40 /* HSYNC instead of HREF */ #define COM10_PCLK_HB 0x20 /* Suppress PCLK on horiz blank */ #define COM10_HREF_REV 0x08 /* Reverse HREF */ #define COM10_VS_LEAD 0x04 /* VSYNC on clock leading edge */ #define COM10_VS_NEG 0x02 /* VSYNC negative */ #define COM10_HS_NEG 0x01 /* HSYNC negative */ #define REG_HSTART 0x17 /* Horiz start high bits */ #define REG_HSTOP 0x18 /* Horiz stop high bits */ #define REG_VSTART 0x19 /* Vert start high bits */ #define REG_VSTOP 0x1a /* Vert stop high bits */ #define REG_PSHFT 0x1b /* Pixel delay after HREF */ #define REG_MIDH 0x1c /* Manuf. ID high */ #define REG_MIDL 0x1d /* Manuf. ID low */ #define REG_MVFP 0x1e /* Mirror / vflip */ #define MVFP_MIRROR 0x20 /* Mirror image */ #define MVFP_FLIP 0x10 /* Vertical flip */ #define REG_AEW 0x24 /* AGC upper limit */ #define REG_AEB 0x25 /* AGC lower limit */ #define REG_VPT 0x26 /* AGC/AEC fast mode op region */ #define REG_HSYST 0x30 /* HSYNC rising edge delay */ #define REG_HSYEN 0x31 /* HSYNC falling edge delay */ #define REG_HREF 0x32 /* HREF pieces */ #define REG_TSLB 0x3a /* lots of stuff */ #define TSLB_YLAST 0x04 /* UYVY or VYUY - see com13 */ #define REG_COM11 0x3b /* Control 11 */ #define COM11_NIGHT 0x80 /* NIght mode enable */ #define COM11_NMFR 0x60 /* Two bit NM frame rate */ #define COM11_HZAUTO 0x10 /* Auto detect 50/60 Hz */ #define COM11_50HZ 0x08 /* Manual 50Hz select */ #define COM11_EXP 0x02 #define REG_COM12 0x3c /* Control 12 */ #define COM12_HREF 0x80 /* HREF always */ #define REG_COM13 0x3d /* Control 13 */ #define COM13_GAMMA 0x80 /* Gamma enable */ #define COM13_UVSAT 0x40 /* UV saturation auto adjustment */ #define COM13_UVSWAP 0x01 /* V before U - w/TSLB */ #define REG_COM14 0x3e /* Control 14 */ #define COM14_DCWEN 0x10 /* DCW/PCLK-scale enable */ #define REG_EDGE 0x3f /* Edge enhancement factor */ #define REG_COM15 0x40 /* Control 15 */ #define COM15_R10F0 0x00 /* Data range 10 to F0 */ #define COM15_R01FE 0x80 /* 01 to FE */ #define COM15_R00FF 0xc0 /* 00 to FF */ #define COM15_RGB565 0x10 /* RGB565 output */ #define COM15_RGB555 0x30 /* RGB555 output */ #define REG_COM16 0x41 /* Control 16 */ #define COM16_AWBGAIN 0x08 /* AWB gain enable */ #define REG_COM17 0x42 /* Control 17 */ #define COM17_AECWIN 0xc0 /* AEC window - must match COM4 */ #define COM17_CBAR 0x08 /* DSP Color bar */ #define REG_RGB444 0x8c /* RGB 444 control */ #define R444_ENABLE 0x02 /* Turn on RGB444, overrides 5x5 */ #define R444_RGBX 0x01 /* Empty nibble at end */ #define REG_HAECC1 0x9f /* Hist AEC/AGC control 1 */ #define REG_HAECC2 0xa0 /* Hist AEC/AGC control 2 */ #define REG_HAECC7 0xaa /* Hist AEC/AGC control 7 */ #define FRONT_CENTER 0x000 #define FRONT_RIGHT 0x100 #define FRONT_LEFT 0x200 #define REAR_STOP 0x080 #define REAR_FORWARD 0x180 #define REAR_REVERSE 0x280 #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 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 SENX0 0 #define SENX1 1 #define SENX2 2 #define SENX3 3 #define SENX4 4 #define ROTU 5 #define ROTL 6 #define RDUTY 7 #define LED_BLIND 0 #define LED_NORMAL 1 #define LED_CRANK 2 #define LED_LANE 3 volatile system_status ; volatile UBYTE bstate ; volatile UBYTE cstate ; volatile UBYTE ccnt ; volatile UBYTE lstate ; volatile UBYTE lcnt ; volatile UBYTE sensor[5] ; volatile UBYTE sensorx ; 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); #define IDLE 0 #define RUN IDLE+1 volatile UBYTE duty ; 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); /* 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_READY); sta_tsk(SDEBUG ,TTS_READY); sta_tsk(LEDFLASH ,TTS_READY); /* show message */ rs_puts("Hello"); crlf(); /* 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 ; *(sbuf+sindex) = ch ; sindex++ ; if ( ch == '\r' ) { sindex = 0 ; uflag = ON ; } } } /* judge timer3 interruption (1ms) */ if ( (IRQSTA & WATCHDOG_TIMER_BIT) == WATCHDOG_TIMER_BIT ) { /* clear timer3 interrupt flag */ T3CLRI = 0xff ; /* increment */ timcnt++ ; icount++ ; /* os timer */ if ( (timcnt & 0xf) == 10 ) { tflag = ON ; } /* blink */ if ( (timcnt & 0x3ff) == 1000 ) { GP4DAT ^= (1 << 23); } /* judge time up */ if ( icount == ILAST ) { icount = 0 ; state = 0 ; xmotor.xfront = 0 ; xmotor.xrear = REAR_STOP ; } } } 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 ; state = 0 ; vscnt = 0 ; str_sft = 0 ; str_sft_p40 = 0 ; str_sft_p41 = 0 ; mode = 0 ; put_mode( mode ); icount = 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 */ { /* */ GP0DAT = 0xDF1F0000 ; } /* P1 */ { /* use UART */ GP1CON = 0x00000011 ; /* */ GP1DAT = 0xfef00000 ; } /* P2 */ { /* all bits outputs */ GP2DAT = 0xff000000 ; } /* P3 */ { /* all bits outputs */ GP3DAT = 0xff000000 ; } /* P4 */ { GP4DAT = 0xfc000000 ; } /* initialize timer 3 (1s) */ { T3LD = 33 ; /* (32.768kHz / 32) = about 1 kHz */ T3CON = 0xc0 ; /* enable , cyclic , 1/1 */ } timcnt = 0 ; fduty = 0 ; rduty = 0 ; fdir = 0 ; rdir = 0 ; /* clear sensor data */ *(sensor+0) = 0 ; *(sensor+1) = 0 ; *(sensor+2) = 0 ; *(sensor+3) = 0 ; *(sensor+4) = 0 ; send_cam_trigger(); /* clear rotary counter */ cur_rcnt = 0 ; pre_rcnt = 0 ; dif_rcnt = cur_rcnt - pre_rcnt ; /* enable timer 3 interrupt and UART interrupt */ IRQEN = WATCHDOG_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 data") ; crlf(); rs_puts("F set front duty and direction") ; crlf(); rs_puts("R set rear duty and direction") ; crlf(); rs_puts("S show duty ratio") ; crlf(); rs_puts("V show registers") ; crlf(); } void show_reg(void) { volatile UWORD i ; volatile UBYTE tmp ; volatile UBYTE msg[2] ; /* 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 < 208 ; i++ ) { /* vertical ruler */ if ( (i % 16) == 0 ) { rs_putchar('+'); rs_putchar( asc_hex[ i >> 4 ] ) ; rs_putchar('0'); rs_putchar(' '); } /* judge */ if ( i > 202 ) { rs_putchar( '0' ) ; rs_putchar( '0' ) ; } else { /* get register data */ tmp = load_cam((UBYTE)i); /* separate */ *(msg+0) = ((tmp >> 4) & MASK0F) ; *(msg+1) = (tmp & MASK0F) ; /* show */ rs_putchar( asc_hex[*(msg+0)] ) ; rs_putchar( asc_hex[*(msg+1)] ) ; } /* add space */ rs_putchar(' '); /* new line */ tmp = i & MASK0F ; if ( tmp == 15 ) { crlf(); } } } void show_sensor(UBYTE x) { int i ; for ( i = 7 ; i > -1 ; i-- ) { rs_putchar('0'+((x >> i) & 1)); } crlf(); } void put_sccb_start(void) { /* both high level */ GP0DAT |= ((1 << S_SCL) | (1 << S_SDA)) ; /* SCL = ON ; SDA = ON */ delay_ms(1); /* 0 -> SDA */ GP0DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */ delay_ms(1); /* 0 -> SCL */ GP0DAT &= ~(1 << S_SCL) ; /* SCL = OFF ; SDA = OFF */ delay_ms(1); } void put_sccb_stop(void) { /* both high level */ GP0DAT |= (1 << S_SCL) ; /* SCL = ON ; SDA = ? */ delay_ms(1); /* 0 -> SDA */ GP0DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */ delay_ms(1); /* both high level */ GP0DAT |= (1 << S_SDA) ; /* SCL = ON ; SDA = ON */ delay_ms(1); } void put_sccb_data(UBYTE x) { volatile UBYTE tmp ; volatile UBYTE i ; /* transfer data with write code */ for ( i = 0 ; i < 8 ; i++ ) { /* send bit datum */ GP0DAT &= ~(1 << S_SDA) ; if ( tmp & MASK80 ) { GP0DAT |= (1 << S_SDA) ; } /* dummy */ tmp = x ; /* 1 -> SCL */ GP0DAT |= (1 << S_SCL) ; delay_ms(1); /* shift */ x <<= 1 ; /* 0 -> SCL */ GP0DAT &= ~(1 << S_SCL) ; delay_ms(1); /* data low */ GP0DAT &= ~(1 << S_SDA) ; } /* change input */ GP0DAT &= ~(1 << 30) ; delay_ms(1) ; /* 1 -> SCL */ GP0DAT |= (1 << S_SCL) ; delay_ms(1) ; /* get acknowledge */ tmp = OFF ; if ( (GP0DAT & MASK40) == MASK40 ) { tmp = ON ; } /* 0 -> SCL */ GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ; /* change output */ GP0DAT |= (1 << 30) ; /* delay */ delay_ms(1); } UBYTE get_sccb_data(void) { volatile UBYTE tmp ; volatile UBYTE i ; /* change inupt */ GP0DAT &= ~(1 << 30) ; /* default */ tmp = 0 ; /* loop */ for ( i = 0 ; i < 8 ; i++ ) { /* shift */ tmp <<= 1 ; /* 1 -> SCL */ GP0DAT |= (1 << S_SCL) ; delay_ms(1) ; /* get bit datum */ if ( (GP0DAT & MASK40) == MASK40 ) { tmp |= ON ; } /* 0 -> SCL */ GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ; } /* 1 -> SCL */ GP0DAT |= (1 << S_SCL) ; delay_ms(1) ; /* dummy shift */ i <<= 1 ; /* 0 -> SCL */ GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ; /* change output */ GP0DAT |= (1 << 30) ; return tmp ; } void send_cam(UBYTE adr,UBYTE dat) { /* start conditon */ put_sccb_start() ; /* send ID */ put_sccb_data(SCCB_ID_WR); /* send address */ put_sccb_data(adr); /* send parameter */ put_sccb_data(dat); /* stop condition */ put_sccb_stop(); /* delay */ delay_ms(1); } UBYTE load_cam(UBYTE adr) { UBYTE result ; /* start conditon */ put_sccb_start() ; /* send ID (write) */ put_sccb_data(SCCB_ID_WR); /* send address */ put_sccb_data(adr); /* stop condition */ put_sccb_stop(); /* start conditon */ put_sccb_start() ; /* send ID (read) */ put_sccb_data(SCCB_ID_RD); /* get parameter */ result = get_sccb_data(); /* stop condition */ put_sccb_stop(); return result ; } void delay_ms(UWORD x) { ULONG last ; /* set */ last = timcnt + x ; /* delay */ while ( timcnt < last ) ; } void delay_s(UBYTE x) { UWORD i ; /* set */ for ( i = 0 ; i < x ; i++ ) { delay_ms(1000); } } void init_cam(UBYTE x) { /* reset camera */ send_cam(REG_COM7,COM7_RESET); rs_puts("Complete reset"); crlf(); /* delay 200ms */ delay_ms(200); send_cam(REG_COM7,0x00); //send_cam(REG_RGB444,R444_ENABLE | R444_RGBX); //0x8c:RGB 444 control send_cam(REG_COM1,0x40); //0x04:COM1(CCIR656,AEC) //0) //0x40) send_cam(REG_COM15,COM15_R01FE|COM15_RGB565); //0x40:COM15 send_cam(REG_COM9, 0x38); //0x14:COM9=max AGC gain ceiling, Freeze AGC/AEC //c(0x3d,0xc3) //(REG_COM13, COM13_GAMMA|COM13_UVSAT|0x2) //0x3d:COM13 send_cam(REG_HAECC7,0x94); //0xaa:Hist AEC/AGC control 7 c(0xAA,0x94) //AEC algorithm send_cam(REG_TSLB,0x04); //0x3a:Neg,UVval,YUYV,window TSLB_YLAST) //0x04) //0x0C) //0x80) //0x00) //0x04) send_cam(0x20,0x0f); //ADCCTR0, A/D range&ref, mu0102 send_cam(REG_COM8,0x9a); //mu0103 send_cam(0x10,0x01); //mu0103 if ( x ) { /* CLKRC divide /4 -> 5MHz */ send_cam(0x11,0x83); } /* COM1 */ send_cam(0x3b,0x0a); /* disable night mode */ /* TSLB 3 bit => select YUV format */ send_cam(0x3a,0x05); /* COM7 2 bit , 0 bit => select YUV format */ send_cam(REG_COM7,0x00); /* COM15 */ send_cam(0x40,0xd0); /* HSTART */ send_cam(0x17,0x16); /* HSTOP */ send_cam(0x18,0x04); /* HREF */ send_cam(0x32,0x24); /* VSTRT */ send_cam(0x19,0x02); /* VSTOP */ send_cam(0x1a,0x7a); /* VREF */ send_cam(0x03,0x0a); /* COM10 */ send_cam(0x15,0x02); /* COM3 */ send_cam(0x0c,0x04); /* COM4 */ send_cam(0x3e,0x1a); /* SCALING_DCWCTR */ send_cam(0x72,0x22); /* SCALING_PCLK_DIV */ send_cam(0x73,0xf2); /* DM_LNL */ send_cam(0x92,0x00); /* DM_LNH */ send_cam(0x93,0x00); /* decrease frame rate put dummy lines 510 (0x01FE) */ if ( x ) { rs_puts("insert dummy line"); crlf(); /* DM_LNL */ send_cam(0x92,0xFE); /* DM_LNH */ send_cam(0x93,0x01); } rs_puts("Exit initialize"); crlf(); } void send_fpga(MOTORP x) { volatile UBYTE loop; volatile UBYTE i; volatile UBYTE dir ; volatile UBYTE dat ; for ( loop = 0 ; loop < 2 ; loop++ ) { /* get front direction and dat */ dir = (x.xfront >> 8) & MASK03 ; dat = x.xfront & MASKFF ; if ( loop ) { dir = (x.xrear >> 8) & MASK03 ; dat = x.xrear & MASKFF ; } /* impress data */ GP3DAT &= 0xff00ffff ; GP3DAT |= (dat << 16) ; /* impress direction */ GP2DAT &= 0xff07ffff ; /* DIR = 00 , OE = 0 */ GP2DAT |= (dir << S_DIR); /* send latch pulse */ GP2DAT |= (1 << S_TRG); /* trigger H */ for ( i = 0 ; i < 10 ; i++ ); /* delay */ GP2DAT &= ~(1 << S_TRG); /* trigger L */ } } UBYTE get_fpga(UBYTE lx) { UBYTE result ; /* send line number */ GP2DAT &= 0xfff0ffff ; GP2DAT |= (lx << 16) ; /* GP3 inputs */ GP3DAT &= 0x00ffffff ; /* enable FPGA output */ GP2DAT |= (1 << S_OE) ; /* data */ result = GP3DAT & MASKFF ; /* disable FPGA output */ GP2DAT &= ~(1 << S_OE) ; /* GP3 outputs */ GP3DAT |= 0xff000000 ; return result ; } UBYTE get_sensor(UBYTE x) { UBYTE result ; /* default */ result = 0 ; /* judge */ if ( x < 5 ) { result = get_fpga(x) ; } return result ; } 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(); } 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 put_mode(UBYTE x) { GP4DAT &= ~(15 << S_MODE_BIT) ; GP4DAT |= (x << S_MODE_BIT) ; } UBYTE is_center(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x18 ) { result = YES ; } if ( x == 0x38 ) { result = YES ; } if ( x == 0x1C ) { result = YES ; } if ( x == 0x3C ) { result = YES ; } return result ; } UBYTE is_left_white(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0xF0 ) { result = YES ; } if ( x == 0xF8 ) { result = YES ; } if ( x == 0xFC ) { result = YES ; } return result ; } UBYTE is_right_white(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x0F ) { result = YES ; } if ( x == 0x1F ) { result = YES ; } if ( x == 0x3F ) { result = YES ; } return result ; } UBYTE is_tiny_right(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x0C ) { result = YES ; } return result ; } UBYTE is_right(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x06 ) { result = YES ; } if ( x == 0x0E ) { result = YES ; } return result ; } UBYTE is_big_right(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x03 ) { result = YES ; } if ( x == 0x07 ) { result = YES ; } return result ; } UBYTE is_tiny_left(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x30 ) { result = YES ; } return result ; } UBYTE is_left(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0x60 ) { result = YES ; } if ( x == 0x70 ) { result = YES ; } return result ; } UBYTE is_big_left(UBYTE x) { UBYTE result ; /* default */ result = NO ; /* judge */ if ( x == 0xC0 ) { result = YES ; } if ( x == 0xE0 ) { result = YES ; } return result ; } /* 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)); } /* 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 ) { system_status = RUN ; rsm_tsk(BLIND_RUN); sus_tsk(LEDFLASH); turn_off_led(); } else { system_status = IDLE ; sus_tsk(NORMAL_RUN); sus_tsk(CRANK_RUN); sus_tsk(LANE_CHANGE); rsm_tsk(LEDFLASH); /* stop */ xmotor.xfront = FRONT_CENTER ; xmotor.xrear = REAR_FORWARD ; send_fpga( xmotor ); } } /* cycle 20ms */ wai_tsk( 2 ); } /* BLIND_RUN */ void tsk1_proc(void) { /* show mode */ turn_on_led(LED_BLIND); /* select CENTER */ xmotor.xfront = FRONT_CENTER ; /* sequencer */ switch ( bstate ) { /* duty 5% */ case 0 : xmotor.xrear = (REAR_FORWARD | 5) ; break ; /* duty 15% */ case 1 : xmotor.xrear = (REAR_FORWARD | 15) ; break ; /* duty 25% */ case 2 : xmotor.xrear = (REAR_FORWARD | 25) ; break ; /* duty 45% */ case 3 : xmotor.xrear = (REAR_FORWARD | 45) ; break ; /* duty 65% */ case 4 : xmotor.xrear = (REAR_FORWARD | 65) ; break ; /* duty 75% */ case 5 : xmotor.xrear = (REAR_FORWARD | 75) ; break ; /* others */ default : break ; } send_fpga( xmotor ); /* 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 flag2 ; /* show mode */ turn_on_led(LED_NORMAL); /* show sensor data */ //show_sensor(sensorx); /* default */ flag2 = OFF ; xmotor.xfront = FRONT_CENTER ; xmotor.xrear = (REAR_FORWARD | 50) ; /* judge */ /* if ( is_center( sensorx ) == YES ) { frontx = ( FRONT_CENTER ); rearx = ( REAR_FORWARD | 50 ) ; } */ if ( is_tiny_right( sensorx ) == YES ) { xmotor.xfront = (FRONT_RIGHT | 50); xmotor.xrear = (REAR_FORWARD | 75); } if ( is_right( sensorx ) == YES ) { xmotor.xfront = (FRONT_RIGHT | 75); xmotor.xrear = (REAR_FORWARD | 55); } if ( is_big_right( sensorx ) == YES ) { xmotor.xfront = (FRONT_RIGHT | 90); xmotor.xrear = (REAR_FORWARD | 45); } if ( is_tiny_left( sensorx ) == YES ) { xmotor.xfront = (FRONT_LEFT | 50); xmotor.xrear = (REAR_FORWARD | 75); } if ( is_left( sensorx ) == YES ) { xmotor.xfront = (FRONT_LEFT | 75); xmotor.xrear = (REAR_FORWARD | 55); } if ( is_big_left( sensorx ) == YES ) { xmotor.xfront = (FRONT_LEFT | 90); xmotor.xrear = (REAR_FORWARD | 45); } send_fpga( xmotor ) ; /* get CRANK mark */ if ( sensorx == ALL_WHITE ) { white_state = WHITE_STATE_ALL ; rsm_tsk( CRANK_RUN ); flag2 = ON ; } /* get LANE LEFT CHANGE mark */ if ( is_left_white(sensorx) == YES ) { white_state = WHITE_STATE_LEFT ; rsm_tsk( LANE_CHANGE ); flag2 = ON ; } /* get LANE RIGHT CHANGE mark */ if ( is_right_white(sensorx) == YES ) { 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) { /* show mode */ turn_on_led(LED_CRANK); /* show sensor data */ //show_sensor(sensorx); /* sequencer */ switch ( cstate ) { /* blind run until minimum distance */ case 0 : cstate = 1 ; ccnt = 30 ; xmotor.xfront = FRONT_CENTER ; xmotor.xrear = (REAR_FORWARD | 75); send_fpga( xmotor ); break ; /* delay */ case 1 : if ( ccnt == 0 ) { cstate = 2 ; } else { ccnt-- ; } break ; /* until CRANK location */ case 2 : if ( is_left_white(sensorx) == YES ) { cstate = 3 ; white_state = WHITE_STATE_LEFT ; } if ( is_right_white(sensorx) == YES ) { cstate = 3 ; white_state = WHITE_STATE_RIGHT ; } break ; /* judge turn location */ case 3 : if ( sensorx == ALL_WHITE || sensorx == ALL_BLACK ) { cstate = 4 ; } break ; /* turn */ case 4 : cstate = 5 ; if ( white_state == WHITE_STATE_LEFT ) { xmotor.xfront = (FRONT_LEFT | 50); } if ( white_state == WHITE_STATE_RIGHT ) { xmotor.xfront = (FRONT_RIGHT | 50); } xmotor.xrear = (REAR_FORWARD | 50); send_fpga( xmotor ) ; break ; /* judge center */ case 5 : if ( is_center(sensorx) == YES ) { cstate = 6 ; xmotor.xfront = FRONT_CENTER ; send_fpga( xmotor ) ; } break ; /* return first state */ case 6 : break ; /* */ default : break ; } /* delay */ if ( cstate < 6 ) { wai_tsk(1) ; /* 10ms interval */ } else { cstate = 0 ; xmotor.xfront = FRONT_CENTER ; xmotor.xrear = (REAR_FORWARD | 75); send_fpga( xmotor ); white_state = WHITE_STATE_NONE ; slp_tsk() ; turn_off_led(); rsm_tsk( NORMAL_RUN ); } } /* LANE_CHANGE */ void tsk4_proc(void) { /* show mode */ turn_on_led(LED_LANE); /* show sensor data */ //show_sensor(sensorx); /* sequencer */ switch ( lstate ) { /* forward until ALL_BLACK */ case 0 : if ( sensorx == ALL_BLACK ) { lstate = 1 ; } break ; /* judge turn */ case 1 : lstate = 2 ; lcnt = 10 ; xmotor.xfront = (FRONT_RIGHT | 80); if ( white_state == WHITE_STATE_LEFT ) { xmotor.xfront = (FRONT_LEFT | 80); } send_fpga( xmotor ); break ; /* turn delay */ case 2 : if ( lcnt == 0 ) { lstate = 3 ; xmotor.xfront = FRONT_CENTER ; send_fpga( xmotor ); } else { lcnt-- ; } break ; /* blind run */ case 3 : lstate = 4 ; lcnt = 10 ; xmotor.xrear = (REAR_FORWARD | 75); send_fpga( xmotor ); break ; /* blind run delay */ case 4 : if ( lcnt == 0 ) { lstate = 5 ; } else { lcnt-- ; } break ; /* judge center */ case 5 : if ( is_center(sensorx) == YES ) { lstate = 6 ; } break ; /* judge turn (reverse) */ case 6 : lstate = 7 ; lcnt = 10 ; xmotor.xfront= (FRONT_LEFT | 80); if ( white_state == WHITE_STATE_LEFT ) { xmotor.xfront = (FRONT_RIGHT | 80); } send_fpga( xmotor ); break ; /* turn (reverse) delay */ case 7 : if ( lcnt == 0 ) { lstate = 8 ; xmotor.xfront = FRONT_CENTER ; send_fpga( xmotor ); } else { lcnt-- ; } break ; /* return first state */ case 8 : break ; /* */ default : break ; } /* delay */ if ( lstate < 8 ) { wai_tsk(1) ; /* 10ms interval */ } else { lstate = 0 ; xmotor.xfront = FRONT_CENTER ; xmotor.xrear = (REAR_FORWARD | 75); send_fpga( xmotor ); white_state = WHITE_STATE_NONE ; slp_tsk() ; turn_off_led(); rsm_tsk( NORMAL_RUN ); } } /* SENSING */ void tsk5_proc(void) { /* get data */ *(sensor+0) = get_fpga(SENX0) ; *(sensor+1) = get_fpga(SENX1) ; *(sensor+2) = get_fpga(SENX2) ; *(sensor+3) = get_fpga(SENX3) ; *(sensor+4) = get_fpga(SENX4) ; /* load */ sensorx = *(sensor+4) ; /* send trigger */ send_cam_trigger(); /* interval */ wai_tsk( 3 ) ; /* 30ms */ } #define MAX_RUN_PULSE 28000 /* MEASURE */ void tsk6_proc(void) { volatile UWORD xmes ; /* update */ pre_rcnt = cur_rcnt ; /* get upper byte */ xmes = get_fpga(ROTU); xmes <<= 8 ; /* get lower byte */ xmes += get_fpga(ROTL); /* update */ cur_rcnt = xmes ; /* differencial pulse count */ dif_rcnt = cur_rcnt - pre_rcnt ; /* judge */ if ( cur_rcnt == MAX_RUN_PULSE ) { /* stop RUN */ sus_tsk(NORMAL_RUN); sus_tsk(CRANK_RUN); sus_tsk(LANE_CHANGE); /* stop motors */ xmotor.xfront = FRONT_CENTER ; xmotor.xrear = REAR_FORWARD ; send_fpga( xmotor ); } /* interval */ wai_tsk( 10 ) ; /* 100ms */ } /* SDEBUG */ void tsk7_proc(void) { volatile UBYTE tmp7 ; /* 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' ) { init_cam( *(sbuf+1)-'0' ) ; } /* 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 ; duty |= MASK80 ; xmotor.xrear = (rdir << 8) | duty ; } else { fdir = direction ; fduty = duty ; duty &= ~MASK80 ; xmotor.xfront = (fdir << 8) | duty ; } /* impress */ send_fpga( xmotor ); } /* show duty */ if ( cmd == 'S' ) { show_duty(); } /* show sensor data */ if ( cmd == 'D' ) { /* get sensor loacation */ tmp7 = get_hex( *(sbuf+1) ) - '0' ; /* */ show_data( get_fpga(tmp7) ); } /* show register values */ if ( cmd == 'V' ) { show_reg() ; } } /* 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); } /*------------------*/ /* 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 ; } }ファームウエアデバッグ
RTOSを使ったシステムでは、タスクの切替えが 設計した通りであるか、確認しなければなりま せん。 MCR_VCマシンでは、タスク切替えにセンサーデータ を利用しているので、センサーデータを自由に可変 して、FPGAに与えられる環境を用意しました。
手持ちのCPLD、DIPスイッチ基板、LED基板を
利用し、DIPスイッチで設定した８ビット値を
FPGAに与えます。
FPGAは与えられた８ビット値を、センサーデータ
としてARMからの要求で、渡します。
ARMに載せてあるファームウエアは、タスクを切替
にセンサーデータを利用しています。８ビット値を
見てタスク切替します。
CPLDが5V動作、FPGAは3.3V動作なので、間に電圧変換
基板を入れてます。また、CPLDでは、DIPスイッチの
８ビット値をLEDに転送し、モニタさせました。
ファームウエアを構成する以下のタスクでは、センサー
データをシリアルインタフェースを利用し、PCの端末
ソフトに送ります。
- NORMAL_RUN
- CRANK_RUN
- LANE_CHANGE
CPLDに入れたデジタル回路は単純で、次のVHDLコード
としています。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity gensen is
generic (
TOPX : integer := 15 ;
XMAX : integer := 20000 --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- trigger
TRG : in std_logic ;
-- data input
DIN : in std_logic_vector(7 downto 0) ;
-- data output
DOUT : out std_logic_vector(7 downto 0) ;
-- monitor output
MDOUT : out std_logic_vector(7 downto 0) --;
);
end gensen;
architecture behavioral of gensen is
-- clock generator component
component clkgenx is
generic (
TOPX : integer ;
RMAX : integer --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- output
CLKOUT : out std_logic -- ;
);
end component ;
-- clock
signal iSCLK : std_logic ;
-- trigger input
signal iTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG : std_logic ;
-- register
signal iREG : std_logic_vector(7 downto 0) ;
begin
-- clock generator component
CLKX : clkgenx generic map (TOPX,XMAX) port map (nRESET,CLOCK,iSCLK) ;
-- trigger input
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iTRG_SFT <= "000" ;
elsif rising_edge(iSCLK) then
iTRG_SFT <= iTRG_SFT(1 downto 0) & (not TRG) ;
end if ;
end process ;
iTRG <= '1' when ( iTRG_SFT = "011" or iTRG_SFT = "001" ) else '0' ;
-- input
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iREG <= (others => '0') ;
elsif rising_edge(iSCLK) then
if ( iTRG = '1' ) then
iREG <= DIN ;
end if ;
end if ;
end process ;
-- output
DOUT <= iREG ;
-- monitor output
MDOUT <= not iREG ;
end behavioral;
4MHzを分周し、10kHzでCPLDを動かし、I/O基板と
情報をやり取りします。
ピンアサインは、以下。
# system
NET "clock" LOC = "P5" ;
NET "nreset" LOC = "P39" ;
# data input
NET "din<0>" LOC = "P1" ;
NET "din<1>" LOC = "P2" ;
NET "din<2>" LOC = "P3" ;
NET "din<3>" LOC = "P4" ;
NET "din<4>" LOC = "P6" ;
NET "din<5>" LOC = "P7" ;
NET "din<6>" LOC = "P8" ;
NET "din<7>" LOC = "P9" ;
# data output
NET "dout<0>" LOC = "P11" ;
NET "dout<1>" LOC = "P12" ;
NET "dout<2>" LOC = "P13" ;
NET "dout<3>" LOC = "P14" ;
NET "dout<4>" LOC = "P18" ;
NET "dout<5>" LOC = "P19" ;
NET "dout<6>" LOC = "P20" ;
NET "dout<7>" LOC = "P22" ;
# monitor
NET "mdout<0>" LOC = "P24" ;
NET "mdout<1>" LOC = "P25" ;
NET "mdout<2>" LOC = "P26" ;
NET "mdout<3>" LOC = "P27" ;
NET "mdout<4>" LOC = "P28" ;
NET "mdout<5>" LOC = "P29" ;
NET "mdout<6>" LOC = "P33" ;
NET "mdout<7>" LOC = "P34" ;
# trigger
NET "trg" LOC = "P40" ;
デバッグ環境で、センサーデータの変数名を
間違えていることを見つけました。
ファームウエアをRTOSで記述しているとタスク
切替の条件が妥当かと、タスクの切替が起きた
かを見ないと実用になりません。このような
スイッチとLEDを利用したデバッグ環境を構築
するには、CPLDやFPGAが有効かも知れません。
目次 前 次