目次
前
次
GameBoyCameraも接続
手元にあるシリアルインタフェースカメラは、１個だけです。
大会会場での破損も想定し、GameBoyCameraも接続できるようにします。
GameBoyCameraの中は、光学系、人工網膜チップ、コネクタが入っています。
GameBoyCameraは、画像データをアナログ出力してくるので、シリアル
インタフェースとは別のポートを利用します。
最初にAVRテストボードを利用し、GameBoyCameraとの接続をテストしました。
テストボードの回路図は、以下です。
AVRテストボードは、汎用で使えるようにしてあるため、接続用アダプタを
用意しています。
回路図（結線図）は、以下です。
接続用アダプタには、照明用ＬＥＤを駆動する回路も入れました。
A/Dコンバータテスト
内蔵のA/Dコンバータを利用するので、回路図からチャネル０に
アナログ電圧を接続します。
アナログ出力を可変できるように、次の治具を作りました。
回路は簡単に、OPアンプで電圧フォロワを構成しています。
単電源で利用できるOPアンプLM358を使っています。
５Ｖ電圧だと、出力が最大3.5Vになりますが、GameBoyCameraの電圧出力が
最大でも２Ｖなので、この治具で充分です。
テストファームウエアは、自作のRTOS(Real Time Operating System)である
USOを使い、単純に作りました。
RTOSを入れたのは、A/D変換を１回だけするか、連続で実行するかを
切り分けて使いたかったから。
アナログ電圧出力治具上の可変抵抗器のワイパーを回し、端末ソフト
で、A/D変換の結果を観測できます。
タスクを３つに分割してあります。
- タスク０ シリアルインタフェース処理
- タスク１ ワンショットA/D変換
- タスク２ 連続A/D変換
連続A/D変換の周期は、１秒にしました。
A/D変換のテストをするため、シリアルインタフェースの通信プロトコルを
次のように定めました。
- 通信速度 9600bps
- データ長 8ビット
- ストップビット 1ビット
- パリティ なし
- フロー制御 なし
コマンドは、次の４つにしました。
- ？ ヘルプ
- Ｃ 連続A/D変換開始
- ｃ 連続A/D変換終了
- Ａ ワンショットＡ／Ｄ変換
ソースコードは、以下です。
#include <avr/io.h>
#include <avr/io8535.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#define OFF 0
#define ON OFF+1
#define FOSC 8000000
#define BAUD 9600
#define MYUBRR (FOSC/16/BAUD)-1
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long ULONG ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
typedef struct {
void (*tsk)(void);
UWORD wcount ;
} TCBP ;
#define TSK_ID_MAX 3
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
volatile TCBP tcb[TSK_ID_MAX];
typedef union {
struct {
unsigned char B0:1;
unsigned char B1:1;
unsigned char B2:1;
unsigned char B3:1;
unsigned char B4:1;
unsigned char B5:1;
unsigned char B6:1;
unsigned char B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP x_flags ;
#define UFLAG x_flags.BIT.B0
#define WFLAG x_flags.BIT.B1
#define CFLAG x_flags.BIT.B2
#define AFLAG x_flags.BIT.B3
#define BUFSIZE 8
#define PMAX 100
volatile UBYTE sbuf[BUFSIZE] ;
volatile UBYTE sindex;
volatile UBYTE mcount ;
volatile UBYTE pcnt ;
volatile UBYTE duty ;
volatile UBYTE dutyx ;
volatile UBYTE run_tsk ;
volatile UBYTE ready ;
volatile UBYTE suspend ;
volatile UBYTE waitq ;
volatile UBYTE bstate ;
const prog_char msg_h[] PROGMEM = "? help" ;
const prog_char msg_c[] PROGMEM = "C start show" ;
const prog_char msg_r[] PROGMEM = "c stop show" ;
const prog_char msg_a[] PROGMEM = "A get ADC" ;
const prog_char msg_p[] PROGMEM = "P set duty ratio(Pvv)" ;
const prog_char msg_sp[] PROGMEM = "p show duty ratio" ;
const prog_char msg_first[] PROGMEM = "ADC test" ;
#define MSG_HELP 0
#define MSG_CONS 1
#define MSG_CONE 2
#define MSG_ADC 3
#define MSG_PULSE 4
#define MSG_DUTY 5
#define NO 0
#define YES NO+1
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASK80 0x80
#define DEBUG TSK_ID0
#define TRIGGER TSK_ID1
#define BLINDRUN TSK_ID2
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void uart_putchar(UBYTE x);
void crlf(void);
void uart_puts(UBYTE *x);
void first_message(void);
UBYTE get_hex(UBYTE x);
void start_adc(void);
void wait_adc(void);
void show_value(UWORD x);
UWORD get_adc(void);
void send_uart_msg(UBYTE x);
/* system call */
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);
void init_os(void);
UBYTE is_tsk_ready(UBYTE tid);
/* task */
void tsk0_proc(void);
void tsk1_proc(void);
void tsk2_proc(void);
/*------*/
/* main */
/*------*/
int main(void)
{
TCBP pcur_tsk ;
UBYTE tmp ;
UBYTE i ;
/* disable interrupt */
cli();
/* initialize port and variables */
user_initialize();
/* initialize monitor */
init_os();
/* create task */
cre_tsk(DEBUG ,tsk0_proc);
cre_tsk(TRIGGER ,tsk1_proc);
cre_tsk(BLINDRUN,tsk2_proc);
/* start task task */
sta_tsk(DEBUG ,TTS_READY);
sta_tsk(TRIGGER ,TTS_SUSPEND);
sta_tsk(BLINDRUN,TTS_SUSPEND);
/* enable interrupt */
sei();
/* endless loop */
first_message();
run_tsk = DEBUG ;
while ( ON ) {
/* RTOS */
pcur_tsk = tcb[run_tsk] ;
if ( is_tsk_ready( run_tsk ) == ON ) { (*(pcur_tsk.tsk))(); }
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = DEBUG ; }
/* cyclic handling and monitering */
if ( WFLAG ) {
WFLAG = OFF ;
tmp = waitq ;
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
if ( tmp & 1 ) {
tcb[i].wcount-- ;
if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
}
tmp >>= 1 ;
}
}
}
/* dummy */
return 0 ;
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
/* PORT A */
PORTA = 0b00001000 ; /* 00001000 */
DDRA = 0b11111100 ; /* ooooooii */
/* PORT B */
PORTB = 0b00000000 ; /* 00000000 */
DDRB = 0b11111111 ; /* oooooooo */
/* PORT C */
PORTC = 0b00000000 ; /* 00000000 */
DDRC = 0b11111111 ; /* oooooooo */
/* PORT D */
PORTD = 0b00001100 ; /* 00001100 */
DDRD = 0b11110010 ; /* ooooiioi */
/* initialize flags */
x_flags.DR = 0 ;
/* initialize serial */
{
/* clear index */
sindex = 0 ;
/* set Baud Rate Registers */
UBRR = MYUBRR ;
/* Enable receive interrupt , receive module and transmit module */
UCR = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE);
}
/* initialize ADC */
{
/* select channel 0 */
ADMUX = 0 ;
/* enable ADC , enable interrupt , select prescaler (/128) */
ADCSR = (1 << ADEN) | (7 << ADPS0) ;
}
/* initialize timer1 */
{
mcount = 0 ;
/* select CTC , (presclae /8) 1MHz */
TCCR1B = (1 << CTC1) | (1 << CS11) ;
/* clear timer/counter */
TCNT1 = 0 ;
OCR1A = 99 ;
OCR1B = 110 ;
}
/* Enable Compare match interruption */
TIMSK = (1 << OCIE1A) ;
/* */
pcnt = 0 ;
duty = 50 ;
dutyx = 50 ;
}
/* UART receive interrupt */
ISR(UART_RX_vect)
{
UBYTE ch ;
/* get 1 charactoer */
ch = UDR ;
/* store */
*(sbuf+sindex) = ch ;
sindex++ ;
/* judge */
if ( ch == '\r' ) {
UFLAG = ON ;
sindex = 0 ;
}
}
/* TIMER1 interrupt */
ISR(TIMER1_COMPA_vect)
{
UBYTE aport ;
/* update monitor count */
mcount++ ;
/* judge */
if ( mcount == 100 ) {
mcount = 0 ;
WFLAG = ON ;
}
/* PWM default */
aport = 0 ;
/* judge */
if ( pcnt < dutyx ) { aport |= MASK80 ; }
/* impress */
PORTA = aport ;
/* increment */
pcnt++ ;
if ( pcnt == PMAX ) {
pcnt = 0 ;
dutyx = duty;
}
}
UBYTE get_hex(UBYTE x)
{
volatile UBYTE result ;
/* default */
result = 0 ;
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 ;
}
void uart_putchar(UBYTE x)
{
while ( !(USR & (1 << UDRE)) ) ;
UDR = x ;
}
void crlf(void)
{
uart_putchar('\r');
uart_putchar('\n');
}
void uart_puts(UBYTE *x)
{
while ( *x != '\0' ) {
uart_putchar( *x ) ;
x++ ;
}
crlf();
}
void first_message(void)
{
char msg[20];
/* copy */
strcpy_P(msg,msg_first);
/* show */
uart_puts((UBYTE *)msg);
}
void start_adc(void)
{
ADCSR |= (1 << ADSC) ;
}
void wait_adc(void)
{
while ( ADCSR & 0x40 ) ;
}
void show_value(UWORD x)
{
UWORD tmp ;
UBYTE i ;
UBYTE val[4] ;
/* store */
tmp = x ;
/* separate */
*(val+0) = tmp / 1000 + '0' ; tmp %= 1000 ;
*(val+1) = tmp / 100 + '0' ; tmp %= 100 ;
*(val+2) = tmp / 10 + '0' ;
*(val+3) = tmp % 10 + '0' ;
/* show */
for ( i = 0 ; i < 4 ; i++ ) { uart_putchar( *(val+i) ); }
/* new line */
crlf();
}
void send_uart_msg(UBYTE x)
{
char msg[20] ;
/* set message */
switch (x) {
case MSG_HELP : strcpy_P(msg,msg_h) ; break ;
case MSG_CONS : strcpy_P(msg,msg_c) ; break ;
case MSG_CONE : strcpy_P(msg,msg_r) ; break ;
case MSG_ADC : strcpy_P(msg,msg_a) ; break ;
case MSG_PULSE : strcpy_P(msg,msg_p) ; break ;
case MSG_DUTY : strcpy_P(msg,msg_sp); break ;
}
/* send */
uart_puts( (UBYTE *)msg );
}
/* system call */
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
tcb[tid].tsk = tsk;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
UBYTE 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)
{
UBYTE tmp ;
tmp = (1 << tid);
ready |= tmp;
suspend &= ~tmp;
waitq &= ~tmp;
}
void sus_tsk(UBYTE tid)
{
UBYTE tmp ;
tmp = (1 << tid);
ready &= ~tmp;
suspend |= tmp;
waitq &= ~tmp;
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
UBYTE tmp ;
tmp = (1 << run_tsk);
ready &= ~tmp;
suspend &= ~tmp;
waitq |= tmp;
tcb[run_tsk].wcount = x ;
}
void init_os(void)
{
ready = 0;
suspend = 0;
waitq = 0;
}
UBYTE is_tsk_ready(UBYTE tid)
{
return( (ready >> tid) & 1 ) ;
}
void tsk0_proc(void)
{
UBYTE cmd ;
UBYTE val[2] ;
/* judge */
if ( UFLAG == OFF ) return ;
/* clear flag */
UFLAG = OFF ;
/* get command character */
cmd = *(sbuf+0) ;
/* one shot */
if ( cmd == 'A' ) { rsm_tsk(TSK_ID1) ; }
/* start continuous */
if ( cmd == 'C' ) { rsm_tsk(TSK_ID2) ; }
/* exit continuous */
if ( cmd == 'c' ) { sus_tsk(TSK_ID2) ; }
/* set duty */
if ( cmd == 'P' ) {
*(val+0) = get_hex( *(sbuf+1) ) ;
*(val+1) = get_hex( *(sbuf+2) ) ;
duty = val[0] * 10 + val[1] ;
}
/* show duty */
if ( cmd == 'p' ) {
/* duty */
*(val+0) = duty / 10 ;
*(val+1) = duty % 10 ;
uart_putchar( *(val+0) + '0' );
uart_putchar( *(val+1) + '0' );
crlf();
}
/* help */
if ( cmd == '?' ) {
send_uart_msg(MSG_HELP);
send_uart_msg(MSG_CONS);
send_uart_msg(MSG_CONE);
send_uart_msg(MSG_ADC);
send_uart_msg(MSG_PULSE);
send_uart_msg(MSG_DUTY);
}
/* new line */
crlf();
}
void tsk1_proc(void)
{
/* show */
show_value( get_adc() );
/* exit */
slp_tsk();
}
void tsk2_proc(void)
{
/* show */
show_value( get_adc() );
/* cyclic 1000 ms */
wai_tsk(100);
}
UWORD get_adc(void)
{
UWORD result ;
UBYTE dh ;
UBYTE dl ;
/* start ADC */
start_adc();
/* wait */
wait_adc();
/* get data */
dl = ADCL ;
dh = ADCH ;
result = (dh << 8) | dl ;
return result ;
}
動作させてわかったことをリストします。
- A/D変換値の１０ビットデータは、下位、上位の順で読み出す
- A/D変換器に与えるクロックは、200kHz以下にする
- A/D変換入力に特別なフィルタ等は不要
GBC利用ファームウエア
A/D変換のテストができたので、GBC用のファームウエアを作成します。
（GBCの使い方は、他のページを参照してください。）
テスト用ボードとして、ATmega328を利用しました。
このボードの右上にある１０ピンコネクタに、ケーブルを利用して
GameBoyCamera用インタフェースボードを接続します。
作成したファームウエアの要点をリストします。
- RTOSを使い、４つのタスクでMCRマシンを制御
- RTOSで、必要機能を単体テストを簡略化
- タスク０は、ハードウエアのテストデバッグに用いる
- タスク１は、スタートとストップのトリガーを処理
- タスク２は、スタートボタンを押してから直進走行を担当
- タスク３は、走行を制御
作成したファームウエアは、以下です。
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#define OFF 0
#define ON OFF+1
#define FOSC 16000000
#define BAUD 38400
#define MYUBRR (FOSC/16/BAUD)-1
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long ULONG ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
typedef struct {
UBYTE direction ;
UBYTE rlduty ;
UBYTE duty ;
} PARAMP ;
volatile PARAMP param ;
typedef struct {
void (*tsk)(void);
UWORD wcount ;
} TCBP ;
#define TSK_ID_MAX 4
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
volatile TCBP tcb[TSK_ID_MAX];
typedef union {
struct {
unsigned char B0:1;
unsigned char B1:1;
unsigned char B2:1;
unsigned char B3:1;
unsigned char B4:1;
unsigned char B5:1;
unsigned char B6:1;
unsigned char B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP x_flags ;
#define UFLAG x_flags.BIT.B0
#define CFLAG x_flags.BIT.B1
#define AFLAG x_flags.BIT.B2
#define WFLAG x_flags.BIT.B3
#define DFLAG x_flags.BIT.B7
#define BUFSIZE 8
#define START 4
#define SIN 5
#define LOAD 4
#define RST 3
#define XCK 2
#define READ 1
#define PMAX 100
#define DIR_CENTER 0
#define DIR_RIGHT 1
#define DIR_LEFT 2
volatile UBYTE sbuf[BUFSIZE] ;
volatile UBYTE sindex;
volatile ULONG tickcount ;
volatile UBYTE mcount ;
volatile UBYTE pcnt ;
volatile UBYTE duty[2] ;
volatile UBYTE dutyx[2] ;
volatile UBYTE direction[2] ;
volatile UBYTE led_duty ;
volatile UBYTE state ;
volatile UBYTE dir ;
volatile UBYTE lane ;
volatile UBYTE threshold ;
volatile UBYTE rdat[8] ;
volatile UBYTE sdat[128] ;
volatile UWORD img_cnt ;
volatile UBYTE bstate ;
volatile UBYTE run_tsk ;
volatile UBYTE ready ;
volatile UBYTE suspend ;
volatile UBYTE waitq ;
const prog_char msg_h[] PROGMEM = "? help" ;
const prog_char msg_c[] PROGMEM = "C reset camera" ;
const prog_char msg_r[] PROGMEM = "R set register value" ;
const prog_char msg_g[] PROGMEM = "G get image" ;
const prog_char msg_s[] PROGMEM = "S default setting" ;
const prog_char msg_d[] PROGMEM = "D debug" ;
const prog_char msg_p[] PROGMEM = "P set duty ratio(Pdvvc)" ;
const prog_char msg_sp[] PROGMEM = "p show duty ratio" ;
const prog_char msg_st[] PROGMEM = "START TRIGGER" ;
const prog_char msg_gi[] PROGMEM = "Get IMAGE" ;
const prog_char msg_cp[] PROGMEM = "COMPLETE" ;
const prog_char msg_first[] PROGMEM = "MCR controller" ;
#define MSG_HELP 0
#define MSG_RESET 1
#define MSG_REGISTER 2
#define MSG_IMAGE 3
#define MSG_DEFAULT 4
#define MSG_DEBUG 5
#define MSG_ST 6
#define MSG_GI 7
#define MSG_CP 8
#define MSG_PULSE 9
#define MSG_DUTY 10
#define NO 0
#define YES NO+1
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASK80 0x80
#define MAX_SIZE 16384
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define LITTLE_LEFT 5
#define LEFT 6
#define BIG_LEFT 7
#define LITTLE_RIGHT 8
#define RIGHT 9
#define BIG_RIGHT 10
#define CRANK_RIGHT 11
#define CRANK_LEFT 12
#define ILLEAGAL 13
#define LANE_NONE 0
#define LANE_RIGHT 1
#define LANE_LEFT 2
#define DEBUG TSK_ID0
#define TRIGGER TSK_ID1
#define BLINDRUN TSK_ID2
#define MOVE TSK_ID3
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void uart_putchar(UBYTE x);
void crlf(void);
void uart_puts(UBYTE *x);
void first_message(void);
UBYTE get_hex(UBYTE x);
void delay_ms(UWORD x);
void set_xck(UBYTE x);
void set_sin(UBYTE x);
void reset_cam(void);
void send_cam(UWORD x);
void send_start(void);
void get_img(UBYTE x);
void send_default(void);
void start_adc(void);
void wai_adc(void);
void show_value(UBYTE x);
void send_uart_msg(UBYTE x);
void update_motor(PARAMP x);
void get_threshold(void);
void get_sensor(void);
UBYTE is_center(UWORD x);
UBYTE is_edge(UWORD x);
/* system call */
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);
void init_os(void);
UBYTE is_tsk_ready(UBYTE tid);
void tsk0_proc(void);
void tsk1_proc(void);
void tsk2_proc(void);
void tsk3_proc(void);
void straight_move(UBYTE x,UWORD tx);
void right_move(UBYTE x);
void left_move(UBYTE x);
void crank_move(UBYTE xa,UBYTE xb,UBYTE xc);
void lane_move(UBYTE xa,UBYTE xb,UBYTE xc);
UBYTE is_crank_center(UBYTE x);
UBYTE is_lane_edge(UBYTE x);
UBYTE convert_code(UBYTE x);
/*------*/
/* main */
/*------*/
int main(void)
{
TCBP pcur_tsk ;
UBYTE tmp ;
UBYTE i ;
/* disable interrupt */
cli();
/* initialize port and variables */
user_initialize();
/* initialize monitor */
init_os();
/* create task */
cre_tsk(DEBUG ,tsk0_proc);
cre_tsk(TRIGGER ,tsk1_proc);
cre_tsk(BLINDRUN,tsk2_proc);
cre_tsk(MOVE ,tsk3_proc);
/* start task task */
sta_tsk(DEBUG ,TTS_READY);
sta_tsk(TRIGGER ,TTS_READY);
sta_tsk(BLINDRUN,TTS_SUSPEND);
sta_tsk(MOVE ,TTS_SUSPEND);
/* enable interrupt */
sei();
/* endless loop */
first_message();
run_tsk = DEBUG ;
while ( ON ) {
/* RTOS */
pcur_tsk = tcb[run_tsk] ;
if ( is_tsk_ready( run_tsk ) == ON ) { (*(pcur_tsk.tsk))(); }
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = DEBUG ; }
/* cyclic handling */
if ( WFLAG ) {
WFLAG = OFF ;
tmp = waitq ;
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
if ( tmp & 1 ) {
tcb[i].wcount-- ;
if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
}
tmp >>= 1 ;
}
}
}
/* dummy */
return 0 ;
}
#/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
UWORD tmp ;
/* PORT B */
PORTB = 0b00000000 ; /* 00000000 */
DDRB = 0b00111111 ; /* iioooooo */
/* PORT C */
PORTC = 0b00001000 ; /* 00001000 */
DDRC = 0b11111100 ; /* ooooooii */
/* PORT D */
PORTD = 0b00001100 ; /* 00001100 */
DDRD = 0b11110010 ; /* oooooioi */
/* initialize flags */
x_flags.DR = 0 ;
/* initialize serial */
{
/* clear index */
sindex = 0 ;
/* set Baud Rate Registers */
tmp = MYUBRR ;
UBRR0H = (tmp >> 8) & MASKFF ;
UBRR0L = tmp & MASKFF;
/* Enable receive interrupt , receive module and transmit module */
UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0) ;
UCSR0C = (3 << UCSZ00) ;
}
/* initialize ADC */
{
/* external 2.56V , select channel 0 */
ADMUX = 0 ;
/* enable ADC , enable interrupt , select prescaler (/128) */
ADCSRA = (1 << ADEN) | (7 << ADPS0) ;
}
/* initialize timer0 */
{
/* select CTC */
TCCR0A = (1 << WGM01) ;
/* (presclae /64) 250kHz */
TCCR0B = (3 << CS00) ;
/* clear timer/counter */
TCNT0 = 0 ;
OCR0A = 125 ;
OCR0B = 250 ;
/* Enable Compare match interruption */
TIMSK0 = (1 << OCIE0A) ;
}
/* initialize timer1 */
{
tickcount = 0 ;
mcount = 0 ;
/* select CTC , (presclae /8) 2MHz */
TCCR1B = (1 << WGM12) | (1 << CS11) ;
/* clear timer/counter */
TCNT1 = 0 ;
OCR1A = 1999 ;
OCR1B = 2500 ;
/* Enable Compare match interruption */
TIMSK1 = (1 << OCIE1A) ;
}
/* initialize interrupt */
{
bstate = 0 ;
/* select falling edge */
EICRA = (1 << ISC01) ;
/* Enable external interruption */
EIMSK = (1 << INT0) ;
}
/* duty ratio */
pcnt = 0 ;
state = 0 ;
*(duty+0) = 0 ;
*(duty+1) = 0 ;
*(dutyx+0) = 0 ;
*(dutyx+1) = 0 ;
*(direction+0) = 0 ;
*(direction+1) = DIR_LEFT ;
led_duty = 10 ;
/* default direction */
dir = DIR_CENTER ;
lane = LANE_NONE ;
}
/* INT0 interrupt */
ISR(INT0_vect)
{
AFLAG = ON ;
}
/* UART receive interrupt */
ISR(USART_RX_vect)
{
UBYTE ch ;
/* get 1 charactoer */
ch = UDR0 ;
/* store */
*(sbuf+sindex) = ch ;
sindex++ ;
/* judge */
if ( ch == '\r' ) {
UFLAG = ON ;
sindex = 0 ;
}
}
/* TIMER0 interrupt */
ISR(TIMER0_COMPA_vect)
{
UBYTE bport ;
UBYTE dport ;
/* default */
bport = 0 ;
dport = PORTD & ~0x20 ;
/* judge */
if ( pcnt < *(dutyx+0) ) {
if ( *(direction+0) == 1 ) { bport |= 0x01 ; }
if ( *(direction+0) == 2 ) { bport |= 0x02 ; }
}
if ( pcnt < *(dutyx+1) ) {
if ( *(direction+1) == 1 ) { bport |= 0x04 ; }
if ( *(direction+1) == 2 ) { bport |= 0x08 ; }
}
if ( pcnt < led_duty ) { dport |= 0x20 ; }
/* impress */
PORTB = bport ;
PORTD = dport ;
/* increment */
pcnt++ ;
if ( pcnt == PMAX ) {
pcnt = 0 ;
*(dutyx+0) = *(duty+0);
*(dutyx+1) = *(duty+1);
}
}
/* TIMER1 interrupt */
ISR(TIMER1_COMPA_vect)
{
/* increment */
tickcount++ ;
/* increment */
mcount++ ;
/* judge */
if ( mcount == 10 ) {
mcount = 0 ;
WFLAG = ON ;
}
}
UBYTE get_hex(UBYTE x)
{
volatile UBYTE result ;
/* default */
result = 0 ;
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 ;
}
void uart_putchar(UBYTE x)
{
while ( !(UCSR0A & (1 << UDRE0)) ) ;
UDR0 = x ;
}
void crlf(void)
{
uart_putchar('\r');
uart_putchar('\n');
}
void uart_puts(UBYTE *x)
{
while ( *x != '\0' ) {
uart_putchar( *x ) ;
x++ ;
}
crlf();
}
void first_message(void)
{
char msg[20];
/* copy */
strcpy_P(msg,msg_first);
/* show */
uart_puts((UBYTE *)msg);
}
void delay_ms(UWORD x)
{
ULONG target ;
/* set counter */
target = tickcount + (ULONG)x ;
/* wait */
while ( target > tickcount ) ;
}
void set_xck(UBYTE x)
{
/* impress signal */
if ( x ) { PORTC |= (1 << XCK) ; }
else { PORTC &= ~(1 << XCK) ; }
}
void set_sin(UBYTE x)
{
if ( x ) { PORTC |= (1 << SIN) ; }
else { PORTC &= ~(1 << SIN) ; }
}
void reset_cam(void)
{
/* impress XCK:L */
set_xck(OFF) ;
/* impress RST:L */
PORTC &= ~(1 << RST) ;
/* impress XCK:H */
set_xck(ON) ;
/* impress RST:H */
PORTC |= (1 << RST) ;
/* impress XCK:L */
set_xck(OFF) ;
/* debugging */
if ( DFLAG ) { send_uart_msg(MSG_RESET) ; }
}
void send_cam(UWORD x)
{
UBYTE i ;
UWORD tmp ;
tmp = x ;
/* impress XCK:L */
set_xck(OFF) ;
/* send address */
for ( i = 0 ; i < 3 ; i++ ) {
/* impress data bit */
set_sin(OFF);
if ( tmp & 0x400 ) { set_sin(ON); }
/* impress XCK:H */
set_xck(ON) ;
/* shift */
tmp <<= 1 ;
/* impress XCK:L */
set_xck(OFF) ;
}
/* send data */
PORTC &= ~(1 << LOAD);
tmp = x ;
for ( i = 0 ; i < 8 ; i++ ) {
/* impress LOAD bit */
if ( i == 7 ) { PORTC |= (1 << LOAD); }
/* impress data bit */
set_sin(OFF);
if ( tmp & MASK80 ) { set_sin(ON); }
/* impress XCK:H */
set_xck(ON) ;
/* shift */
tmp <<= 1 ;
/* impress XCK:L */
set_xck(OFF) ;
}
PORTC &= ~(1 << LOAD);
set_sin(OFF);
}
void send_start(void)
{
/* impress XCK:L */
set_xck(OFF) ;
/* impress STARST:H */
PORTD |= (1 << START);
/* impress XCK:H */
set_xck(ON) ;
/* impress RST:L */
PORTD &= ~(1 << START);
/* impress XCK:L */
set_xck(OFF) ;
}
void get_img(UBYTE x)
{
UBYTE dh ;
UBYTE dl ;
UWORD result ;
UWORD max ;
UWORD min ;
/* debug */
if ( DFLAG ) { send_uart_msg(MSG_ST) ; }
/* send start */
send_start();
/* debug */
if ( DFLAG ) { send_uart_msg(MSG_GI) ; }
/* READ data and show*/
img_cnt = 0 ;
max = 0 ;
min = 0 ;
while ( img_cnt < MAX_SIZE ) {
/* impress XCK:H */
set_xck(ON);
/* impress XCK:L */
set_xck(OFF) ;
/* show */
if ( PINC & 2 ) {
/* start conversion */
start_adc() ;
/* wait */
wai_adc() ;
/* get data */
dl = ADCL ;
dh = ADCH ;
result = (dh << 8) + dl ;
result >>= 2 ;
/* show */
if ( x == 0 ) {
show_value( (UBYTE)result );
/* new line */
if ( (img_cnt % 16) == 15 ) { crlf(); }
}
/* store */
if ( x == 1 ) {
if ( is_center( img_cnt ) ) { max += result ; }
if ( is_edge( img_cnt ) ) { min += result ; }
}
/* increment */
img_cnt++ ;
}
}
/* debug */
if ( DFLAG ) { send_uart_msg(MSG_CP) ; }
/* calculate */
if ( x == 1 ) {
max >>= 2 ;
min >>= 2 ;
/* store */
threshold = (UBYTE)((max+min) >> 1);
}
}
void send_default(void)
{
send_cam((0 << 8) | 0x80);
/* exposure */
send_cam((1 << 8) | 0x03);
/* exposure time change this to adjust picture light */
send_cam((2 << 8) | 0x00);
send_cam((3 << 8) | 0x30);
send_cam((4 << 8) | 0x01);
send_cam((5 << 8) | 0x00);
send_cam((6 << 8) | 0x01);
send_cam((7 << 8) | 0x21);
/* 0x80, 0x03, 0x00, 0x30, 0x01, 0x00, 0x01, 0x21 */
/* debugging */
if ( DFLAG ) { send_uart_msg(MSG_DEFAULT) ; }
}
void start_adc(void)
{
ADCSRA |= (1 << ADSC) ;
}
void wai_adc(void)
{
while ( (ADCSRA & (1 << ADSC)) ) ;
}
void show_value(UBYTE x)
{
UBYTE tmp ;
UBYTE val[3] ;
/* store */
tmp = x ;
/* separate */
*(val+0) = tmp / 100 + '0' ; tmp %= 100 ;
*(val+1) = tmp / 10 + '0' ;
*(val+2) = tmp % 10 + '0' ;
/* show */
uart_putchar( *(val+0) );
uart_putchar( *(val+1) );
uart_putchar( *(val+2) );
/* send space */
uart_putchar( ' ' );
}
void send_uart_msg(UBYTE x)
{
char msg[20] ;
/* set message */
switch (x) {
case MSG_HELP : strcpy_P(msg,msg_h) ; break ;
case MSG_RESET : strcpy_P(msg,msg_c) ; break ;
case MSG_REGISTER : strcpy_P(msg,msg_r) ; break ;
case MSG_IMAGE : strcpy_P(msg,msg_g) ; break ;
case MSG_DEFAULT : strcpy_P(msg,msg_s) ; break ;
case MSG_DEBUG : strcpy_P(msg,msg_d) ; break ;
case MSG_PULSE : strcpy_P(msg,msg_p) ; break ;
case MSG_ST : strcpy_P(msg,msg_st); break ;
case MSG_GI : strcpy_P(msg,msg_gi); break ;
case MSG_CP : strcpy_P(msg,msg_cp); break ;
case MSG_DUTY : strcpy_P(msg,msg_sp); break ;
}
/* send */
uart_puts((UBYTE *)msg );
}
void update_motor(PARAMP x)
{
/* front */
*(direction+0) = x.direction ;
*(duty+0) = x.rlduty ;
if ( *(direction+0) == DIR_CENTER ) { *(duty+0) = 0 ; }
/* rear */
*(direction+1) = DIR_LEFT ;
*(duty+1) = x.duty ;
}
void get_sensor(void)
{
UWORD result ;
UBYTE index ;
UBYTE res ;
UBYTE i ;
UBYTE sensor ;
UBYTE avr ;
UBYTE dh ;
UBYTE dl ;
/* send start */
send_start();
/* READ data and show*/
img_cnt = 0 ;
index = 0 ;
i = 0 ;
while ( img_cnt < MAX_SIZE ) {
/* impress XCK:H */
set_xck(ON);
/* impress XCK:L */
set_xck(OFF) ;
/* judge */
if ( PINC & 2 ) {
/* start conversion */
start_adc() ;
/* wait */
wai_adc() ;
/* get data */
dl = ADCL ;
dh = ADCH ;
result = (dh << 8) + dl ;
/* shift */
result >>= 2 ;
/* judge */
if ( result > threshold ) { res++ ; }
/* generate line code */
if ( (img_cnt % 16) == 15 ) {
*(rdat+i) = res ;
res = 0 ;
i++ ;
i %= 8 ;
}
/* generate sensor data */
if ( (img_cnt % 128) == 127 ) {
/* average */
avr = 0 ;
avr += *(rdat+0) ; avr += *(rdat+1) ;
avr += *(rdat+2) ; avr += *(rdat+3) ;
avr += *(rdat+4) ; avr += *(rdat+5) ;
avr += *(rdat+6) ; avr += *(rdat+7) ;
avr >>= 3 ;
/* compare */
sensor = 0 ;
if ( *(rdat+0) > avr ) { sensor |= 0x80 ; }
if ( *(rdat+1) > avr ) { sensor |= 0x40 ; }
if ( *(rdat+2) > avr ) { sensor |= 0x20 ; }
if ( *(rdat+3) > avr ) { sensor |= 0x10 ; }
if ( *(rdat+4) > avr ) { sensor |= 0x08 ; }
if ( *(rdat+5) > avr ) { sensor |= 0x04 ; }
if ( *(rdat+6) > avr ) { sensor |= 0x02 ; }
if ( *(rdat+7) > avr ) { sensor |= 0x01 ; }
/* store */
*(sdat+index) = sensor ;
/* increment */
index++ ;
}
/* increment */
img_cnt++ ;
}
}
}
UBYTE is_center(UWORD x)
{
if ( x == 8127 || x == 8128 ) return ON ; /* 128*63+63 128*63+64 */
if ( x == 8255 || x == 8256 ) return ON ; /* 128*64+63 128*64+64 */
return OFF ;
}
UBYTE is_edge(UWORD x)
{
if ( x == 8064 || x == 8191 ) return ON ; /* 128*63 128*63+127 */
if ( x == 8192 || x == 8319 ) return ON ; /* 128*64 128*64+127 */
return OFF ;
}
/* system call */
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
tcb[tid].tsk = tsk;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
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)
{
UWORD tmp ;
tmp = (1 << tid);
ready |= tmp;
suspend &= ~tmp;
waitq &= ~tmp;
}
void sus_tsk(UBYTE tid)
{
UWORD tmp ;
tmp = (1 << tid);
ready &= ~tmp;
suspend |= tmp;
waitq &= ~tmp;
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
UWORD tmp ;
tmp = (1 << run_tsk);
ready &= ~tmp;
suspend &= ~tmp;
waitq |= tmp;
tcb[run_tsk].wcount = x ;
}
void init_os(void)
{
ready = 0;
suspend = 0;
waitq = 0;
}
UBYTE is_tsk_ready(UBYTE tid)
{
return( (ready >> tid) & 1 ) ;
}
void tsk0_proc(void)
{
UBYTE cmd ;
UBYTE chx ;
UBYTE dir ;
UBYTE val[2] ;
UBYTE valx ;
/* check */
if ( UFLAG == OFF ) { return ; }
/* clear flag */
UFLAG = OFF ;
/* interpreter */
/* get command character */
cmd = *(sbuf+0) ;
/* reset camera */
if ( cmd == 'C' ) { reset_cam() ; }
/* set register value */
if ( cmd == 'R' ) {
/* get channel */
chx = get_hex( *(sbuf+3) );
/* get code */
*(val+0) = get_hex( *(sbuf+1) );
*(val+1) = get_hex( *(sbuf+2) );
/* send this value to camera */
valx = (val[0] << 4)+ val[1] ;
send_cam( (chx << 8) | valx );
}
/* set default value */
if ( cmd == 'S' ) { send_default() ; }
/* set debug flag */
if ( cmd == 'D' ) {
DFLAG = OFF ;
if ( *(sbuf+1) == '1' ) { DFLAG = ON ; }
}
/* get picture */
if ( cmd == 'G' ) { crlf(); get_img(0) ; }
/* set duty */
if ( cmd == 'P' ) {
/* get channel */
chx = get_hex( *(sbuf+4) );
/* get direction */
dir = get_hex( *(sbuf+1) );
/* get code */
*(val+0) = get_hex( *(sbuf+2) );
*(val+1) = get_hex( *(sbuf+3) );
/* calculate */
valx = 10 * val[0] + val[1] ;
/* store */
if ( chx == 2 ) {
led_duty = valx ;
} else {
*(direction+chx) = dir ;
*(duty+chx) = valx ;
}
}
/* show duty */
if ( cmd == 'p' ) {
for ( chx = 0 ; chx < 2 ; chx++ ) {
/* direction */
uart_putchar( *(direction+chx) + '0' );
uart_putchar( ' ' );
/* duty */
*(val+0) = *(duty+chx) / 10 ;
*(val+1) = *(duty+chx) % 10 ;
uart_putchar( *(val+0) + '0' );
uart_putchar( *(val+1) + '0' );
crlf();
}
/* LED duty */
*(val+0) = led_duty / 10 ;
*(val+1) = led_duty % 10 ;
uart_putchar( *(val+0) + '0' );
uart_putchar( *(val+1) + '0' );
crlf();
}
/* help */
if ( cmd == '?' ) {
send_uart_msg(MSG_HELP);
send_uart_msg(MSG_RESET);
send_uart_msg(MSG_REGISTER);
send_uart_msg(MSG_IMAGE);
send_uart_msg(MSG_DEFAULT);
send_uart_msg(MSG_DEBUG);
send_uart_msg(MSG_PULSE);
send_uart_msg(MSG_DUTY);
}
/* new line */
crlf();
}
void tsk1_proc(void)
{
/* judge */
if ( AFLAG == OFF ) { return ; }
/* clear flag */
AFLAG = OFF ;
/* increment */
bstate++ ;
/* change */
if ( bstate & 1 ) {
rsm_tsk(BLINDRUN);
led_duty = 99 ;
} else {
sus_tsk(BLINDRUN);
sus_tsk(MOVE);
crank_move(0,0,0);
led_duty = 10 ;
}
}
void tsk2_proc(void)
{
/* reset camera */
reset_cam();
/* send paramaters */
send_default();
/* get threshold */
get_img(1);
/* slow */
straight_move(10,1000);
/* mid */
straight_move(20,1000);
/* top */
straight_move(30,1000);
/* wake up MOVE task */
rsm_tsk(MOVE);
/* exit */
slp_tsk();
}
void tsk3_proc(void)
{
UBYTE sensor_data ;
/* get sensor data */
sensor_data = convert_code(*(sdat+63));
/* state machine */
switch ( state ) {
case 0 : /* judge */
state = 0 ;
if ( sensor_data == ALL_WHITE ) { state = 100 ; }
if ( sensor_data == LEFT_WHITE ) { state = 200 ; lane = LANE_LEFT ; }
if ( sensor_data == RIGHT_WHITE ) { state = 200 ; lane = LANE_RIGHT; }
if ( sensor_data == CENTER ) { state = 10 ; }
if ( sensor_data == LITTLE_LEFT ) { state = 20 ; }
if ( sensor_data == LEFT ) { state = 30 ; }
if ( sensor_data == BIG_LEFT ) { state = 40 ; }
if ( sensor_data == LITTLE_RIGHT ) { state = 50 ; }
if ( sensor_data == RIGHT ) { state = 60 ; }
if ( sensor_data == BIG_RIGHT ) { state = 70 ; }
break ;
/* CENTER */
case 10 : state = 80 ; straight_move(20,0); break ;
/* LITTLE_LEFT (move center) */
case 20 : state = 80 ; right_move( 5 ); break ;
/* LEFT (move center) */
case 30 : state = 80 ; right_move( 10); break ;
/* BIG_LEFT (move center) */
case 40 : state = 80 ; right_move( 15); break ;
/* LITTLE_RIGHT (move center) */
case 50 : state = 80 ; left_move( 5 ); break ;
/* RIGHT (move center) */
case 60 : state = 80 ; left_move( 10); break ;
/* BIG_RIGHT (move center) */
case 70 : state = 80 ; left_move( 15); break ;
/* delay */
case 80 : state = 90 ; delay_ms(1000) ; break ;
/* return */
case 90 : state = 0 ; straight_move(20,0) ; break ;
/* CRANK */
/* crank slow move */
case 100 :
if ( sensor_data == CRANK_RIGHT ) { state = 110 ; dir = DIR_RIGHT ; }
if ( sensor_data == CRANK_LEFT ) { state = 110 ; dir = DIR_LEFT ; }
straight_move(10,1000);
break ;
/* crank rotate */
case 110 : state = 120 ; crank_move(dir,10,5); delay_ms(3000); break ;
/* crank rotate right (bit by bit) */
case 120 :
if ( is_crank_center(sensor_data) == ON ) {
state = 130 ;
} else {
crank_move(dir,5,5);
delay_ms(100);
}
break ;
/* crank slow move */
case 130 : state = 140 ; straight_move(10,1000); break ;
/* return */
case 140 : state = 0 ; straight_move(20,0); break ;
/* LANE */
/* lane slow move */
case 200 :
if ( sensor_data == ALL_BLACK ) {
state = 210 ;
dir = DIR_RIGHT ;
if ( lane == LANE_LEFT ) { dir = DIR_LEFT ; }
} else {
straight_move(10,1000);
}
break ;
/* lane change */
case 210 : state = 220 ; lane_move(dir,10,5); delay_ms(1000); break ;
/* lane moving */
case 220 :
if ( is_lane_edge(sensor_data) == ON ) {
state = 230 ;
} else {
straight_move(10,1000);
}
break ;
/* lane change completed */
case 230 :
state = 240 ;
if ( dir == DIR_RIGHT ) { dir = DIR_LEFT ; }
else { dir = DIR_RIGHT ; }
lane_move(dir,10,5);
delay_ms(1000);
break ;
/* lane slow move */
case 240 : state = 250 ; straight_move(10,1000); break ;
/* return */
case 250 : state = 0 ; straight_move(20,0); break ;
}
/* */
get_sensor() ;
}
void straight_move(UBYTE x,UWORD tx)
{
crank_move(DIR_CENTER,0,x);
if ( tx ) { delay_ms(tx) ; }
}
void right_move(UBYTE x)
{
crank_move(DIR_RIGHT,x,10);
}
void left_move(UBYTE x)
{
crank_move(DIR_LEFT,x,10);
}
void crank_move(UBYTE xa,UBYTE xb,UBYTE xc)
{
param.direction = xa;
param.rlduty = xb;
param.duty = xc;
update_motor( param );
}
void lane_move(UBYTE xa,UBYTE xb,UBYTE xc)
{
crank_move(xa,xb,xc);
}
UBYTE is_crank_center(UBYTE x)
{
UBYTE result ;
/* judge */
switch ( x ) {
case CENTER :
case LITTLE_LEFT :
case LITTLE_RIGHT : result = ON ; break ;
default : result = OFF; break ;
}
return result ;
}
UBYTE is_lane_edge(UBYTE x)
{
UBYTE result ;
/* judge */
switch ( x ) {
case CENTER :
case LITTLE_LEFT :
case LEFT :
case BIG_LEFT :
case LITTLE_RIGHT :
case RIGHT :
case BIG_RIGHT : result = ON ; break ;
default : result = OFF; break ;
}
return result ;
}
UBYTE convert_code(UBYTE x)
{
UBYTE result ;
/* default */
result = ILLEAGAL ;
/* judge */
if ( x == 0x00 ) { result = ALL_BLACK ; }
if ( x == 0xff ) { result = ALL_WHITE ; }
if ( x == 0x38 || x == 0x18 || x == 0x1c ) { result = CENTER ; }
if ( x == 0xf0 || x == 0xf8 ) { result = LEFT_WHITE ; }
if ( x == 0x0f || x == 0x1f ) { result = RIGHT_WHITE ; }
if ( x == 0x10 || x == 0x30 ) { result = LITTLE_LEFT ; }
if ( x == 0x60 || x == 0x70 ) { result = LEFT ; }
if ( x == 0xc0 || x == 0xe0 ) { result = BIG_LEFT ; }
if ( x == 0x08 || x == 0x0c ) { result = LITTLE_RIGHT ; }
if ( x == 0x06 || x == 0x0e ) { result = RIGHT ; }
if ( x == 0x03 || x == 0x07 ) { result = BIG_RIGHT ; }
if ( 100 <= state && state < 200 ) {
if ( x == 0x0e ||
x == 0x0f ||
x == 0x1f ||
x == 0x3f ) {
result = CRANK_RIGHT ;
}
if ( x == 0xe0 ||
x == 0xf0 ||
x == 0xf8 ||
x == 0xfc ) {
result = CRANK_LEFT ;
}
}
return result ;
}
テストボードの回路は、以下です。
目次
前
次
