ソースコード
実際に利用したソースコードは、以下です。
#include <3048.h>
/* I/O redefine */
#define P1DR P1.DR.BYTE
#define P1DDR P1.DDR
#define P2DR P2.DR.BYTE
#define P2DDR P2.DDR
#define P3DR P3.DR.BYTE
#define P3DDR P3.DDR
#define P4DR P4.DR.BYTE
#define P4DDR P4.DDR
#define P5DR P5.DR.BYTE
#define P5DDR P5.DDR
#define P6DR P6.DR.BYTE
#define P6DDR P6.DDR
#define P7DR P7.DR.BYTE
#define P9DR P9.DR.BYTE
#define P9DDR P9.DDR
#define PADR PA.DR.BYTE
#define PADDR PA.DDR
#define PBDR PB.DR.BYTE
#define PBDDR PB.DDR
#define START_BUTTON PB.DR.BIT.B4
#define MODE_BUTTON PB.DR.BIT.B5
#define SRAM_DAT P3.DR.BYTE
#define SRAM_WE P5.DR.BIT.B0
#define SRAM_OE P5.DR.BIT.B1
#define SRAM_CS0 P5.DR.BIT.B2
#define SRAM_CS1 P5.DR.BIT.B3
#define SRAM_CS2 P6.DR.BIT.B0
#define SRAM_CS3 P6.DR.BIT.B1
#define NORMAL 0
#define CRANK 1
#define LANE_CHANGE 2
#define OUT_OF_LANE 4
#define OFF 0
#define ON OFF+1
#define TSK_ID_MAX 5
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TSK_ID4 4
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 TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
#define NO 0
#define YES 1
UBYTE ready ;
UBYTE suspend;
UBYTE vldtsk ;
UBYTE run_tsk;
UBYTE os_cnt ;
TCBP tcb[TSK_ID_MAX];
/*------------------------*/
/* task function protoype */
/*------------------------*/
void tsk0_proc(void);
void tsk1_proc(void);
void tsk2_proc(void);
void tsk3_proc(void);
void tsk4_proc(void);
/*-----------------------*/
/* system call prototype */
/*-----------------------*/
void init_os(void);
void cre_tsk(UBYTE tid,void (*tsk)(void));
void sta_tsk(UBYTE tid,UBYTE sta);
void rsm_tsk(UBYTE tid);
void sus_tsk(UBYTE tid);
void slp_tsk(void);
void wai_tsk(UWORD x);
UBYTE is_tsk_ready(UBYTE tid);
/* global variables */
ULONG millisec_cnt ;
UBYTE sbuf[16];
UBYTE sindex ;
UBYTE pwm_cnt ;
UBYTE left_duty ;
UBYTE right_duty ;
UWORD dcount ;
typedef union {
struct {
unsigned B7:1;
unsigned B6:1;
unsigned B5:1;
unsigned B4:1;
unsigned B3:1;
unsigned B2:1;
unsigned B1:1;
unsigned B0:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
FLAGSP x_flags ;
#define RIGHT_MOTOR x_flags.BIT.B0
#define LEFT_MOTOR x_flags.BIT.B1
#define SFLAG x_flags.BIT.B2
#define EFLAG x_flags.BIT.B3
#define CUR_MODE x_flags.BIT.B4
#define PRE_MODE x_flags.BIT.B5
#define DFLAG x_flags.BIT.B7
typedef struct {
UBYTE data[128];
UBYTE capacity;
UBYTE rdp;
UBYTE wrp;
} RINGP ;
typedef struct {
UBYTE right;
UBYTE left;
UWORD wtime;
} DRIVEP ;
DRIVEP drive[32];
UBYTE mugen_mode ;
#define NORMAL_NONE 0
#define NORMAL_CENTER 1
#define NORMAL_BIT_RIGHT 2
#define NORMAL_RIGHT 3
#define NORMAL_MUCH_RIGHT 4
#define NORMAL_BIT_LEFT 5
#define NORMAL_LEFT 6
#define NORMAL_MUCH_LEFT 7
#define CRANK_CENTER 8
#define CRANK_RIGHT 9
#define CRANK_LEFT 10
#define LANE_RIGHT 11
#define LANE_CENTER 12
#define LANE_LEFT 13
#define S_NONE 0
#define S_BIT_RIGHT 1
#define S_RIGHT 2
#define S_MUCH_RIGHT 3
#define S_CENTER 4
#define S_BIT_LEFT 5
#define S_LEFT 6
#define S_MUCH_LEFT 7
#define S_ALL_WHITE 8
#define S_ALL_RIGHT 9
#define S_ALL_LEFT 10
#define S_EDGE_RIGHT 11
#define S_EDGE_LEFT 12
/* クロック16MHz時 */
//#ifdef SYS_CLOCK_16
#define ITU0_AREG 16000
#define ITU1_AREG 40
//#endif
#define XLAST 80
#define YLAST 60
#define SENTRY 0x0000
#define DENTRY 0x2000
#define LCD_DAT P4.DR.BIT.B0
#define LCD_CLK P4.DR.BIT.B1
#define LCD_RS P4.DR.BIT.B2
#define LCD_E P4.DR.BIT.B3
#define ROM_DO P4.DR.BIT.B4
#define ROM_DI P4.DR.BIT.B5
#define ROM_SK P4.DR.BIT.B6
#define ROM_CS P4.DR.BIT.B7
#define MASKFFFF 0xffff
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASK80 0x80
#define MASK7F 0x7F
#define IDLE 0
#define RUN IDLE+1
UBYTE src0[XLAST];
UBYTE src1[XLAST];
UBYTE btn_start ;
UBYTE m_state ;
UBYTE m_dir ;
UBYTE state ;
RINGP rings ;
UBYTE c328_cmd[5];
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void get_drivec(void);
void init_timer0(void);
void init_timer1(void);
void init_SCI0(TBaudRate x);
void init_SCI1(TBaudRate x);
void init_pwm(void);
void start_pwm(void);
void stop_pwm(void);
void update_motor(UBYTE dl,UBYTE dr);
UBYTE get_hex(UBYTE x);
UBYTE get_hex2(UBYTE *x);
UWORD get_hex4(UBYTE *x);
UBYTE get_dec2(UBYTE *x);
void write_epa(UBYTE adr,UWORD dat);
UWORD read_epa(UBYTE adr);
void enable_sram(UBYTE bkn,UBYTE onoff);
void put_sram(UWORD adr,UBYTE dat);
UBYTE get_sram(UWORD adr);
void verify_sram(UBYTE dat);
void sci0_putchar(UBYTE x);
void sci1_putchar(UBYTE x);
void sci1_crlf(void);
void sci1_puts(UBYTE *x);
UBYTE conv_hexdigit(UBYTE x);
ULONG millisec(void);
void delay_ms(UWORD x);
void delay_sec(UWORD x);
void send_c328_primitive(void);
UBYTE establish_c328(void);
UBYTE send_init_c328(void);
UBYTE send_snapshot_c328(void);
void send_sync_c328(void);
void send_ack_c328(void);
void put_ring(UBYTE x);
UBYTE get_ring(void);
UBYTE get_ring_cap(void);
void init_ring(void);
void init_lcd(void);
void init_display(void);
void put_lcd_primitive(UBYTE which,UBYTE x);
void put_lcd_str(UBYTE r,UBYTE c,UBYTE *ptr);
void put_lcd_clear(UBYTE r);
void xcopy(UBYTE *dptr,UBYTE *sptr);
void get_line(UWORD x,UBYTE *ptr);
UBYTE get_road(UBYTE *x);
UBYTE get_place(UBYTE x);
void show_help(void);
/*------*/
/* main */
/*------*/
int main(void)
{
TCBP pcur_tsk ;
/* disable interrupt */
DI ;
/* initialize */
user_initialize();
/* initialize monitor */
init_os();
/* create tasks */
cre_tsk(TSK_ID0,tsk0_proc);
cre_tsk(TSK_ID1,tsk1_proc);
cre_tsk(TSK_ID2,tsk2_proc);
cre_tsk(TSK_ID3,tsk3_proc);
cre_tsk(TSK_ID4,tsk4_proc);
/* initialize task states */
sta_tsk(TSK_ID0,TTS_READY );
sta_tsk(TSK_ID1,TTS_READY );
sta_tsk(TSK_ID2,TTS_SUSPEND);
sta_tsk(TSK_ID3,TTS_SUSPEND);
sta_tsk(TSK_ID4,TTS_SUSPEND);
sci1_puts((UBYTE *)"Hello");
/* enable interrupt */
EI;
/* initialize LCD */
init_lcd();
init_display();
/* dispatch and scheduler */
run_tsk = TSK_ID0 ;
while ( ON ) {
pcur_tsk = tcb[run_tsk] ;
if ( is_tsk_ready( run_tsk ) == YES ) { (*(pcur_tsk.tsk))(); }
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
}
/* dummy */
return 0 ;
}
/*-----------------*/
/* debugging task */
/*-----------------*/
void tsk0_proc(void)
{
UBYTE eadr,cmd,tmp[10] ;
UWORD i,edat ;
/* check flag */
if ( SFLAG == OFF ) return ;
/* clear flag */
SFLAG = OFF ;
/* branch */
cmd = *(sbuf+0) ;
/* control debug mode */
if ( cmd == 'D' ) {
DFLAG = OFF ; xcopy(tmp,(UBYTE *)"DBG:OFF");
*(tmp+0) = get_hex( *(sbuf+1) );
if ( *(tmp+0) ) { DFLAG = ON ; xcopy(tmp,(UBYTE *)"DBG:ON "); }
/* clear line */
put_lcd_clear(0);
/* indicate */
put_lcd_str(0,0,tmp);
}
/* show status */
if ( cmd == 'N' ) {
xcopy(tmp,(UBYTE *)"DBG:OFF");
if ( DFLAG ) { xcopy(tmp,(UBYTE *)"DBG:ON "); }
sci1_puts(tmp);
}
/* show help */
if ( cmd == '?' ) { show_help(); }
/* debug code */
if ( DFLAG == OFF ) return ;
/* test control bit */
if ( cmd == 'C' ) {
*(tmp+0) = *(sbuf+1) ; /* get sub command character */
/* judge */
if ( *(tmp+0) == 'W' || *(tmp+0) == 'R' ) {
/* default */
SRAM_WE = ON ; SRAM_OE = ON ;
/* get logical value */
*(tmp+1) = get_hex( *(sbuf+2) ) ;
if ( *(tmp+1) ) {
if ( *(tmp+0) == 'W' ) { SRAM_WE = OFF ; }
if ( *(tmp+0) == 'R' ) { SRAM_OE = OFF ; }
}
}
if ( *(tmp+0) == 'S' ) {
/* default */
SRAM_CS0 = ON ; SRAM_CS1 = ON ; SRAM_CS2 = ON ; SRAM_CS3 = ON ;
/* get channel number */
*(tmp+1) = get_hex( *(sbuf+2) ) ;
/* get logical value */
*(tmp+2) = get_hex( *(sbuf+3) ) ;
if ( *(tmp+2) ) {
if ( *(tmp+1) == 0 ) { SRAM_CS0 = OFF ; }
if ( *(tmp+1) == 1 ) { SRAM_CS1 = OFF ; }
if ( *(tmp+1) == 2 ) { SRAM_CS2 = OFF ; }
if ( *(tmp+1) == 3 ) { SRAM_CS3 = OFF ; }
}
}
}
/* EEPROM access */
if ( cmd == 'E' ) {
/* show data */
if ( *(sbuf+1) == 'A' ) {
for ( i = 0 ; i < 64 ; i++ ) {
/* get data */
edat = read_epa( eadr );
/* separate */
for ( i = 3 ; i >= 0 ; i-- ) {
*(tmp+i) = conv_hexdigit( edat & MASK0F ) ;
edat >>= 4 ;
}
/* show */
for ( i = 0 ; i < 4 ; i++ ) { sci1_putchar( *(tmp+i) ) ; }
sci1_putchar( ' ' ) ;
/* new line */
if ( (i % 8) == 7 ) { sci1_crlf() ; }
}
}
/* set data */
if ( *(sbuf+1) == 'S' ) {
/* get 1 byte data */
eadr = get_hex2( sbuf+2 ) ;
/* get 1 word data */
edat = get_hex4( sbuf+4 ) ;
/* store data */
write_epa( eadr , edat );
}
}
/* get image */
if ( cmd == 'G' ) { tsk2_proc(); }
/* read image data */
if ( cmd == 'R' ) {
/* get data */
eadr = get_hex4( sbuf+1 ) ;
/* get data */
edat = get_sram( eadr ) ;
/* separate */
for ( i = 3 ; i >= 0 ; i-- ) {
*(tmp+i) = conv_hexdigit( edat & MASK0F ) ;
edat >>= 4 ;
}
/* show */
for ( i = 0 ; i < 4 ; i++ ) { sci1_putchar( *(tmp+i) ) ; }
/* new line */
sci1_crlf() ;
}
/* test LCD */
if ( cmd == 'L' ) {
/* get line number */
*(tmp+0) = get_hex( *(sbuf+1) ) ;
/* show */
put_lcd_str(*(tmp+0),0,sbuf+2);
}
/* test motor */
if ( cmd == 'M' ) {
/* get left duty ratio */
*(tmp+0) = get_dec2( sbuf+1 ) ;
/* get left duty ratio */
*(tmp+1) = get_dec2( sbuf+3 ) ;
/* get delay time */
dcount = 1000 * get_dec2( sbuf+5 ) ;
/* run */
update_motor( *(tmp+0) , *(tmp+1) ); start_pwm(); delay_sec( dcount );
/* stop */
stop_pwm();
}
/* test memory */
if ( cmd == 'S' ) {
put_lcd_clear(0);
/* set 0x55 */
xcopy(tmp,(UBYTE *)"0x55"); put_lcd_str(0,0,tmp);
/* verify */
verify_sram( 0x55 );
/* set 0xaa */
xcopy(tmp,(UBYTE *)"0xaa"); put_lcd_str(0,0,tmp);
/* verify */
verify_sram( 0xaa );
/* exit */
put_lcd_clear(1); put_lcd_str(1,0,(UBYTE *)"M:fine");
}
/* get button state */
if ( cmd == 'B' ) {
/* get button state and judge */
*(tmp+0) = '0' ; if ( PB.DR.BIT.B5 ) { *(tmp+0) = '1' ; }
*(tmp+1) = '0' ; if ( PB.DR.BIT.B4 ) { *(tmp+1) = '1' ; }
/* show information */
sci1_puts((UBYTE *)"Button=>");
sci1_putchar( *(tmp+0) ); sci1_putchar( *(tmp+1) ); sci1_crlf();
}
}
/*-------------------------*/
/* start and stop trigger */
/*-------------------------*/
void tsk1_proc(void)
{
/* show */
put_lcd_clear(0); put_lcd_str(0,0,(UBYTE *)"WAIT TRIGGER");
/* shift and store data */
btn_start <<= 1 ; if ( PB.DR.BIT.B4 ) { btn_start |= ON ; }
/* judge */
if ( (btn_start & MASK0F) == 0 ) {
/* store previous mode */
PRE_MODE = CUR_MODE ;
/* change current mode */
if ( CUR_MODE ) { CUR_MODE = OFF ; }
else { CUR_MODE = ON ; }
/* judge */
if ( PRE_MODE == OFF && CUR_MODE == ON ) {
/* wakeup tasks */
rsm_tsk(TSK_ID2); rsm_tsk(TSK_ID3); rsm_tsk(TSK_ID4);
/* start PWM */
start_pwm();
}
if ( PRE_MODE == ON && CUR_MODE == OFF ) {
/* suspend tasks */
sus_tsk(TSK_ID2); sus_tsk(TSK_ID3); sus_tsk(TSK_ID4);
/* stop PWM */
stop_pwm();
}
}
/* delay 20ms */
wai_tsk( 2 );
}
/*--------------------------------------------*/
/* Get Image data from c328 and Edge handling */
/*--------------------------------------------*/
void tsk2_proc(void)
{
ULONG last ;
UBYTE result,ack_dat[6],prm_dat[6] ;
UBYTE i,j ;
UWORD xadr ;
/* show */
put_lcd_clear(1); put_lcd_str(1,0,(UBYTE *)"Get image");
/* enable SRAM */
enable_sram(0,ON);
/* initialize */
result = send_init_c328();
if ( result & DFLAG ) {
sci1_puts((UBYTE *)"Initialize error!") ;
return ;
}
/* snapshot uncompressed data */
result = send_snapshot_c328();
if ( result & DFLAG ) {
sci1_puts((UBYTE *)"SnapShot error!") ;
return ;
}
/* send Get picture command */
*(c328_cmd+0) = 0x04 ; *(c328_cmd+1) = 0x01 ;
*(c328_cmd+2) = *(c328_cmd+3) = *(c328_cmd+4) = 0x00 ;
send_c328_primitive();
/* wait 12 data come */
while ( get_ring_cap() < 12 ) ;
/* get Ack data from ring buffer */
for ( i = 0 ; i < 6 ; i++ ) { *(ack_dat+i) = get_ring(); }
/* judge */
if ( *(ack_dat+1) != 0x0E || *(ack_dat+2) != 0x04 ) {
/* dummy read */
for ( i = 0 ; i < 6 ; i++ ) { *(prm_dat+i) = get_ring(); }
/* show error message to HOST */
sci1_puts((UBYTE *)"Ack error!") ;
/* */
return ;
}
/* get Ack data from ring buffer */
for ( i = 0 ; i < 6 ; i++ ) { *(prm_dat+i) = get_ring(); }
/* judge */
if ( *(prm_dat+1) != 0x0A || *(prm_dat+2) != 0x01 ) {
/* dummy read */
last = get_ring_cap() ;
for ( i = 0 ; i < (UWORD)last ; i++ ) { *(ack_dat+i) = get_ring() ; }
/* show error message to HOST */
sci1_puts((UBYTE *)"Data error!") ;
/* */
return ;
}
/* calculate data size */
last = *(prm_dat+3) ; last <<= 8 ;
last += *(prm_dat+4) ; last <<= 8 ;
last += *(prm_dat+5) ;
/* get image data */
xadr = SENTRY ;
for ( j = 0 ; j < YLAST ; j++ ) {
/* raster handling */
for ( i = 0 ; i < XLAST ; i++ ) {
/* wait until data aquired */
while ( get_ring_cap() == 0 ) ;
/* get data from receive buffer */
result = get_ring();
/* store data to SRAM */
put_sram(xadr,result);
/* address increment */
xadr++ ;
}
}
/* send ACK */
send_ack_c328();
/* disable SRAM */
enable_sram(0,OFF);
}
/*----------*/
/* Analizer */
/*----------*/
void tsk3_proc(void)
{
UBYTE idx,long_range,short_range,sense ;
/* show */
put_lcd_clear(1); put_lcd_str(1,0,(UBYTE *)"Analyze");
/* get LongRange line */
get_line(SENTRY+XLAST*5,src0);
/* convert sensor data */
long_range = get_road(src0) & 0x81 ;
/* get ShortRange line */
get_line(SENTRY+XLAST*40,src1);
/* convert binary data */
short_range = get_road(src1);
/* default */
idx = NORMAL_CENTER ;
/* judge */
if ( m_state == LANE_CHANGE ) {
/* sequencer */
switch ( state ) {
/* straight */
case 20 : state = 20 ; sense = get_place(long_range) ;
if ( sense == S_EDGE_LEFT ) { state = 21 ; }
if ( sense == S_EDGE_RIGHT ) { state = 25 ; }
break ;
/* turn right with 45 degree */
case 21 : state = 22 ; break ;
/* move straight until detect center white */
case 22 : state = 22 ; sense = get_place(short_range) ;
if ( sense == S_CENTER ) { state = 23 ; }
break ;
/* turn left with 45 degree */
case 23 : state = 24 ; break ;
/* return NORMAL mode */
case 24 : m_state = NORMAL ; state = 0 ; break ;
/* turn left with 45 degree */
case 25 : state = 26 ; break ;
/* move straight until detect center white */
case 26 : state = 26 ; sense = get_place(short_range) ;
if ( sense == S_CENTER ) { state = 27 ; }
break ;
/* turn right with 45 degree */
case 27 : state = 28 ; break ;
/* return NORMAL mode */
case 28 : m_state = NORMAL ; state = 0 ; break ;
default : break ;
}
m_dir = state % 10 ;
/* straight */
if ( (m_dir % 2) == 0 ) { idx = LANE_CENTER ; }
/* turn right 45 degree */
if ( m_dir == 1 || m_dir == 7 ) { idx = LANE_RIGHT ; }
/* turn left 45 degree */
if ( m_dir == 3 || m_dir == 5 ) { idx = LANE_LEFT ; }
/* mode */
put_lcd_str(1,7,(UBYTE *)"LANE");
}
else
/* CRANK */
{
if ( m_state == LANE_CHANGE ) {
/* sequencer */
switch ( state ) {
/* straight */
case 10 : state = 10 ; sense = get_place(long_range) ;
if ( sense == S_EDGE_LEFT ) { state = 11 ; }
if ( sense == S_EDGE_RIGHT ) { state = 15 ; }
break ;
/* turn right with 45 degree */
case 11 : state = 12 ; break ;
/* move straight until detect center white */
case 12 : state = 12 ; sense = get_place(short_range) ;
if ( sense == S_CENTER ) { state = 13 ; }
break ;
/* turn right with 45 degree */
case 13 : state = 14 ; break ;
/* return NORMAL mode */
case 14 : m_state = NORMAL ; state = 0 ; break ;
/* turn left with 45 degree */
case 15 : state = 16 ; break ;
/* move straight until detect center white */
case 16 : state = 16 ; sense = get_place(short_range) ;
if ( sense == S_CENTER ) { state = 17 ; }
break ;
/* turn left with 45 degree */
case 17 : state = 18 ; break ;
/* return NORMAL mode */
case 18 : m_state = NORMAL ; state = 0 ; break ;
default : break ;
}
m_dir = state % 10 ;
/* straight */
if ( (m_dir % 2) == 0 ) { idx = CRANK_LEFT ; }
/* turn right with 45 degree */
if ( m_dir == 1 || m_dir == 5 ) { idx = CRANK_RIGHT ; }
/* turn left with 45 degree */
if ( m_dir == 3 || m_dir == 7 ) { idx = CRANK_LEFT ; }
/* mode */
put_lcd_str(1,7,(UBYTE *)"CRANK");
}
/* NORMAL */
else
{
/* judge directions */
if ( m_state == NORMAL ) {
/* default */
state = 0 ;
/* get sensor value */
sense = get_place(short_range) ;
/* judge */
if ( sense == S_CENTER ) { idx = NORMAL_CENTER ; }
if ( sense == S_BIT_RIGHT ) { idx = NORMAL_BIT_RIGHT ; }
if ( sense == S_RIGHT ) { idx = NORMAL_RIGHT ; }
if ( sense == S_MUCH_RIGHT ) { idx = NORMAL_MUCH_RIGHT; }
if ( sense == S_BIT_LEFT ) { idx = NORMAL_BIT_LEFT ; }
if ( sense == S_LEFT ) { idx = NORMAL_LEFT ; }
if ( sense == S_MUCH_LEFT ) { idx = NORMAL_MUCH_LEFT ; }
if ( sense == S_ALL_WHITE ) {
m_state = CRANK ; state = 10 ; idx = CRANK_CENTER ;
}
if ( sense == S_ALL_LEFT || sense == S_ALL_RIGHT ) {
m_state = LANE_CHANGE ; state = 20 ; idx = LANE_CENTER ;
}
/* mode */
put_lcd_str(1,7,(UBYTE *)"NORMAL");
}
}
}
/* set parameters */
left_duty = drive[idx].left ;
right_duty = drive[idx].right ;
dcount = drive[idx].wtime ;
/* show */
put_lcd_clear(1); put_lcd_str(1,0,(UBYTE *)"Generate code");
}
/*----------------------------------*/
/* Update duty ratio and wait delay */
/*----------------------------------*/
void tsk4_proc(void)
{
UBYTE dstr[3] ;
/* update both duty ratios */
update_motor(left_duty,right_duty);
/* set delay */
delay_ms( dcount );
/* show duty ratios */
put_lcd_clear(0);
*(dstr+2) = 0 ;
/* left duty */
*(dstr+0) = left_duty / 10 + '0' ;
*(dstr+1) = left_duty % 10 + '0' ;
put_lcd_str(0,0,dstr);
/* right duty */
*(dstr+0) = right_duty / 10 + '0' ;
*(dstr+1) = right_duty % 10 + '0' ;
put_lcd_str(0,2,(UBYTE *)"% ");
put_lcd_str(0,4,dstr);
put_lcd_str(0,6,(UBYTE *)"%");
}
/*------------------*/
/* system call body */
/*------------------*/
void init_os(void)
{
ready = vldtsk = 0 ;
}
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
vldtsk |= (1 << tid) ;
tcb[tid].tsk = tsk;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
if ( sta == TTS_READY ) { ready |= (1 << tid); suspend &= ~(1 << tid); }
if ( sta == TTS_WAIT ) { ready &= ~(1 << tid); suspend &= ~(1 << tid); }
if ( sta == TTS_SUSPEND ) { ready &= ~(1 << tid); suspend |= (1 << tid); }
}
void rsm_tsk(UBYTE tid)
{
ready |= (1 << tid); suspend &= ~(1 << tid);
}
void sus_tsk(UBYTE tid)
{
ready &= ~(1 << tid); suspend |= (1 << tid);
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
ready &= ~(1 << run_tsk);
suspend &= ~(1 << run_tsk);
tcb[run_tsk].wcount = x ;
}
UBYTE is_tsk_ready(UBYTE tid)
{
return( (ready >> tid) & 1 ) ;
}
/*-----------------------------*/
/* timer handler */
/* call from timer interrupt */
/*-----------------------------*/
void timer_handler(void)
{
UBYTE tmp,i;
tmp = (ready ^ vldtsk) ^ suspend ;
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
if ( tmp & 1 ) {
tcb[i].wcount-- ;
if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
}
tmp >>= 1 ;
}
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
/* PORT 1 */
P1DR = 0x00 ; /* lower address */
P1DDR = MASKFF ; /* all outputs */
/* PORT 2 */
P2DR = 0x03 ; /* upper address */
P2DDR = MASKFF ; /* all outputs */
/* PORT 3 */
P3DR = 0x00 ; /* SRAM bus interface */
P3DDR = 0xff ;
/* PORT 4 */
P4DR = 0xff ;
P4DDR = 0xef ; /* P44 : input , others : output */
/* PORT 5 */
P5DR = 0x03 ; /* P50 : nWE , P51 : nOE , P52 : nCS0 , P53 : nCS1 */
P5DDR = MASKFF ; /* all outputs */
/* PORT 6 */
P6DR = 0xff ; /* */
P6DDR = 0xff ; /* all outputs */
/* PORT 9 */
P9DR = 0x00 ; /* */
P9DDR = 0x03 ; /* P95 P94 P91 P90 => outputs */
/* PORT A */
PADR = 0xff ; /* turn off all LEDs */
PADDR = 0xff ; /* all outputs */
/* PORT B */
PBDR = 0xc0 ; /* button and control */
PBDDR = 0xcf ; /* PB5 : mode , PB4 : start */
/* initialize SCI */
init_SCI0(br14400); init_SCI1(br57600);
/* initialize ITU0 */
init_timer0(); init_timer1();
/* initialize PWM module */
init_pwm();
/* initialize RTOS */
os_cnt = 10 ;
/* initialize serial buffer */
sindex = *(sbuf+0) = 0 ;
/* */
btn_start = MASKFF ;
/* initialize CAMERA */
init_ring();
establish_c328();
/* clear system timer */
millisec_cnt = 0 ;
/* get drive pattern */
get_drivec();
/* state */
m_state = NORMAL ;
state = 0 ;
/* module stanby */
/*
MSTCR.BIT._DMAC = ON ; // disable DMAC
MSTCR.BIT._RFSHC = ON ; // disable REFRESH
MSTCR.BIT._AD = ON ; // disable A/D
*/
}
/*********************************/
/* get dirve pattern from EEPROM */
/*********************************/
void get_drivec(void)
{
UWORD tmp ;
UBYTE i ;
/* transfer data */
for ( i = 0 ; i < 64 ; i++ ) {
if ( (i % 2) == 0 ) {
/* get data (DUTY radio) */
tmp = read_epa( i ) ;
/* store DUTY ratio */
drive[i].left = (tmp / 100) ;
drive[i].right = (tmp % 100) ;
} else {
/* get delay (others) */
drive[i].wtime = read_epa( i ) ;
}
}
/* risky mode */
if ( MODE_BUTTON ) {
for ( i = 1 ; i < 8 ; i++ ) {
/* times 1.2 */
tmp = drive[i].left ; tmp *= 12 ; drive[i].left = (tmp / 10) ;
tmp = drive[i].right ; tmp *= 12 ; drive[i].right = (tmp / 10) ;
/* times 0.85 */
tmp = drive[i].wtime ; tmp *= 85 ; drive[i].wtime = (tmp / 100) ;
}
}
}
/**************************/
/* initialize PWM modules */
/**************************/
void init_pwm(void)
{
pwm_cnt = left_duty = right_duty = 0 ;
}
/**********************/
/* enable PWM modules */
/**********************/
void start_pwm(void)
{
ITU.TSTR.BIT.STR1 = ON;
}
/***********************/
/* disable PWM modules */
/***********************/
void stop_pwm(void)
{
ITU.TSTR.BIT.STR1 = OFF;
/* set logical level */
PB.DR.BIT.B0 = OFF ;
PB.DR.BIT.B2 = OFF ;
}
/***************************/
/* update both motor speed */
/***************************/
void update_motor(UBYTE dl,UBYTE dr)
{
left_duty = dl ;
right_duty = dr ;
}
/*************************/
/* hexadecimal to number */
/*************************/
UBYTE get_hex(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* calculate */
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 ;
}
/******************************************/
/* hexadecimal to number formated 2 bytes */
/******************************************/
UBYTE get_hex2(UBYTE *x)
{
UBYTE result ;
/* get first byte */
result = get_hex(*x) ;
result <<= 4 ;
/* get next byte */
result += get_hex(*(x+1));
return result ;
}
/******************************************/
/* hexadecimal to number formated 4 bytes */
/******************************************/
UWORD get_hex4(UBYTE *x)
{
UWORD result ;
/* get first word */
result = get_hex2(x) ;
result <<= 8 ;
/* get next word */
result += get_hex2(x+2) ;
return result ;
}
/**************************************/
/* decimal to number formated 2 bytes */
/**************************************/
UBYTE get_dec2(UBYTE *x)
{
UBYTE result ;
/* get first byte */
result = get_hex(*x) ;
result *= 10 ;
/* get next byte */
result += get_hex(*(x+1));
return result ;
}
/**********************/
/* put word to EEPROM */
/**********************/
void write_epa(UBYTE adr,UWORD dat)
{
UBYTE loop ;
UWORD cmd ;
/* default */
ROM_SK = OFF ; ROM_CS = ON ;
/* set command */
cmd = 0x130 ;
for ( loop = 0 ; loop < 9 ; loop++ ) {
/* send command */
ROM_DI = OFF ;
if ( cmd & 0x100 ) { ROM_DI = ON ; }
/* impress SK : H */
ROM_SK = ON ;
/* shift */
cmd <<= 1 ;
/* impress SK : L */
ROM_SK = OFF ;
}
/* send address */
cmd = 0x140 | (adr & 0x3f) ;
for ( loop = 0 ; loop < 9 ; loop++ ) {
/* send command */
ROM_DI = OFF ;
if ( cmd & 0x100 ) { ROM_DI = ON ; }
/* impress SK : H */
ROM_SK = ON ;
/* shift */
cmd <<= 1 ;
/* impress SK : L */
ROM_SK = OFF ;
}
/* send data */
cmd = dat ;
for ( loop = 0 ; loop < 16 ; loop++ ) {
/* send command */
ROM_DI = OFF ;
if ( cmd & 0x8000 ) { ROM_DI = ON ; }
/* impress SK : H */
ROM_SK = ON ;
/* shift */
cmd <<= 1 ;
/* impress SK : L */
ROM_SK = OFF ;
}
ROM_DI = OFF ;
/* disable CS */
ROM_CS = OFF ;
/* 10ms delay */
delay_ms(10);
}
/************************/
/* get word from EEPROM */
/************************/
UWORD read_epa(UBYTE adr)
{
UBYTE loop ;
UWORD result ;
UWORD cmd ;
/* default */
ROM_SK = OFF ;
/* set address */
cmd = 0x180 | (adr & 0x3f) ;
/* enable CS */
ROM_CS = ON ;
/* get data */
result = 0 ;
for ( loop = 0 ; loop < 25 ; loop++ ) {
/* send command */
ROM_DI = OFF ;
if ( loop < 9 ) { if ( cmd & 0x100 ) { ROM_DI = ON ; } }
/* impress SK : H */
ROM_SK = ON ;
/* shift */
cmd <<= 1 ;
/* impress SK : L */
ROM_SK = OFF ;
/* shift */
result <<= 1 ;
/* get data */
if ( ROM_DO == ON ) { result |= ON ; }
}
/* disable CS */
ROM_CS = OFF ;
return result;
}
/**************************/
/* enable or disable SRAM */
/**************************/
void enable_sram(UBYTE bkn,UBYTE onoff)
{
switch ( bkn ) {
case 1 : SRAM_CS1 = ON ; if ( onoff ) { SRAM_CS1 = OFF ; }
break ;
case 2 : SRAM_CS2 = ON ; if ( onoff ) { SRAM_CS2 = OFF ; }
break ;
case 3 : SRAM_CS3 = ON ; if ( onoff ) { SRAM_CS3 = OFF ; }
break ;
default: SRAM_CS0 = ON ; if ( onoff ) { SRAM_CS0 = OFF ; }
break ;
}
}
/********************/
/* put byte to SRAM */
/********************/
void put_sram(UWORD adr,UBYTE dat)
{
/* impress address */
P1DR = adr & MASKFF ;
P2DR = (adr >> 8) & MASKFF ;
/* impress data */
P3DR = dat ;
/* impress WE : L */
SRAM_WE = OFF ;
/* dummy procedure */
adr <<= 1 ;
/* impress WE : H */
SRAM_WE = ON ;
/* impress data : 00000000 */
P3DR = 0 ;
}
/**********************/
/* get byte from SRAM */
/**********************/
UBYTE get_sram(UWORD adr)
{
UBYTE result ;
/* impress address */
P1DR = adr & MASKFF ;
P2DR = (adr >> 8) & MASKFF ;
/* change direction */
P3DDR = 0x00 ;
/* impress OE : L */
SRAM_OE = OFF ;
/* get data */
result = P3DR ;
/* impress OE : H */
SRAM_OE = ON ;
/* change direction */
P3DDR = 0xff ;
/* impress data : 00000000 */
P3DR = 0 ;
return result;
}
void verify_sram(UBYTE x)
{
UWORD i ;
UBYTE edat,tmp[8],flag;
/* set data */
for ( i = 0 ; i < 32768 ; i++ ) { put_sram( i , x ); }
/* verify */
flag = OFF ;
for ( i = 0 ; i < 32768 ; i++ ) {
edat = get_sram( i ) ;
if ( edat != x ) { flag = ON ; }
}
put_lcd_clear(0);
xcopy(tmp,(UBYTE *)"M:OK!");
if ( flag ) { xcopy(tmp,(UBYTE *)"M:NG!"); }
put_lcd_str(0,5,tmp);
}
/*+++++++++++++++++*/
/* ITU0 initialize */
/*+++++++++++++++++*/
void init_timer0(void)
{
/* stop timer */
ITU.TSTR.BIT.STR0 = OFF ;
/* TOER : Timer Output Enable Register
7 **** -> 0
6 **** -> 0
5 EXB4 -> 0
4 EXA4 -> 0
3 EB3 -> 0
2 EB4 -> 0
1 EA4 -> 0
0 EA3 -> 0
*/
ITU.TOER.BYTE = 0 ;
/* TIOR : Timer I/O Control Register
7 **** -> 0
6 IOB2 -> 0 GRB is not output compare match register
5 IOB1 -> 0
4 IOB0 -> 0
3 **** -> 0
2 IOA2 -> 0 GRA is not output compare match register
1 IOA1 -> 0
0 IOA0 -> 0
*/
ITU0.TIOR.BYTE = 0 ;
/* TCR : Timer Control Register
7 **** -> 0
6 CCLR1 -> 0 clear TCNT if GRA = TCNT
5 CCLR0 -> 1
4 CKEG1 -> 0 rising edge
3 CKEG0 -> 0
2 TPSC2 -> 0 φ利用
1 TPSC1 -> 0
0 TPSC0 -> 0
*/
ITU0.TCR.BYTE = 0x20 ;
/* TIER : Timer Interrupt Enable Register
7 **** -> 0
6 *** -> 0
5 *** -> 0
4 *** -> 0
3 *** -> 0
2 OVIE -> 0
1 IMIEB -> 0
0 IMIEA -> 1 select compare match interrupt
*/
ITU0.TIER.BIT.IMIEA = ON ;
/* reference */
ITU0.GRA = ITU0_AREG ;
ITU0.GRB = MASKFFFF ;
/* counter */
ITU0.TCNT = 0 ;
/* start timer */
ITU.TSTR.BIT.STR0 = ON ;
}
/*+++++++++++++++++++++++++++++++++++++*/
/* ITU0 interrupt with compare match A */
/* 1ms interval */
/*+++++++++++++++++++++++++++++++++++++*/
void int_imia0(void)
{
UBYTE dummy ;
/* clear flag */
dummy = ITU0.TSR.BIT.IMFA ;
ITU0.TSR.BIT.IMFA = OFF ;
/* increment */
millisec_cnt++ ;
/* RTOS handler */
os_cnt-- ;
if ( os_cnt > 0 ) return ;
os_cnt = 10 ;
timer_handler();
}
/*+++++++++++++++++*/
/* ITU1 initialize */
/*+++++++++++++++++*/
void init_timer1(void)
{
/* stop timer */
ITU.TSTR.BIT.STR1 = OFF ;
ITU.TOER.BYTE = 0 ;
ITU1.TIOR.BYTE = 0 ;
ITU1.TCR.BYTE = 0x20 ;
ITU1.TIER.BIT.IMIEA = ON ;
/* reference */
ITU1.GRA = ITU1_AREG ;
ITU1.GRB = MASKFFFF ;
/* counter */
ITU1.TCNT = 0 ;
}
/*+++++++++++++++++++++++++++++++++++++*/
/* ITU1 interrupt with compare match A */
/* 400kHz interval */
/*+++++++++++++++++++++++++++++++++++++*/
void int_imia1(void)
{
UBYTE dummy ;
/* clear flag */
dummy = ITU1.TSR.BIT.IMFA ;
ITU1.TSR.BIT.IMFA = OFF ;
/* increment */
pwm_cnt++ ;
if ( pwm_cnt == 100 ) { pwm_cnt = 0 ; }
/* judge logical level */
LEFT_MOTOR = RIGHT_MOTOR = OFF ;
if ( pwm_cnt < left_duty ) { LEFT_MOTOR = ON ; }
if ( pwm_cnt < right_duty ) { RIGHT_MOTOR = ON ; }
/* put logical level */
PB.DR.BIT.B2 = LEFT_MOTOR ;
PB.DR.BIT.B0 = RIGHT_MOTOR ;
}
/*++++++++++++++++++++++*/
/* SCI1 object handling */
/*++++++++++++++++++++++*/
void init_SCI0(TBaudRate x)
{
volatile UWORD i;
/* SCR : Serial Control Register
7 bit TIE -> 0 Transmit Interrupt Enable(disable)
6 bit RIE -> 0 Receive Interrupt Enable(disable)
5 bit TE -> 0 Transmit Enable(disable)
4 bit RE -> 0 Receive Enable(disable)
3 bit MPIE -> 0 Multi Processor Interrupt Enable(disable)
2 bit TEIE -> 0 Transmit End Interrupt Enable(disable)
1 bit CKE1 -> 0 Clock Source (Use Internal Baud Rate Generator)
0 bit CKE0 -> 0
*/
SCI0.SCR.BYTE = 0 ;
/* SMR : Serial Mode Register
7 bit C/nA -> 0 Communication Mode(Asynchronous)
6 bit CHR -> 0 data Charactor (8 bits)
5 bit PE -> 0 Parity Enable(disable)
4 bit O/nE -> 0 Parity Mode(even)
3 bit STOP -> 0 Stop Bit(1 bit)
2 bit MP -> 0 Multi Processor(disable)
1 bit CKS1 -> 0 Clock Source ( φ )
0 bit CKS0 -> 0
*/
SCI0.SMR.BYTE = 0 ;
/* data transfer speed */
SCI0.BRR = x ;
/* wait 1 frame */
for (i = 0; i < 3000 ; i++) ;
/* enable Transmmit and Receive with interrupt */
SCI0.SCR.BYTE = 0x70 ;
}
void init_SCI1(TBaudRate x)
{
volatile UWORD i;
/* SCR : Serial Control Register */
SCI1.SCR.BYTE = 0 ;
/* SMR : Serial Mode Register */
SCI1.SMR.BYTE = 0 ;
/* data transfer speed */
SCI1.BRR = x ;
/* wait 1 frame */
for (i = 0; i < 3000 ; i++) ;
/* enable Transmmit and Receive with interrupt */
SCI1.SCR.BYTE = 0x70 ;
}
/*--------------------*/
/* SCI0 receive error */
/*--------------------*/
void int_eri0(void)
{
/* SSR : Serial State Register
7 bit TDRE -> 0 Transmit Data Buffer Empty
6 bit RDRF -> 1 Receive Data Buffer Full
5 bit ORER -> 1 Overrun Error
4 bit FER -> 1 Framming Error
3 bit PER -> 0 Parity Error
2 bit TEND -> 0 Transmit End
1 bit MPB -> 0 Multi Processor Receive Bit
0 bit MPBT -> 0 Multi Processor Bit Transfer
*/
/* Framing Error */
SCI0.SSR.BYTE &= ~0x30 ;
}
/*--------------------*/
/* SCI1 receive error */
/*--------------------*/
void int_eri1(void)
{
/* SSR : Serial State Register
7 bit TDRE -> 0 Transmit Data Buffer Empty
6 bit RDRF -> 1 Receive Data Buffer Full
5 bit ORER -> 1 Overrun Error
4 bit FER -> 1 Framming Error
3 bit PER -> 0 Parity Error
2 bit TEND -> 0 Transmit End
1 bit MPB -> 0 Multi Processor Receive Bit
0 bit MPBT -> 0 Multi Processor Bit Transfer
*/
/* Framing Error */
SCI1.SSR.BYTE &= ~0x30 ;
}
/*------------------------*/
/* SCI0 receive interrupt */
/*------------------------*/
void int_rxi0(void)
{
volatile UBYTE ch,dummy ;
/* clear flag */
dummy = SCI0.SSR.BYTE ;
SCI0.SSR.BIT.RDRF = OFF ;
/* get a character */
ch = SCI0.RDR ;
/* store */
put_ring( ch ) ;
}
/*------------------------*/
/* SCI1 receive interrupt */
/*------------------------*/
void int_rxi1(void)
{
volatile UBYTE ch,dummy ;
/* clear flag */
dummy = SCI1.SSR.BYTE ;
SCI1.SSR.BIT.RDRF = OFF ;
/* get a character */
ch = SCI1.RDR ;
/* store */
*(sbuf+sindex) = ch ;
sindex++ ;
/* check */
if ( ch == '\r' ) {
*(sbuf+sindex) = 0 ;
sindex = 0 ;
SFLAG = ON ;
}
}
/*-----------------------*/
/* SCI0 send 1 charactor */
/*-----------------------*/
void sci0_putchar(UBYTE x)
{
/* wait data transfer */
while ( SCI0.SSR.BIT.TDRE == OFF ) ;
/* put */
SCI0.TDR = x ;
SCI0.SSR.BIT.TDRE = OFF ;
}
/*-----------------------*/
/* SCI1 send 1 charactor */
/*-----------------------*/
void sci1_putchar(UBYTE x)
{
/* wait data transfer */
while ( SCI1.SSR.BIT.TDRE == OFF ) ;
/* put */
SCI1.TDR = x ;
SCI1.SSR.BIT.TDRE = OFF ;
}
/*------------*/
/* SCI1 CR LF */
/*------------*/
void sci1_crlf(void)
{
sci1_putchar('\r');
sci1_putchar('\n');
}
/*---------------------*/
/* SCI1 send 1 strings */
/*---------------------*/
void sci1_puts(UBYTE *x)
{
/* send 1 charactors */
while ( *x ) {
sci1_putchar(*x);
x++ ;
}
/* CR LF */
sci1_crlf();
}
/*----------------------*/
/* number to ASCII code */
/*----------------------*/
UBYTE conv_hexdigit(UBYTE x)
{
UBYTE result ;
result = '0' ;
if ( x <= 9 ) { result = x + '0' ; }
if ( 9 < x && x < 16 ) { result = x - 10 + 'A' ; }
return result ;
}
/*------------------------*/
/* get milli second count */
/*------------------------*/
ULONG millisec(void)
{
return millisec_cnt ;
}
/*----------------------------*/
/* delay N ms (N = 1-> 65535) */
/*----------------------------*/
void delay_ms(UWORD x)
{
ULONG mstarget ;
/* calculate last count */
mstarget = millisec() + x ;
/* wait */
while ( millisec() < mstarget ) ;
}
/*-----------------------------*/
/* delay N sec (N = 1-> 65535) */
/*-----------------------------*/
void delay_sec(UWORD x)
{
while ( x ) {
delay_ms(1000);
x-- ;
}
}
/*------------------------*/
/* send c328 control word */
/*------------------------*/
void send_c328_primitive(void)
{
sci0_putchar( 0xAA ) ; sci0_putchar( *(c328_cmd+0) );
sci0_putchar( *(c328_cmd+1) ); sci0_putchar( *(c328_cmd+2) );
sci0_putchar( *(c328_cmd+3) ); sci0_putchar( *(c328_cmd+4) );
}
/*--------------------------------*/
/* establish connection with c328 */
/*--------------------------------*/
UBYTE establish_c328(void)
{
UBYTE loop,result ;
/* default */
result = ON ;
EFLAG = OFF ;
/* send sync */
for ( loop = 0 ; loop < 80 ; loop++ ) {
/* send command */
send_sync_c328();
/* delay */
delay_ms(10);
/* get capacity */
if ( get_ring_cap() >= 12 ) { EFLAG = ON ; }
if ( EFLAG ) break ;
}
/* judge */
if ( EFLAG ) {
send_ack_c328() ;
result = OFF ;
} else {
sci1_puts((UBYTE *)"Not established with serial camera!");
}
return result ;
}
/*-----------------*/
/* initialize c328 */
/*-----------------*/
UBYTE send_init_c328(void)
{
UBYTE i ;
UBYTE result ;
UBYTE ack_dat[6] ;
/* set parameters */
*(c328_cmd+0) = 0x01 ;
*(c328_cmd+1) = 0x00 ;
*(c328_cmd+2) = 0x03 ; /* Grayscale : 8bits */
*(c328_cmd+3) = 0x01 ; /* Preview : resolution : 80 x 60 */
*(c328_cmd+4) = 0x01 ; /* JPEG : resolution : 80 x 64 */
/* send */
send_c328_primitive();
/* delay 15ms */
delay_ms(15);
/* judge */
result = OFF ;
if ( get_ring_cap() < 6 ) {
result = ON ;
} else {
/* read data from ring buffer */
for ( i = 0 ; i < 6 ; i++ ) { *(ack_dat+i) = get_ring(); }
/* judge */
if ( *(ack_dat+1) != 0x0E || *(ack_dat+2) != 0x01 ) { result = ON ; }
}
return result ;
}
/*-----------------------------*/
/* send snap_shot word to c328 */
/*-----------------------------*/
UBYTE send_snapshot_c328(void)
{
UBYTE i,result,ack_dat[6];
/* set parameters */
*(c328_cmd+0) = 0x05 ;
*(c328_cmd+1) = 0x01 ; /* uncompressed picture */
*(c328_cmd+2) = 0x01 ; /* captures next frame */
*(c328_cmd+3) = 0x00 ;
*(c328_cmd+4) = 0x00 ;
/* send */
send_c328_primitive();
/* delay 15ms */
delay_ms(15);
/* judge */
result = OFF ;
if ( get_ring_cap() < 6 ) {
result = ON ;
} else {
/* read data from ring buffer */
for ( i = 0 ; i < 6 ; i++ ) { *(ack_dat+i) = get_ring(); }
/* judge */
if ( *(ack_dat+1) != 0x0E || *(ack_dat+2) != 0x05 ) { result = ON ; }
}
return result ;
}
/*-------------------------------*/
/* send synchronous word to c328 */
/*-------------------------------*/
void send_sync_c328(void)
{
/* set parameters */
*(c328_cmd+0) = 0x0D ;
*(c328_cmd+1) = 0x00 ; *(c328_cmd+2) = 0x00 ;
*(c328_cmd+3) = 0x00 ; *(c328_cmd+4) = 0x00 ;
/* send */
send_c328_primitive();
}
/*-------------------------*/
/* send acknowlege to c328 */
/*-------------------------*/
void send_ack_c328(void)
{
/* set parameters */
*(c328_cmd+0) = 0x0E ;
*(c328_cmd+1) = 0x0D ; *(c328_cmd+2) = 0x00 ;
*(c328_cmd+3) = 0x00 ; *(c328_cmd+4) = 0x00 ;
/* send */
send_c328_primitive();
}
/*----------------------------------*/
/* put one charactor to ring buffer */
/*----------------------------------*/
void put_ring(UBYTE x)
{
UBYTE ptr ;
/* store data */
ptr = rings.wrp ;
rings.data[ptr] = x ;
/* pointer increment */
ptr++ ;
if ( ptr == MASK80 ) { ptr = 0 ; }
rings.wrp = ptr ;
/* capacity increment */
rings.capacity = rings.capacity + 1 ;
}
/*------------------------------------*/
/* get one charactor from ring buffer */
/*------------------------------------*/
UBYTE get_ring(void)
{
UBYTE ptr ;
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( rings.capacity == 0 ) { return result ; }
/* read data */
ptr = rings.rdp ;
result = rings.data[ptr] ;
/* pointer increment */
ptr++ ;
if ( ptr == MASK80 ) { ptr = 0 ; }
rings.rdp = ptr ;
/* capacity decrement */
rings.capacity = rings.capacity - 1 ;
return result ;
}
/*--------------------------*/
/* get ring buffer capactiy */
/*--------------------------*/
UBYTE get_ring_cap(void)
{
return rings.capacity ;
}
/*------------------------*/
/* initialize ring buffer */
/*------------------------*/
void init_ring(void)
{
UBYTE loop ;
/* clear data area */
for ( loop = 0 ; loop < 128 ; loop++ ) { rings.data[loop] = 0 ; }
/* clear capacity */
rings.capacity = 0 ;
/* initialize read pointer */
rings.rdp = 0 ;
/* initialize write pointer */
rings.wrp = 0 ;
}
/*------------------------------*/
/* initialize LCD with power up */
/*------------------------------*/
void init_lcd(void)
{
/* initialize hardware */
delay_ms(20) ; /* 20ms */
put_lcd_primitive(OFF,0x30);
delay_ms(5) ; /* 5ms */
put_lcd_primitive(OFF,0x30);
delay_ms(1); /* 1ms */
put_lcd_primitive(OFF,0x30);
delay_ms(1); /* 1ms */
put_lcd_primitive(OFF,0x20);
delay_ms(1); /* 1ms */
/* set function */
put_lcd_primitive(OFF,0x28);
/*
Function
001 DL N F * *
DL(Data Length) = 0 (4bits)
N(Row) = 1 (2 row)
F(Font) = 0 (5x7)
001 0 1 0 * *
*/
put_lcd_primitive(OFF,0x08);
/*
Display off
0000 1 D C B
D(Display) = 0 (OFF)
C(Cursor) = 0 (OFF)
B(Blink) = 0 (OFF)
0000 1 0 0 0
*/
put_lcd_primitive(OFF,0x01);
/*
Clear
0000 0001
*/
delay_ms(2); /* 2ms */
put_lcd_primitive(OFF,0x06);
/*
Entry Mode
0000 01 I/D S
I/D(Increment/Decrement) = 1 (Increment)
S(Shift) = 0 (No shift)
0000 01 1 0
*/
delay_ms(2); /* 2ms */
put_lcd_primitive(OFF,0x0c);
/*
Display on
0000 1 D C B
D(Display) = 1 (ON)
C(Cursor) = 0 (OFF)
B(Blink) = 0 (OFF)
0000 1 1 0 0
*/
put_lcd_primitive(OFF,0x02);
/*
Cursor Home
0000 001*
*/
/* place 0 row 0 column */
put_lcd_primitive(OFF,0x80);
}
/*--------------------*/
/* initialize display */
/*--------------------*/
void init_display(void)
{
put_lcd_clear(0);
put_lcd_clear(1);
put_lcd_str(0,0,(UBYTE *)"INITIALIZE"); /* MODE */
}
/*---------------------------------*/
/* transfer data to shift register */
/*---------------------------------*/
void put_lcd_primitive(UBYTE which,UBYTE x)
{
UBYTE i,tmp ;
/* get data */
tmp = x ;
/* impress signals */
LCD_CLK = OFF ;
LCD_E = OFF ;
LCD_RS = which ;
/* set data */
for ( i = 0 ; i < 8 ; i++ ) {
/* set data */
LCD_DAT = OFF ;
if ( tmp & MASK80 ) { LCD_DAT = ON ; }
/* TRG : H */
LCD_CLK = ON ;
/* shift */
tmp <<= 1 ;
/* TRG : L */
LCD_CLK = OFF ;
}
/* impress E signal : H */
LCD_E = ON ;
/* delay */
tmp <<= 1 ;
/* impress E signal : H */
LCD_E = OFF ;
LCD_RS = OFF ;
}
/*----------------------------*/
/* send string to target line */
/*----------------------------*/
void put_lcd_str(UBYTE r,UBYTE c,UBYTE *ptr)
{
UBYTE adr ;
/* check range */
if ( r > 1 ) return ;
if ( c > 15 ) return ;
/* set address */
switch ( r ) {
case 1 : adr = 0x40 ; break ;
case 2 : adr = 0x14 ; break ;
case 3 : adr = 0x54 ; break ;
default : adr = 0x00 ; break ;
}
adr += c ;
adr |= MASK80 ;
put_lcd_primitive(OFF,adr);
/* send charactor */
for ( adr = 0 ; adr < 20 ; adr++ ) {
/* judge */
if ( *ptr == '\r' ) break ;
if ( *ptr == '\0' ) break ;
/* set data */
put_lcd_primitive(ON,*ptr);
/* update pointer */
ptr++ ;
}
}
/*-----------------------*/
/* send spaces to r line */
/*-----------------------*/
void put_lcd_clear(UBYTE r)
{
UBYTE i,tmp[17] ;
/* clear buffer */
*(tmp+16) = 0 ;
for ( i = 0 ; i < 16 ; i++ ) { *(tmp+i) = ' ' ; }
/* send string */
put_lcd_str(r,i,tmp);
}
/*------------------------------------------*/
/* copy string to target distination memory */
/*------------------------------------------*/
void xcopy(UBYTE *dptr,UBYTE *sptr)
{
while ( *sptr ) {
*dptr = *sptr ;
dptr++ ;
sptr++ ;
}
*dptr = 0 ;
}
/**********************/
/* get line from SRAM */
/**********************/
UBYTE get_road(UBYTE *x)
{
UBYTE result,res0 ;
UBYTE i,smax,smin ;
UBYTE gdat[80],cat[8] ;
UWORD avr ;
/* calculate maximum and minimum */
for ( i = 0 ; i < 80 ; i++ ) {
/* set initial values */
*(gdat+i) = 0 ;
if ( i == 0 ) { smax = smin = *x ; }
/* compare */
if ( smax < *(x+i) ) { smax = *(x+i) ; }
if ( smin > *(x+i) ) { smin = *(x+i) ; }
}
/* average */
avr = (smax + smin) >> 1 ;
/* store */
for ( i = 0 ; i < 80 ; i++ ) { if ( *(x+i) > avr ) { *(gdat+i) = 1 ; } }
/* smax = smin */
res0 = 0 ;
if ( smax == smin ) { res0 = smax ; }
/* calculate block sum */
*(cat+0) = *(cat+1) = *(cat+2) = *(cat+3) = 0 ;
*(cat+4) = *(cat+5) = *(cat+6) = *(cat+7) = 0 ;
for ( i = 0 ; i < 10 ; i++ ) {
*(cat+0) += *(gdat+i+10*0); *(cat+1) += *(gdat+i+10*1);
*(cat+2) += *(gdat+i+10*2); *(cat+3) += *(gdat+i+10*3);
*(cat+4) += *(gdat+i+10*4); *(cat+5) += *(gdat+i+10*5);
*(cat+6) += *(gdat+i+10*6); *(cat+7) += *(gdat+i+10*7);
}
/* calculate block line maximum and minimum */
for ( i = 0 ; i < 8 ; i++ ) {
if ( i == 0 ) { smax = smin = *(cat+0) ; }
if ( smax < *(cat+i) ) { smax = *(cat+i) ; }
if ( smin > *(cat+i) ) { smin = *(cat+i) ; }
}
/* average */
avr = (smax + smin) >> 1 ;
/* make data with binary format */
for ( i = 0 ; i < 8 ; i++ ) {
if ( *(cat+i) > avr ) { *(cat+i) = 1 ; }
else { *(cat+i) = 0 ; }
}
/* 80 bits => 8 bits */
result = 0 ;
for ( i = 0 ; i < 8 ; i++ ) {
result <<= 1 ;
result |= *(cat+i);
}
/* judge all one */
if ( res0 > 180 ) { result = MASKFF ; }
return result ;
}
/******************/
/* get place code */
/******************/
UBYTE get_place(UBYTE x)
{
UBYTE result ;
/* default */
result = S_NONE ;
/* judge */
if ( x == 0x18 || x == 0x1c || x == 0x38 ) { result = S_CENTER ; }
if ( x == 0xff || x == 0x7f || x == 0x7e || x == 0xfe ) {
result = S_CENTER ;
}
if ( x == 0x0f || x == 0x1f || x == 0x0e || x == 0x1e ) {
result = S_ALL_RIGHT ;
} if ( x == 0xf0 || x == 0xf8 || x == 0xe0 || x == 0xe8 ) {
result = S_ALL_LEFT ;
}
if ( x == 0x80 ) { result = S_EDGE_LEFT ; }
if ( x == 0x01 ) { result = S_EDGE_RIGHT ; }
if ( x == 0x30 ) { result = S_BIT_RIGHT ; }
if ( x == 0x60 ) { result = S_RIGHT ; }
if ( x == 0xc0 ) { result = S_MUCH_RIGHT ; }
if ( x == 0x0c ) { result = S_BIT_LEFT ; }
if ( x == 0x06 ) { result = S_LEFT ; }
if ( x == 0x03 ) { result = S_MUCH_LEFT ; }
return result ;
}
/**********************/
/* get line from SRAM */
/**********************/
void get_line(UWORD x,UBYTE *ptr)
{
UBYTE i,tmp ;
for ( i = 0 ; i < XLAST ; i++ ) { tmp = get_sram(x); *(ptr+i) = tmp; x++; }
}
void show_help(void)
{
sci1_puts((UBYTE *)"C => control bit test");
sci1_puts((UBYTE *)" CW0 => nWE = 1 CW1 => nWE = 0");
sci1_puts((UBYTE *)" CR0 => nOE = 1 CR1 => nOE = 0");
sci1_puts((UBYTE *)" CS00 => nCS0 = 1 CS01 => nCS0 = 0");
sci1_puts((UBYTE *)" CS10 => nCS1 = 1 CS11 => nCS1 = 0");
sci1_puts((UBYTE *)" CS20 => nCS2 = 1 CS21 => nCS2 = 0");
sci1_puts((UBYTE *)" CS30 => nCS3 = 1 CS31 => nCS3 = 0");
sci1_puts((UBYTE *)"E => EEPROM access");
sci1_puts((UBYTE *)" EA => get 128 word data");
sci1_puts((UBYTE *)" ESaadddd => set 1 word data");
sci1_puts((UBYTE *)"D => debug");
sci1_puts((UBYTE *)" D1 => debug in");
sci1_puts((UBYTE *)" D0 => debug out");
sci1_puts((UBYTE *)"G => get image");
sci1_puts((UBYTE *)"R => read SRAM");
sci1_puts((UBYTE *)" Raaaa : aaaa <= address");
sci1_puts((UBYTE *)"L => test LCD");
sci1_puts((UBYTE *)" Lnstr => n:line number 0 or 1");
sci1_puts((UBYTE *)"M => test motor");
sci1_puts((UBYTE *)" Mllrrtt => ll:left ratio,rr:right ratio,tt:delay time");
sci1_puts((UBYTE *)"N => show status");
sci1_puts((UBYTE *)"S => test memory");
sci1_puts((UBYTE *)"B => get button state");
sci1_puts((UBYTE *)"? => help");
}
ハードウエアのテストが済んだ時点で、関数tsk0_procの内容は
変更する方がよいでしょう。
目次
前
次