目次
前
次
ファームウエアダイエット
CUIによるテスト、デバッグが終了したので、実走行に移ります。
実際に走行するときには、ハードウエアの動作は、すべて
関数で記述してあることから、走行に関連する内容だけを
取り出してまとめます。
これまで使ってきたファームウエアをハードウエアテストと実走行の
２つに分割して、それぞれをダイエットします。
ハードウエアテスト
パーソナルコンピュータとMCR-VCマシンを、シリアルで
接続して、コマンドによりハードウエアをテストします。
音を出してデバッグしようとすると、周囲の迷惑になるので
２個のLEDを接続して、NORMAL、CRANK、LANEのどの状態かを
光の組み合わせでわかるようにしました。
PD7、PD6に赤LEDを接続し、次の組合せで表現します。
- (PD7、PD6)=(0,0) 点灯 点灯 → LANE
- (PD7、PD6)=(1,0) 消灯 点灯 → NORMAL
- (PD7、PD6)=(0,1) 点灯 消灯 → CRANK
- (PD7、PD6)=(1,1) 消灯 消灯 → 上記以外
ソースコードは、以下です。
#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 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 BCNT00 0
#define BCNT01 1
#define BCNT02 1
#define BCNT03 2
#define BCNT04 1
#define BCNT05 2
#define BCNT06 2
#define BCNT07 3
#define BCNT08 1
#define BCNT09 2
#define BCNT10 2
#define BCNT11 3
#define BCNT12 2
#define BCNT13 3
#define BCNT14 3
#define BCNT15 4
#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
#define MODE_NORMAL 0
#define MODE_CRANK 1
#define MODE_LANE 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 mode ;
volatile UBYTE action ;
volatile UBYTE dir ;
volatile UBYTE lane ;
volatile UBYTE threshold ;
volatile UBYTE sensor_data ;
volatile UBYTE b1_cnt[16] ;
volatile UBYTE rdat[8] ;
volatile UBYTE rdatx[8] ;
volatile UBYTE sdat[128] ;
volatile UWORD img_cnt ;
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_l[] PROGMEM = "L test LED" ;
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 MSG_LED 13
#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
/*--------------------------------*/
/* 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);
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);
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 convert_code(UBYTE x);
/*------*/
/* main */
/*------*/
int main(void)
{
UBYTE cmd ;
UBYTE chx ;
UBYTE dir ;
UBYTE val[2] ;
UBYTE valx ;
/* disable interrupt */
cli();
/* initialize port and variables */
user_initialize();
/* enable interrupt */
sei();
/* endless loop */
first_message();
while ( ON ) {
/* check */
if ( UFLAG == OFF ) continue ;
/* 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();
}
/* LED test */
if ( cmd == 'L' ) {
/* get first bit value */
valx = get_hex( *(sbuf+1) );
valx <<= 1 ;
/* add next bit value */
valx |= get_hex( *(sbuf+2) );
/* default */
PORTD |= 0xc0 ;
/* impress */
if ( valx == 1 ) { PORTD |= (1 << 7) ; PORTD &= ~(1 << 6) ; }
if ( valx == 2 ) { PORTD &= ~(1 << 7) ; PORTD |= (1 << 6) ; }
if ( valx == 3 ) { PORTD &= ~(1 << 7) ; PORTD &= ~(1 << 6) ; }
}
/* 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_LED);
}
/* new line */
crlf();
}
/* 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 = 0b11001100 ; /* 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 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 */
{
/* select falling edge */
EICRA = (1 << ISC01) ;
/* Enable external interruption */
EIMSK = (1 << INT0) ;
}
/* duty ratio */
pcnt = 0 ;
action = 0 ;
*(duty+0) = 0 ;
*(duty+1) = 0 ;
*(dutyx+0) = 0 ;
*(dutyx+1) = 0 ;
*(direction+0) = 0 ;
*(direction+1) = DIR_LEFT ;
led_duty = 10 ;
/* default */
mode = MODE_NORMAL ;
dir = DIR_CENTER ;
lane = LANE_NONE ;
/* make table */
*(b1_cnt+ 0) = BCNT00 ; *(b1_cnt+ 1) = BCNT01 ;
*(b1_cnt+ 2) = BCNT02 ; *(b1_cnt+ 3) = BCNT03 ;
*(b1_cnt+ 4) = BCNT04 ; *(b1_cnt+ 5) = BCNT05 ;
*(b1_cnt+ 6) = BCNT06 ; *(b1_cnt+ 7) = BCNT07 ;
*(b1_cnt+ 8) = BCNT08 ; *(b1_cnt+ 9) = BCNT09 ;
*(b1_cnt+10) = BCNT10 ; *(b1_cnt+11) = BCNT11 ;
*(b1_cnt+12) = BCNT12 ; *(b1_cnt+13) = BCNT13 ;
*(b1_cnt+14) = BCNT14 ; *(b1_cnt+15) = BCNT15 ;
}
/* 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) ;
/* impress LOAD:L */
PORTC &= ~(1 << LOAD);
/* send */
for ( i = 0 ; i < 11 ; i++ ) {
/* impress LOAD bit */
if ( i == 10 ) { PORTC |= (1 << LOAD); }
/* 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) ;
}
/* default */
PORTC &= ~(1 << LOAD);
set_sin(OFF);
}
void send_start(void)
{
/* impress XCK:L */
set_xck(OFF) ;
/* impress STARST:H */
PORTD |= (1 << START);
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_LED : strcpy_P(msg,msg_l) ; break ;
default : *(msg+0) = ' ' ; *(msg+1) = '\0' ; 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 ;
}
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;
UBYTE max_x;
UBYTE min_x;
UBYTE exeflag ;
/* send start */
send_start();
/* READ data and show*/
img_cnt = 0 ;
index = 0 ;
i = 0 ;
res = 0 ;
avr = 0 ;
max = 0 ;
min = 16 ;
max_x = 0 ;
min_x = 16 ;
dl = 0 ;
dl = 0 ;
exeflag = OFF ;
while ( img_cnt < MAX_SIZE ) {
/* impress XCK:H */
set_xck(ON);
/* generate sensor data */
if ( exeflag == ON ) {
/* clear trigger */
exeflag = OFF ;
/* calculate threshold */
avr = ((max_x+min_x) >> 2) ;
if ( avr < min_x ) { avr = min_x ; }
/* compare */
sensor = 0 ;
if ( *(rdatx+0) > avr ) { sensor |= 0x80 ; }
if ( *(rdatx+1) > avr ) { sensor |= 0x40 ; }
if ( *(rdatx+2) > avr ) { sensor |= 0x20 ; }
if ( *(rdatx+3) > avr ) { sensor |= 0x10 ; }
if ( *(rdatx+4) > avr ) { sensor |= 0x08 ; }
if ( *(rdatx+5) > avr ) { sensor |= 0x04 ; }
if ( *(rdatx+6) > avr ) { sensor |= 0x02 ; }
if ( *(rdatx+7) > avr ) { sensor |= 0x01 ; }
/* store */
*(sdat+index) = sensor ;
/* increment */
index++ ;
}
/* 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 ;
/* transfer */
if ( i == 0 ) {
/* transfer */
max_x = max ;
min_x = min ;
*(rdatx+0) = *(rdat+0) ;
*(rdatx+1) = *(rdat+1) ;
*(rdatx+2) = *(rdat+2) ;
*(rdatx+3) = *(rdat+3) ;
*(rdatx+4) = *(rdat+4) ;
*(rdatx+5) = *(rdat+5) ;
*(rdatx+6) = *(rdat+6) ;
*(rdatx+7) = *(rdat+7) ;
/* initialize max and min */
max = 0 ;
min = 16 ;
}
}
/* trigger */
if ( (img_cnt % 128) == 127 ) { exeflag = ON ; }
/* 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 ;
}
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 ( 99 < action && action < 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) ;
}
NORMAL状態のモータに与えるDUTY比は、マイコンとPCを
接続して、実際に動かしながら決めました。
この作業に、本ファームウエアを利用しています。
実走行ファームウエア
ハードウエアテストを別のファームウエアとしたので
タスクは、４つにしました。
各タスクの役割は、以下です。
- タスク０ スタートトリガー、ストップトリガー待ち
- タスク１ BLIND RUN
- タスク２ センサーデータ入力と動作確定
- タスク３ モータ制御
各タスクの初期状態は、次のように決めます。
- タスク０ READY
- タスク１ SUSPEND
- タスク２ SUSPEND
- タスク３ SUSPEND
各関数の内容は、冗長な記述をしないようにしてあります。
#include <avr/io.h>
#include <avr/interrupt.h>
#define OFF 0
#define ON OFF+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 CFLAG x_flags.BIT.B0
#define AFLAG x_flags.BIT.B1
#define DFLAG x_flags.BIT.B7
#define BCNT00 0
#define BCNT01 1
#define BCNT02 1
#define BCNT03 2
#define BCNT04 1
#define BCNT05 2
#define BCNT06 2
#define BCNT07 3
#define BCNT08 1
#define BCNT09 2
#define BCNT10 2
#define BCNT11 3
#define BCNT12 2
#define BCNT13 3
#define BCNT14 3
#define BCNT15 4
#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
#define MODE_NORMAL 0
#define MODE_CRANK 1
#define MODE_LANE 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 mode ;
volatile UBYTE action ;
volatile UBYTE dir ;
volatile UBYTE lane ;
volatile UBYTE threshold ;
volatile UBYTE sensor_data ;
volatile UBYTE b1_cnt[16] ;
volatile UBYTE rdat[8] ;
volatile UBYTE rdatx[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 ;
#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 TRIGGER TSK_ID0
#define BLINDRUN TSK_ID1
#define SJUDGE TSK_ID2
#define MOVE TSK_ID3
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
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 update_motor(PARAMP x);
void get_threshold(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);
/* 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);
void set_mode_led(UBYTE x);
/*------*/
/* main */
/*------*/
int main(void)
{
TCBP pcur_tsk ;
/* initialize port and variables */
user_initialize();
/* initialize monitor */
init_os();
/* create task */
cre_tsk(TRIGGER ,tsk0_proc);
cre_tsk(BLINDRUN,tsk1_proc);
cre_tsk(SJUDGE ,tsk2_proc);
cre_tsk(MOVE ,tsk3_proc);
/* start task task */
sta_tsk(TRIGGER ,TTS_READY);
sta_tsk(BLINDRUN,TTS_SUSPEND);
sta_tsk(SJUDGE ,TTS_SUSPEND);
sta_tsk(MOVE ,TTS_SUSPEND);
/* enable interrupt */
sei();
/* endless loop */
run_tsk = TRIGGER ;
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 = TRIGGER ; }
}
/* dummy */
return 0 ;
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
/* PORT B */
PORTB = 0b00000000 ; /* 00000000 */
DDRB = 0b00111111 ; /* iioooooo */
/* PORT C */
PORTC = 0b00001000 ; /* 00001000 */
DDRC = 0b11111100 ; /* ooooooii */
/* PORT D */
PORTD = 0b11001100 ; /* 11001100 */
DDRD = 0b11110010 ; /* ooooiioi */
/* initialize flags */
x_flags.DR = 0 ;
/* initialize ADC */
{
/* external 2.56V , select channel 0 */
ADMUX = 0 ;
/* enable ADC , enable interrupt , select prescaler (/128) */
ADCSRA = (1 << ADEN) | (7 << ADPS0) ;
}
/* 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 ;
action = 0 ;
*(duty+0) = 0 ;
*(duty+1) = 0 ;
*(dutyx+0) = 0 ;
*(dutyx+1) = 0 ;
*(direction+0) = 0 ;
*(direction+1) = DIR_LEFT ;
led_duty = 10 ;
/* default */
mode = MODE_NORMAL ;
dir = DIR_CENTER ;
lane = LANE_NONE ;
/* make table */
*(b1_cnt+ 0) = BCNT00 ; *(b1_cnt+ 1) = BCNT01 ;
*(b1_cnt+ 2) = BCNT02 ; *(b1_cnt+ 3) = BCNT03 ;
*(b1_cnt+ 4) = BCNT04 ; *(b1_cnt+ 5) = BCNT05 ;
*(b1_cnt+ 6) = BCNT06 ; *(b1_cnt+ 7) = BCNT07 ;
*(b1_cnt+ 8) = BCNT08 ; *(b1_cnt+ 9) = BCNT09 ;
*(b1_cnt+10) = BCNT10 ; *(b1_cnt+11) = BCNT11 ;
*(b1_cnt+12) = BCNT12 ; *(b1_cnt+13) = BCNT13 ;
*(b1_cnt+14) = BCNT14 ; *(b1_cnt+15) = BCNT15 ;
}
/* INT0 interrupt */
ISR(INT0_vect)
{
AFLAG = ON ;
}
/* 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);
}
}
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) ;
}
void send_cam(UWORD x)
{
UBYTE i ;
UWORD tmp ;
tmp = x ;
/* impress XCK:L */
set_xck(OFF) ;
/* impress LOAD:L */
PORTC &= ~(1 << LOAD);
/* send */
for ( i = 0 ; i < 11 ; i++ ) {
/* impress LOAD bit */
if ( i == 10 ) { PORTC |= (1 << LOAD); }
/* 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) ;
}
/* default */
PORTC &= ~(1 << LOAD);
set_sin(OFF);
}
void send_start(void)
{
/* impress XCK:L */
set_xck(OFF) ;
/* impress STARST:H */
PORTD |= (1 << START);
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 ;
/* send start */
send_start();
/* 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 ) {
}
/* store */
if ( x == 1 ) {
if ( is_center( img_cnt ) ) { max += result ; }
if ( is_edge( img_cnt ) ) { min += result ; }
}
/* increment */
img_cnt++ ;
}
}
/* 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);
}
void start_adc(void)
{
ADCSRA |= (1 << ADSC) ;
}
void wai_adc(void)
{
while ( (ADCSRA & (1 << ADSC)) ) ;
}
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 ;
}
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;
UBYTE max_x;
UBYTE min_x;
UBYTE exeflag ;
/* send start */
send_start();
/* READ data and show*/
img_cnt = 0 ;
index = 0 ;
i = 0 ;
res = 0 ;
avr = 0 ;
max = 0 ;
min = 16 ;
max_x = 0 ;
min_x = 16 ;
dl = 0 ;
dl = 0 ;
exeflag = OFF ;
while ( img_cnt < MAX_SIZE ) {
/* impress XCK:H */
set_xck(ON);
/* generate sensor data */
if ( exeflag == ON ) {
/* clear trigger */
exeflag = OFF ;
/* calculate threshold */
avr = ((max_x+min_x) >> 2) ;
if ( avr < min_x ) { avr = min_x ; }
/* compare */
sensor = 0 ;
if ( *(rdatx+0) > avr ) { sensor |= 0x80 ; }
if ( *(rdatx+1) > avr ) { sensor |= 0x40 ; }
if ( *(rdatx+2) > avr ) { sensor |= 0x20 ; }
if ( *(rdatx+3) > avr ) { sensor |= 0x10 ; }
if ( *(rdatx+4) > avr ) { sensor |= 0x08 ; }
if ( *(rdatx+5) > avr ) { sensor |= 0x04 ; }
if ( *(rdatx+6) > avr ) { sensor |= 0x02 ; }
if ( *(rdatx+7) > avr ) { sensor |= 0x01 ; }
/* store */
*(sdat+index) = sensor ;
/* increment */
index++ ;
}
/* 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 ;
/* transfer */
if ( i == 0 ) {
/* transfer */
max_x = max ;
min_x = min ;
*(rdatx+0) = *(rdat+0) ;
*(rdatx+1) = *(rdat+1) ;
*(rdatx+2) = *(rdat+2) ;
*(rdatx+3) = *(rdat+3) ;
*(rdatx+4) = *(rdat+4) ;
*(rdatx+5) = *(rdat+5) ;
*(rdatx+6) = *(rdat+6) ;
*(rdatx+7) = *(rdat+7) ;
/* initialize max and min */
max = 0 ;
min = 16 ;
}
}
/* trigger */
if ( (img_cnt % 128) == 127 ) { exeflag = ON ; }
/* 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)
{
/* judge */
if ( AFLAG == OFF ) { return ; }
/* clear flag */
AFLAG = OFF ;
/* increment */
bstate++ ;
/* change */
if ( bstate & 1 ) {
rsm_tsk(BLINDRUN);
led_duty = 95 ;
} else {
sus_tsk(BLINDRUN);
sus_tsk(SJUDGE);
sus_tsk(MOVE);
crank_move(0,0,0);
led_duty = 10 ;
}
}
void tsk1_proc(void)
{
/* reset camera */
reset_cam();
/* send paramaters */
send_default();
/* get threshold */
get_img(1);
/* indicate mode */
PORTD &= ~0xc0 ;
/* slow */
straight_move(10,1000);
/* mid */
straight_move(20,1000);
/* top */
straight_move(30,1000);
/* indicate mode */
PORTD |= 0xc0 ;
/* wake up JUDGE and MOVE task */
rsm_tsk(SJUDGE);
rsm_tsk(MOVE);
/* exit */
slp_tsk();
}
void tsk2_proc(void)
{
/* get sensor data */
get_sensor();
/* get sensor data */
sensor_data = convert_code(*(sdat+63));
/* judge */
if ( sensor_data == ALL_WHITE ) { action = 100 ; mode = MODE_CRANK ; }
if ( sensor_data == LEFT_WHITE ) { action = 200 ; mode = MODE_LANE ; lane = LANE_LEFT ; }
if ( sensor_data == RIGHT_WHITE ) { action = 200 ; mode = MODE_LANE ; lane = LANE_RIGHT; }
if ( mode == MODE_NORMAL ) {
if ( sensor_data == CENTER ) { action = 10 ; }
if ( sensor_data == LITTLE_LEFT ) { action = 20 ; }
if ( sensor_data == LEFT ) { action = 30 ; }
if ( sensor_data == BIG_LEFT ) { action = 40 ; }
if ( sensor_data == LITTLE_RIGHT ) { action = 50 ; }
if ( sensor_data == RIGHT ) { action = 60 ; }
if ( sensor_data == BIG_RIGHT ) { action = 70 ; }
}
/* show mode */
set_mode_led( action );
}
void tsk3_proc(void)
{
switch ( action ) {
/* NORMAL */
/* CENTER */
case 10 : straight_move(20,0); delay_ms(1000); break ;
/* LITTLE_LEFT (move center) */
case 20 : right_move( 5 ); delay_ms(1000); straight_move(20,0); break ;
/* LEFT (move center) */
case 30 : right_move( 10); delay_ms(1000); straight_move(20,0); break ;
/* BIG_LEFT (move center) */
case 40 : right_move( 15); delay_ms(1000); straight_move(20,0); break ;
/* LITTLE_RIGHT (move center) */
case 50 : left_move( 5 ); delay_ms(1000); straight_move(20,0); break ;
/* RIGHT (move center) */
case 60 : left_move( 10); delay_ms(1000); straight_move(20,0); break ;
/* BIG_RIGHT (move center) */
case 70 : left_move( 15); delay_ms(1000); straight_move(20,0); break ;
/* CRANK */
/* crank slow move */
case 100 :
if ( sensor_data == CRANK_RIGHT ) { action = 110 ; dir = DIR_RIGHT ; }
else if ( sensor_data == CRANK_LEFT ) { action = 110 ; dir = DIR_LEFT ; }
else { straight_move(10,1000); }
break ;
/* crank rotate */
case 110 :
action = 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 ) { action = 130 ; }
else { crank_move(dir,5,5); delay_ms(100); }
break ;
/* crank slow move */
case 130 :
action = 140 ; straight_move(10,1000);
break ;
/* return */
case 140 :
straight_move(20,0); mode = MODE_NORMAL;
break ;
/* LANE */
/* lane slow move */
case 200 :
if ( sensor_data == ALL_BLACK ) { action = 210 ; dir = DIR_RIGHT;
if ( lane == LANE_LEFT ) { dir = DIR_LEFT ; }
} else { straight_move(10,1000); }
break ;
/* lane change */
case 210 :
action = 220 ; lane_move(dir,10,5); delay_ms(1000);
break ;
/* lane moving */
case 220 :
if ( is_lane_edge(sensor_data) == ON ) { action = 230 ; }
else { straight_move(10,1000); }
break ;
/* lane change completed */
case 230 :
action = 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 :
action = 250 ; straight_move(10,1000);
break ;
/* return */
case 250 :
straight_move(20,0); mode = MODE_NORMAL;
break ;
}
}
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 ( 99 < action && action < 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 set_mode_led(UBYTE x)
{
/* default */
PORTD |= 0x80 ;
PORTD |= 0x40 ;
/* judge */
if ( x < 100 ) { PORTD &= ~0x80 ; }
else {
if ( x < 200 ) { PORTD &= ~0x40 ; }
else { PORTD &= ~0xc0 ; }
}
}
目次
前
次
