目次
前
次
走行リアルタイムデバッグ
マシンを走らせてみると、どのステートを実行しているのか
を知りたくなりました。
ＬＣＤを接続してみても、マシンと一緒にコースを走る羽目に
なるので、この方法はパスです。
少し考えて、２つの方法を検討しました。
- VTRカメラで走行を撮影するとしてLEDの点滅パターンで表示
- 音でどの状態かを知らせる
音であれば、走行しながら状態を判定できます。
ドプラー効果で音が変わるかも知れないのですが、自分が使っている
コースの走行面積から、それほど神経質になることはないとしました。
音を出すために、ATmega328の内蔵タイマーをひとつ遊んでいたので
音出し専用にします。
実際に音を出せるかを、デバッガでテストしました。
デバッガの中に、コマンドを２つ用意します。
これらのコマンドを用意した後、可能な限り簡単に音が出せるよう
各タイマーの使い方を見直しました。
見直し後、３つのタイマーを次のように割当てます。
- タイマー０ 出力音周波数設定
- タイマー１ システム起動からの経過時間カウント
- タイマー２ PWM処理
タイマー０で、出力音の周波数を指定したので、PD6を
スピーカに接続します。
出力音の周波数を変更するため、新たに関数を定義しました。
void update_frq(UBYTE x)
{
/* judge */
if ( x == 0 ) return ;
/* disable timer0 */
TCCR0B = 0 ;
/* update */
OCR0A = x ;
/* enable timer0 */
TCCR0B = (5 << CS00) ;
}
やっていることは単純で、次のシーケンスとしています。
人間の可聴域である２０Ｈｚ〜２０ｋＨｚになるよう、初期化
しておきます。
16MHzを1024分周し、約16kHzでタイマ−のカウンタを動かします。
この周波数を分周するので、必ず可聴域の音が出ます。
走行中に、出力周波数を変更するため、走行処理のステートマシン
の中で関数update_frqを呼び出し、分周値を設定します。
音を出すようにしたソースコードは、以下です。
#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 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 b1_cnt[16] ;
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_f[] PROGMEM = "F set sound value" ;
const prog_char msg_sf[] PROGMEM = "f show sound value" ;
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 MSG_FRQ 11
#define MSG_FRQS 12
#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 make_code(void);
UBYTE get_1_cnt(UBYTE x);
UBYTE get_1_count(UWORD x);
void get_sensor(void);
UBYTE is_center(UWORD x);
UBYTE is_edge(UWORD x);
void update_frq(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 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 ;
/* 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 ; }
}
/* 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 ; /* ooooiioi */
/* 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 */
{
/* use OC0A , select CTC */
TCCR0A = (1 << COM0A0) | (1 << WGM01) ;
/* (presclae /1024) about 16kHz */
TCCR0B = (5 << CS00) ;
/* clear timer/counter */
TCNT0 = 0 ;
OCR0A = 25 ;
OCR0B = 250 ;
}
/* 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 timer2 */
{
/* select CTC */
TCCR2A = (1 << WGM21) ;
/* (presclae /64) 250kHz */
TCCR2B = (1 << CS22) ;
/* clear timer/counter */
TCNT2 = 0 ;
OCR2A = 125 ;
OCR2B = 250 ;
/* Enable Compare match interruption */
TIMSK2 = (1 << OCIE2A) ;
}
/* 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 ;
/* make table */
make_code();
}
/* 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 ;
}
}
/* TIMER1 interrupt */
ISR(TIMER1_COMPA_vect)
{
/* increment */
tickcount++ ;
/* increment */
mcount++ ;
/* judge */
if ( mcount == 10 ) {
mcount = 0 ;
}
}
/* TIMER2 interrupt */
ISR(TIMER2_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);
}
}
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);
PORTD &= ~(1 << LOAD) ;
set_sin(OFF) ;
/* 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 ) {
/* calculate and store */
threshold = (UBYTE)((max+min) >> 3);
}
}
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);
/* 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 ;
case MSG_FRQ : strcpy_P(msg,msg_f) ; break ;
case MSG_FRQS : strcpy_P(msg,msg_sf); 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 make_code(void)
{
UBYTE i ;
UBYTE cnt ;
for ( i = 0 ; i < 16 ; i++ ) {
cnt = 0 ;
if ( i & 1 ) { cnt++ ; }
if ( i & 2 ) { cnt++ ; }
if ( i & 4 ) { cnt++ ; }
if ( i & 8 ) { cnt++ ; }
*(b1_cnt+i) = cnt ;
}
}
UBYTE get_1_cnt(UBYTE x)
{
UBYTE bh;
UBYTE bl;
UBYTE result ;
/* separate */
bl = x & 15 ; x >>= 4 ;
bh = x & 15 ;
/* sum */
result = 0 ;
result += b1_cnt[bh] ;
result += b1_cnt[bl] ;
return result ;
}
UBYTE get_1_count(UWORD x)
{
UBYTE bh;
UBYTE bl;
UBYTE result ;
/* separate */
bl = x & 255 ; x >>= 8 ;
bh = x & 255 ;
/* sum */
result = 0 ;
result += get_1_cnt(bl) ;
result += get_1_cnt(bh) ;
return result ;
}
void get_sensor(void)
{
UWORD result ;
UBYTE index ;
UWORD res ;
UBYTE i ;
UBYTE sensor ;
UBYTE avr ;
UBYTE dh ;
UBYTE dl ;
UBYTE max;
UBYTE min;
/* send start */
send_start();
/* READ data and show*/
img_cnt = 0 ;
index = 0 ;
i = 0 ;
res = 0 ;
avr = 0 ;
max = 0 ;
min = 16 ;
dl = 0 ;
dl = 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 */
res <<= 1 ;
if ( result > threshold ) { res |= 1 ; }
/* generate line code */
if ( (img_cnt % 16) == 15 ) {
*(rdat+i) = get_1_count(res) ;
/* judge */
if ( max < *(rdat+i) ) { max = *(rdat+i); }
if ( min > *(rdat+i) ) { min = *(rdat+i); }
/* clear */
res = 0 ;
/* update pointer */
i++ ;
i %= 8 ;
}
/* generate sensor data */
if ( (img_cnt % 128) == 127 ) {
/* calculate threshold */
avr = ((max+min) >> 2) ;
if ( avr < min ) { avr = min ; }
/* 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++ ;
/* initialize max and min */
max = 0 ;
min = 16 ;
}
/* 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 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 */
show_value( *(duty+chx) ) ;
crlf();
}
/* LED duty */
show_value( led_duty ) ;
crlf();
}
/* set frquency divider */
if ( cmd == 'F' ) {
/* get value */
valx = get_hex( *(sbuf+1) );
valx = valx * 10 + get_hex( *(sbuf+2) );
valx = valx * 10 + get_hex( *(sbuf+3) );
/* update */
update_frq( valx );
crlf();
}
/* show frquency divider */
if ( cmd == 'f' ) {
/* get value */
valx = OCR0A ;
/* show */
show_value( valx );
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);
send_uart_msg(MSG_FRQ);
send_uart_msg(MSG_FRQS);
}
/* 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);
/* sound straight */
update_frq(100) ;
/* 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); update_frq(100) ; break ;
/* LITTLE_LEFT (move center) */
case 20 : state = 80 ; right_move( 5 ); update_frq(102) ; break ;
/* LEFT (move center) */
case 30 : state = 80 ; right_move( 10); update_frq(104) ; break ;
/* BIG_LEFT (move center) */
case 40 : state = 80 ; right_move( 15); update_frq(106) ;break ;
/* LITTLE_RIGHT (move center) */
case 50 : state = 80 ; left_move( 5 ); update_frq(108) ; break ;
/* RIGHT (move center) */
case 60 : state = 80 ; left_move( 10); update_frq(110) ; break ;
/* BIG_RIGHT (move center) */
case 70 : state = 80 ; left_move( 15); update_frq(112) ;break ;
/* delay */
case 80 : state = 90 ; delay_ms(1000) ; break ;
/* return */
case 90 : state = 0 ; straight_move(20,0) ; update_frq(100) ; 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);
update_frq(50) ;
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);
update_frq(52) ;
delay_ms(100);
}
break ;
/* crank slow move */
case 130 : state = 140 ; straight_move(10,1000); update_frq(54) ; break ;
/* return */
case 140 : state = 0 ; straight_move(20,0); update_frq(100) ; 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);
update_frq(30) ;
}
break ;
/* lane change */
case 210 : state = 220 ; lane_move(dir,10,5); update_frq(32) ; delay_ms(1000); break ;
/* lane moving */
case 220 :
if ( is_lane_edge(sensor_data) == ON ) {
state = 230 ;
} else {
straight_move(10,1000);
update_frq(34) ;
}
break ;
/* lane change completed */
case 230 :
state = 240 ;
if ( dir == DIR_RIGHT ) { dir = DIR_LEFT ; }
else { dir = DIR_RIGHT ; }
lane_move(dir,10,5);
update_frq(36) ;
delay_ms(1000);
break ;
/* lane slow move */
case 240 : state = 250 ; straight_move(10,1000); update_frq(38) ; break ;
/* return */
case 250 : state = 0 ; straight_move(20,0); update_frq(100) ;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 ;
}
void update_frq(UBYTE x)
{
/* judge */
if ( x == 0 ) return ;
/* disable timer0 */
TCCR0B = 0 ;
/* update */
OCR0A = x ;
/* enable timer0 */
TCCR0B = (5 << CS00) ;
}
目次
前
次
