秋の試走会からの改良
９月２９日に試走会があったので、RTOS(Real Time Operating System)を
使わないで、マシンを動かしてみることに。
ファームウエアは、以下のように変更しました。
#include <ADuC7026.h>
#define OFF 0
#define ON OFF+1
#define NO 0
#define YES NO+1
/* data definitions */
typedef unsigned char UBYTE ;
typedef signed char SBYTE ;
typedef unsigned short UWORD ;
typedef signed short SWORD ;
typedef unsigned long ULONG ;
typedef signed long SLONG ;
void IRQ_Handler(void) __irq;
void init_usr(void);
#define MASKFF 0xFF
#define MASK0F 0x0F
#define MASK80 0x80
#define MASK40 0x40
#define MASK20 0x20
#define MASK10 0x10
#define MASK08 0x08
#define MASK04 0x04
#define MASK02 0x02
#define MASK03 0x03
#define MASK01 0x01
#define MASKF0 0xF0
#define MASK07 0x07
#define SCCB_ID_WR 0x42
#define SCCB_ID_RD 0x43
#define S_SCL 17
#define S_SDA 16
#define S_CTRG 23
#define S_TRG 22
#define S_DIR 20
#define S_OE 19
volatile UBYTE tflag ;
/*--------------------*/
/* function prototype */
/*--------------------*/
void rs_putchar(UBYTE x);
void crlf(void);
void rs_puts(UBYTE *x);
UBYTE get_hex(UBYTE x);
void show_help(void);
void init_cam(UBYTE x);
void put_sccb_start(void);
void put_sccb_stop(void);
void put_sccb_data(UBYTE x);
UBYTE get_sccb_data(void);
UBYTE load_cam(UBYTE adr);
void show_reg(void);
void show_sensor(UBYTE x);
void send_cam(UBYTE adr,UBYTE dat);
void delay_sccb(UBYTE x);
void delay_100us(UWORD x);
void delay_ms(UWORD x);
/*---------------*/
/* FPGA handling */
/*---------------*/
typedef struct {
UWORD xfront ;
UWORD xrear ;
} MOTORP ;
MOTORP xmotor ;
void send_fpga(MOTORP x);
UBYTE get_fpga(UBYTE lx);
void show_duty(void);
void show_data(UBYTE x);
UBYTE get_sensor(UBYTE x);
void send_cam_trigger(void);
UBYTE is_center(UBYTE x);
UBYTE is_left_white(UBYTE x);
UBYTE is_right_white(UBYTE x);
UBYTE is_tiny_right(UBYTE x);
UBYTE is_right(UBYTE x);
UBYTE is_big_right(UBYTE x);
UBYTE is_tiny_left(UBYTE x);
UBYTE is_left(UBYTE x);
UBYTE is_big_left(UBYTE x);
void turn_off_led(void);
void turn_on_led(UBYTE x);
/* global variables */
volatile UBYTE uflag ;
volatile UBYTE sbuf[8] ;
volatile UBYTE sindex ;
volatile UBYTE cmd ;
volatile UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
volatile ULONG timcnt ;
volatile UBYTE state ;
volatile UWORD vscnt ;
volatile UBYTE vtrg ;
volatile UBYTE ptrg ;
volatile UBYTE str_sft ;
volatile UBYTE str_trg ;
volatile UBYTE fduty ;
volatile UBYTE rduty ;
volatile UBYTE fdir ;
volatile UBYTE rdir ;
volatile UWORD icount ;
#define ILAST 30000
#define S_MODE_BIT 18
#define REG_GAIN 0x00 /* Gain lower 8 bits (rest in vref) */
#define REG_BLUE 0x01 /* blue gain */
#define REG_RED 0x02 /* red gain */
#define REG_VREF 0x03 /* Pieces of GAIN, VSTART, VSTOP */
#define REG_COM1 0x04 /* Control 1 */
#define COM1_CCIR656 0x40 /* CCIR656 enable */
#define REG_BAVE 0x05 /* U/B Average level */
#define REG_GbAVE 0x06 /* Y/Gb Average level */
#define REG_AECHH 0x07 /* AEC MS 5 bits */
#define REG_RAVE 0x08 /* V/R Average level */
#define REG_COM2 0x09 /* Control 2 */
#define COM2_SSLEEP 0x10 /* Soft sleep mode */
#define REG_PID 0x0a /* Product ID MSB */
#define REG_VER 0x0b /* Product ID LSB */
#define REG_COM3 0x0c /* Control 3 */
#define COM3_SWAP 0x40 /* Byte swap */
#define COM3_SCALEEN 0x08 /* Enable scaling */
#define COM3_DCWEN 0x04 /* Enable downsamp/crop/window */
#define REG_COM4 0x0d /* Control 4 */
#define REG_COM5 0x0e /* All "reserved" */
#define REG_COM6 0x0f /* Control 6 */
#define REG_AECH 0x10 /* More bits of AEC value */
#define REG_CLKRC 0x11 /* Clocl control */
#define CLK_EXT 0x40 /* Use external clock directly */
#define CLK_SCALE 0x3f /* Mask for internal clock scale */
#define REG_COM7 0x12 /* Control 7 */
#define COM7_RESET 0x80 /* Register reset */
#define COM7_FMT_MASK 0x38
#define COM7_FMT_VGA 0x00
#define COM7_FMT_CIF 0x20 /* CIF format */
#define COM7_FMT_QVGA 0x10 /* QVGA format */
#define COM7_FMT_QCIF 0x08 /* QCIF format */
#define COM7_RGB 0x04 /* bits 0 and 2 - RGB format */
#define COM7_YUV 0x00 /* YUV */
#define COM7_BAYER 0x01 /* Bayer format */
#define COM7_PBAYER 0x05 /* "Processed bayer" */
#define REG_COM8 0x13 /* Control 8 */
#define COM8_FASTAEC 0x80 /* Enable fast AGC/AEC */
#define COM8_AECSTEP 0x40 /* Unlimited AEC step size */
#define COM8_BFILT 0x20 /* Band filter enable */
#define COM8_AGC 0x04 /* Auto gain enable */
#define COM8_AWB 0x02 /* White balance enable */
#define COM8_AEC 0x01 /* Auto exposure enable */
#define REG_COM9 0x14 /* Control 9 - gain ceiling */
#define REG_COM10 0x15 /* Control 10 */
#define COM10_HSYNC 0x40 /* HSYNC instead of HREF */
#define COM10_PCLK_HB 0x20 /* Suppress PCLK on horiz blank */
#define COM10_HREF_REV 0x08 /* Reverse HREF */
#define COM10_VS_LEAD 0x04 /* VSYNC on clock leading edge */
#define COM10_VS_NEG 0x02 /* VSYNC negative */
#define COM10_HS_NEG 0x01 /* HSYNC negative */
#define REG_HSTART 0x17 /* Horiz start high bits */
#define REG_HSTOP 0x18 /* Horiz stop high bits */
#define REG_VSTART 0x19 /* Vert start high bits */
#define REG_VSTOP 0x1a /* Vert stop high bits */
#define REG_PSHFT 0x1b /* Pixel delay after HREF */
#define REG_MIDH 0x1c /* Manuf. ID high */
#define REG_MIDL 0x1d /* Manuf. ID low */
#define REG_MVFP 0x1e /* Mirror / vflip */
#define MVFP_MIRROR 0x20 /* Mirror image */
#define MVFP_FLIP 0x10 /* Vertical flip */
#define REG_AEW 0x24 /* AGC upper limit */
#define REG_AEB 0x25 /* AGC lower limit */
#define REG_VPT 0x26 /* AGC/AEC fast mode op region */
#define REG_HSYST 0x30 /* HSYNC rising edge delay */
#define REG_HSYEN 0x31 /* HSYNC falling edge delay */
#define REG_HREF 0x32 /* HREF pieces */
#define REG_TSLB 0x3a /* lots of stuff */
#define TSLB_YLAST 0x04 /* UYVY or VYUY - see com13 */
#define REG_COM11 0x3b /* Control 11 */
#define COM11_NIGHT 0x80 /* NIght mode enable */
#define COM11_NMFR 0x60 /* Two bit NM frame rate */
#define COM11_HZAUTO 0x10 /* Auto detect 50/60 Hz */
#define COM11_50HZ 0x08 /* Manual 50Hz select */
#define COM11_EXP 0x02
#define REG_COM12 0x3c /* Control 12 */
#define COM12_HREF 0x80 /* HREF always */
#define REG_COM13 0x3d /* Control 13 */
#define COM13_GAMMA 0x80 /* Gamma enable */
#define COM13_UVSAT 0x40 /* UV saturation auto adjustment */
#define COM13_UVSWAP 0x01 /* V before U - w/TSLB */
#define REG_COM14 0x3e /* Control 14 */
#define COM14_DCWEN 0x10 /* DCW/PCLK-scale enable */
#define REG_EDGE 0x3f /* Edge enhancement factor */
#define REG_COM15 0x40 /* Control 15 */
#define COM15_R10F0 0x00 /* Data range 10 to F0 */
#define COM15_R01FE 0x80 /* 01 to FE */
#define COM15_R00FF 0xc0 /* 00 to FF */
#define COM15_RGB565 0x10 /* RGB565 output */
#define COM15_RGB555 0x30 /* RGB555 output */
#define REG_COM16 0x41 /* Control 16 */
#define COM16_AWBGAIN 0x08 /* AWB gain enable */
#define REG_COM17 0x42 /* Control 17 */
#define COM17_AECWIN 0xc0 /* AEC window - must match COM4 */
#define COM17_CBAR 0x08 /* DSP Color bar */
#define REG_RGB444 0x8c /* RGB 444 control */
#define R444_ENABLE 0x02 /* Turn on RGB444, overrides 5x5 */
#define R444_RGBX 0x01 /* Empty nibble at end */
#define REG_HAECC1 0x9f /* Hist AEC/AGC control 1 */
#define REG_HAECC2 0xa0 /* Hist AEC/AGC control 2 */
#define REG_HAECC7 0xaa /* Hist AEC/AGC control 7 */
#define FRONT_CENTER 0x000
#define FRONT_RIGHT 0x100
#define FRONT_LEFT 0x200
#define REAR_STOP 0x080
#define REAR_FORWARD 0x180
#define REAR_REVERSE 0x280
#define STATE_WAIT 0
#define STATE_NORMAL 1
#define STATE_CRANK 2
#define STATE_LANEC 3
#define WHITE_STATE_NONE 0
#define WHITE_STATE_RIGHT 1
#define WHITE_STATE_LEFT 2
#define WHITE_STATE_ALL 3
#define ALL_BLACK 0x00
#define ALL_WHITE 0xff
#define SENX0 0
#define SENX1 1
#define SENX2 2
#define SENX3 3
#define SENX4 4
#define ROTU 5
#define ROTL 6
#define RDUTY 7
#define LED_BLIND 0
#define LED_NORMAL 1
#define LED_CRANK 2
#define LED_LANE 3
#define MAX_RUN_PULSE 28000
volatile UBYTE bstate ;
volatile UBYTE ccnt ;
volatile UBYTE lcnt ;
volatile UBYTE sensor[5] ;
volatile UBYTE sensorx ;
volatile UBYTE direction ;
volatile UBYTE white_state ;
volatile UWORD cur_rcnt ;
volatile UWORD pre_rcnt ;
volatile UWORD dif_rcnt ;
volatile UBYTE cyclic[6] = {LED_BLIND,LED_NORMAL,LED_CRANK,LED_LANE,LED_CRANK,LED_NORMAL};
volatile UBYTE fstate ;
void tsk2_proc(void);
#define IDLE 0
#define RUN IDLE+1
volatile UBYTE duty ;
volatile UBYTE strg ;
volatile UBYTE bstate_pat[6] = {5,15,25,45,65,75} ;
void main(void)
{
volatile UBYTE tmp ;
volatile UBYTE loop ;
volatile UWORD xmes ;
/* initialize user */
init_usr();
/* show message */
rs_puts("Hello"); crlf();
/* endless loop */
while (ON)
{
/* sequencer */
switch ( state ) {
/* */
case 0 : /* start trigger */
if ( strg == ON ) {
/* clear */
strg = OFF ;
/* next state */
state = 10 ;
/* clear BLIND_RUN state */
bstate = 0 ;
}
/* debugguing */
if ( uflag == ON ) {
/* clear */
uflag = OFF ;
/* next state */
state = 1 ;
}
break ;
/* judge command */
case 1 : state = 2 ;
/* default */
tmp = 0 ;
/* new line */
crlf();
/* judge */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help(); }
/* initialize CAMERA */
if ( cmd == 'I' ) {
turn_off_led();
init_cam( *(sbuf+1)-'0' ) ;
}
/* set front parameters */
if ( cmd == 'F' || cmd == 'R' ) {
/* duty */
duty = get_hex( *(sbuf+1) ) ;
duty *= 10 ;
duty += get_hex( *(sbuf+2) ) ;
/* direction */
direction = get_hex( *(sbuf+3) ) ;
/* rear or front */
if ( cmd == 'R' ) {
rdir = direction ;
rduty = duty ;
duty |= MASK80 ;
xmotor.xrear = (rdir << 8) | duty ;
} else {
fdir = direction ;
fduty = duty ;
duty &= ~MASK80 ;
xmotor.xfront = (fdir << 8) | duty ;
}
/* impress */
send_fpga( xmotor );
}
/* set encoder pulse */
if ( cmd == 'P' ) {
for ( loop = 0 ; loop < 2 ; loop++ ) {
/* get byte */
tmp = get_fpga(ROTU);
if ( loop ) { tmp = get_fpga(ROTL); }
/* convert and show */
rs_putchar( asc_hex[(tmp >> 4) & MASK0F] );
rs_putchar( asc_hex[tmp & MASK0F] );
}
/* new line */
crlf() ;
}
/* show duty */
if ( cmd == 'S' ) { show_duty(); }
/* show sensor data */
if ( cmd == 'D' ) {
/* get sensor loacation */
tmp = get_hex( *(sbuf+1) ) - '0' ;
/* */
show_data( get_fpga(tmp) );
}
/* show register values */
if ( cmd == 'V' ) {
turn_off_led();
show_reg() ;
}
break ;
/* return first state */
case 2 : state = 0 ;
break ;
/* BLIND_RUN */
/* judge */
case 10 : /* show mode */
turn_on_led(LED_BLIND);
/* next state */
state = 11 ;
if ( bstate == 6 ) {
bstate = 0 ;
state = 20 ;
}
/* select CENTER */
xmotor.xfront = FRONT_CENTER ;
break ;
/* move straight */
case 11 : state = 12 ; /* next state */
xmotor.xrear = (REAR_FORWARD | *(bstate_pat+bstate)) ;
send_fpga( xmotor );
break ;
/* delay */
case 12 : state = 13 ; /* next state */
/* delay */
delay_ms(500) ;
break ;
/* return first state */
case 13 : state = 10 ;
/* increment */
bstate++ ;
break ;
/* NORMAL_RUN */
/* default */
case 20 : state = 21 ;
/* show mode */
turn_on_led(LED_NORMAL);
/* default */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 50) ;
break ;
/* judge */
case 21 : state = 22 ;
if ( is_tiny_right( sensorx ) == YES ) {
xmotor.xfront = (FRONT_RIGHT | 50);
xmotor.xrear = (REAR_FORWARD | 75);
}
if ( is_right( sensorx ) == YES ) {
xmotor.xfront = (FRONT_RIGHT | 75);
xmotor.xrear = (REAR_FORWARD | 55);
}
if ( is_big_right( sensorx ) == YES ) {
xmotor.xfront = (FRONT_RIGHT | 90);
xmotor.xrear = (REAR_FORWARD | 45);
}
if ( is_tiny_left( sensorx ) == YES ) {
xmotor.xfront = (FRONT_LEFT | 50);
xmotor.xrear = (REAR_FORWARD | 75);
}
if ( is_left( sensorx ) == YES ) {
xmotor.xfront = (FRONT_LEFT | 75);
xmotor.xrear = (REAR_FORWARD | 55);
}
if ( is_big_left( sensorx ) == YES ) {
xmotor.xfront = (FRONT_LEFT | 90);
xmotor.xrear = (REAR_FORWARD | 45);
}
send_fpga( xmotor ) ;
break ;
/* dalay and branch */
case 22 : state = 23 ;
delay_ms(10);
/* get CRANK mark */
if ( sensorx == ALL_WHITE ) {
white_state = WHITE_STATE_ALL ;
state = 30 ;
}
/* get LANE LEFT CHANGE mark */
if ( is_left_white(sensorx) == YES ) {
white_state = WHITE_STATE_LEFT ;
state = 40 ;
}
/* get LANE RIGHT CHANGE mark */
if ( is_right_white(sensorx) == YES ) {
white_state = WHITE_STATE_RIGHT ;
state = 40 ;
}
break ;
/* return first state */
case 23 : state = 20 ;
break ;
/* CRANK_RUN */
/* blind run until minimum distance */
case 30 : turn_on_led(LED_CRANK); /* show mode */
state = 31 ;
ccnt = 30 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga( xmotor );
break ;
/* delay */
case 31 : if ( ccnt == 0 ) {
state = 32 ;
} else {
ccnt-- ;
delay_ms(10) ;
}
break ;
/* until CRANK location */
case 32 : if ( is_left_white(sensorx) == YES ) {
state = 33 ;
white_state = WHITE_STATE_LEFT ;
}
if ( is_right_white(sensorx) == YES ) {
state = 33 ;
white_state = WHITE_STATE_RIGHT ;
}
break ;
/* judge turn location */
case 33 : if ( sensorx == ALL_WHITE || sensorx == ALL_BLACK ) {
state = 34 ;
}
break ;
/* turn */
case 34 : state = 35 ;
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 50);
}
if ( white_state == WHITE_STATE_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 50);
}
xmotor.xrear = (REAR_FORWARD | 50);
send_fpga( xmotor ) ;
break ;
/* judge center */
case 35 : if ( is_center(sensorx) == YES ) {
state = 36 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor ) ;
}
break ;
/* return NORMAL_RUN */
case 36 : state = 20 ;
break ;
/* LANE_CHANGE */
case 40 : turn_on_led(LED_LANE);
if ( sensorx == ALL_BLACK ) { state = 41 ; }
break ;
/* judge turn */
case 41 : state = 42 ;
lcnt = 10 ;
xmotor.xfront = (FRONT_RIGHT | 80);
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 80);
}
send_fpga( xmotor );
break ;
/* turn delay */
case 42 : if ( lcnt == 0 ) {
state = 43 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor );
} else {
lcnt-- ;
delay_ms(10);
}
break ;
/* blind run */
case 43 : state = 44 ;
lcnt = 10 ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga( xmotor );
break ;
/* blind run delay */
case 4 : if ( lcnt == 0 ) {
state = 45 ;
} else {
lcnt-- ;
delay_ms(10);
}
break ;
/* judge center */
case 45 : if ( is_center(sensorx) == YES ) { state = 46 ; }
break ;
/* judge turn (reverse) */
case 46 : state = 47 ;
lcnt = 10 ;
xmotor.xfront= (FRONT_LEFT | 80);
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_RIGHT | 80);
}
send_fpga( xmotor );
break ;
/* turn (reverse) delay */
case 47 : if ( lcnt == 0 ) {
state = 48 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga( xmotor );
} else {
lcnt-- ;
delay_ms(10);
}
break ;
/* return NORMAL_RUN */
case 48 : state = 20 ;
break ;
/* */
default : state = 0 ;
break ;
}
/* get data */
{
*(sensor+0) = get_fpga(SENX0) ;
*(sensor+1) = get_fpga(SENX1) ;
*(sensor+2) = get_fpga(SENX2) ;
*(sensor+3) = get_fpga(SENX3) ;
*(sensor+4) = get_fpga(SENX4) ;
/* load */
sensorx = *(sensor+4) ;
/* send trigger */
send_cam_trigger();
}
/* measure distance */
{
/* update */
pre_rcnt = cur_rcnt ;
/* get upper byte */
xmes = get_fpga(ROTU);
xmes <<= 8 ;
/* get lower byte */
xmes += get_fpga(ROTL);
/* update */
cur_rcnt = xmes ;
/* differencial pulse count */
dif_rcnt = cur_rcnt - pre_rcnt ;
/* judge */
if ( cur_rcnt == MAX_RUN_PULSE ) {
/* stop RUN */
state = 0 ;
/* stop motors */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = REAR_FORWARD ;
send_fpga( xmotor );
}
}
/* shift flashing */
if ( tflag == ON ) {
/* clear flag */
tflag = OFF ;
/* judge */
if ( state < 10 ) {
/* impress */
turn_on_led( cyclic[fstate] );
/* update state */
fstate++ ;
/* judge */
if ( fstate == 6 ) { fstate = 0 ; }
}
}
}
}
void IRQ_Handler(void) __irq
{
volatile UBYTE ch ;
/* judge UART receive interruption */
if ( (IRQSTA & UART_BIT) == UART_BIT ) {
/* judge */
if ( COMSTA0 & 1 ) {
/* clear flag */
ch = COMRX ;
*(sbuf+sindex) = ch ;
sindex++ ;
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
/* judge timer0 interruption (100us) */
if ( (IRQSTA & RTOS_TIMER_BIT) == RTOS_TIMER_BIT ) {
/* clear timer0 interrupt flag */
T0CLRI = 0xff ;
/* increment */
timcnt++ ;
/* judge */
if ( (timcnt & 0x7f) == 0 ) {
icount++ ;
/* get data */
str_sft <<= 1 ;
str_sft &= MASK07 ;
if ( GP0DAT & MASK20 ) { str_sft |= ON ; }
/* judge */
if ( str_sft == 3 ) { strg = ON ; }
}
if ( (timcnt & 0xfff) == 0 ) {
GP4DAT ^= (1 << 23);
tflag = ON ;
}
/* judge time up */
if ( icount == ILAST ) {
icount = 0 ;
state = 0 ;
xmotor.xfront = 0 ;
xmotor.xrear = REAR_STOP ;
send_fpga( xmotor );
}
}
}
void init_usr(void)
{
/* select clock 10.44MHz
initialized in start up routine
*/
PLLKEY1 = 0xaa ;
PLLCON = 0x01 ;
PLLKEY2 = 0x55 ;
/* power control
initialized in start up routine
*/
/* clear flag */
uflag = OFF ;
str_trg = OFF ;
vtrg = OFF ;
ptrg = OFF ;
/* clear counter */
sindex = 0 ;
state = 0 ;
vscnt = 0 ;
str_sft = 0 ;
icount = 0 ;
white_state = WHITE_STATE_NONE ;
bstate = 0 ; /* blinde run */
ccnt = 0 ; /* crank run */
lcnt = 0 ; /* lane change */
fstate = 0 ; /* flashing LED */
/* initialize UART */
{
/* set baud rate 19200 bps CD = 2 */
COMCON0 = 0x80 ; /* select COMDIV1 and COMDIV0 */
COMDIV0 = 0x11 ;
COMDIV1 = 0x00 ;
/* set conditions */
COMCON0 = 0x03 ; /* select COMRX and COMTX , 8bit data , 1 stop bit , no parity */
/* enable interrupt */
COMIEN0 = 0x01 ; /* ERBFI */
}
/* P0 */
{
/* use GPIO */
GP0CON = 0x00000000 ;
/* */
GP0DAT = 0xDF1F0000 ;
}
/* P1 */
{
/* use UART */
GP1CON = 0x00000011 ;
/* */
GP1DAT = 0xfef00000 ;
}
/* P2 */
{
/* all bits outputs */
GP2DAT = 0xff000000 ;
}
/* P3 */
{
/* all bits outputs */
GP3DAT = 0xff000000 ;
}
/* P4 */
{
GP4DAT = 0xe3030000 ;
}
/* initialize timer 0 (10kHz) */
{
T0LD = 1044 ; /* (10.44MHz / 1) / 10kHz */
T0CON = 0xc0 ; /* enable , cyclic , 1/1 */
}
timcnt = 0 ;
fduty = 0 ;
rduty = 0 ;
fdir = 0 ;
rdir = 0 ;
/* clear sensor data */
*(sensor+0) = 0 ;
*(sensor+1) = 0 ;
*(sensor+2) = 0 ;
*(sensor+3) = 0 ;
*(sensor+4) = 0 ;
send_cam_trigger();
/* clear rotary counter */
cur_rcnt = 0 ;
pre_rcnt = 0 ;
dif_rcnt = cur_rcnt - pre_rcnt ;
/* enable timer 0 and UART interrupt */
IRQEN = RTOS_TIMER_BIT | UART_BIT ;
}
/* UART putchar */
void rs_putchar(UBYTE x)
{
/* ? transmmit buffer empty */
while( (COMSTA0 & 0x40) == 0 ) ;
/* set value */
COMTX = x ;
}
/* carriage return and line feed */
void crlf(void)
{
rs_putchar('\r');
rs_putchar('\n');
}
/* UART puts */
void rs_puts(UBYTE *x)
{
while ( *x != '\0' ) {
rs_putchar( *x ) ;
x++ ;
}
}
/* convert ASCII to number */
UBYTE get_hex(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
/* show help */
void show_help(void)
{
rs_puts("? help") ; crlf();
rs_puts("I initialize camera") ; crlf();
rs_puts("D show data") ; crlf();
rs_puts("F set front duty and direction") ; crlf();
rs_puts("R set rear duty and direction") ; crlf();
rs_puts("P pulse count") ; crlf();
rs_puts("S show duty ratio") ; crlf();
rs_puts("V show registers") ; crlf();
}
void show_reg(void)
{
UBYTE i ;
volatile UBYTE tmp ;
volatile UBYTE msg[2] ;
/* horizontal ruler */
for ( i = 0 ; i < 4 ; i++ ) { rs_putchar(' ') ; }
/* 0 -> F */
for ( i = 0 ; i < 16 ; i++ ) {
rs_putchar('+');
rs_putchar( asc_hex[i] ) ;
rs_putchar(' ');
}
/* new line */
crlf();
/* show */
for ( i = 0 ; i < 208 ; i++ ) {
/* vertical ruler */
if ( (i % 16) == 0 ) {
rs_putchar('+');
rs_putchar( asc_hex[ i >> 4 ] ) ;
rs_putchar('0');
rs_putchar(' ');
}
/* get register data */
tmp = load_cam(i) ;
/* separate */
*(msg+0) = ((tmp >> 4) & MASK0F) ;
*(msg+1) = (tmp & MASK0F) ;
/* show */
rs_putchar( asc_hex[*(msg+0)] ) ;
rs_putchar( asc_hex[*(msg+1)] ) ;
/* add space */
rs_putchar(' ');
/* new line */
tmp = i & MASK0F ;
if ( tmp == 15 ) { crlf(); }
}
}
void show_sensor(UBYTE x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar('0'+((x >> i) & 1));
}
crlf();
}
#define SCCB_DELAY_CNT 1
void put_sccb_start(void)
{
/* both high level */
GP4DAT |= (1 << S_SCL) | (1 << S_SDA) ; /* SCL = ON ; SDA = ON */
delay_100us(SCCB_DELAY_CNT);
/* 0 -> SDA */
GP4DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* 0 -> SCL */
GP4DAT &= ~(1 << S_SCL) ; /* SCL = OFF ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
}
void put_sccb_stop(void)
{
/* both low level */
GP4DAT &= ~((1 << S_SCL) | (1 << S_SDA)) ; /* SCL = OFF ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 << S_SCL) ; /* SCL = ON ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* both high level */
GP4DAT |= (1 << S_SDA) ; /* SCL = ON ; SDA = ON */
delay_100us(SCCB_DELAY_CNT);
}
void put_sccb_data(UBYTE x)
{
volatile UBYTE tmp ;
volatile UBYTE i ;
/* */
tmp = x ;
/* transfer data with write code */
for ( i = 0 ; i < 8 ; i++ ) {
/* send bit datum */
GP4DAT &= ~(1 << S_SDA) ;
if ( tmp & MASK80 ) { GP0DAT |= (1 << S_SDA) ; }
/* dummy */
delay_100us(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* shift */
tmp <<= 1 ;
/* 0 -> SCL */
GP4DAT &= ~(1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
}
/* change input */
GP4DAT &= ~(1 << 24) ; delay_sccb(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* get acknowledge */
tmp = OFF ;
if ( (GP4DAT & MASK01) ) { tmp |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* change output */
GP4DAT |= (1 << 24) ;
/* delay */
delay_100us(5);
}
UBYTE get_sccb_data(void)
{
volatile UBYTE tmp ;
volatile UBYTE i ;
/* change inupt */
GP4DAT &= ~(1 << 24) ;
/* default */
tmp = 0 ;
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* shift */
tmp <<= 1 ;
/* 1 -> SCL */
GP4DAT |= (1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* get bit datum */
if ( (GP4DAT & MASK01) ) { tmp |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
}
/* 1 -> SCL */
GP4DAT |= (1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* dummy shift */
i = OFF ;
if ( (GP4DAT & MASK01) ) { i |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 << S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* change output */
GP4DAT |= (1 << 24) ;
return tmp ;
}
void send_cam(UBYTE adr,UBYTE dat)
{
/* start conditon */
put_sccb_start() ;
/* send ID */
put_sccb_data(SCCB_ID_WR);
/* send address */
put_sccb_data(adr);
/* send parameter */
put_sccb_data(dat);
/* stop condition */
put_sccb_stop();
/* delay */
delay_100us(SCCB_DELAY_CNT);
}
UBYTE load_cam(UBYTE adr)
{
UBYTE result ;
/* start conditon */
put_sccb_start() ;
/* send ID (write) */
put_sccb_data(SCCB_ID_WR);
/* send address */
put_sccb_data(adr);
/* stop condition */
put_sccb_stop();
/* start conditon */
put_sccb_start() ;
/* send ID (read) */
put_sccb_data(SCCB_ID_RD);
/* get parameter */
result = get_sccb_data();
/* stop condition */
put_sccb_stop();
return result ;
}
void delay_sccb(UBYTE x)
{
UBYTE i ;
for ( i = 0 ; i < x ; i++ ) ;
}
void delay_100us(UWORD x)
{
ULONG last ;
/* calculate */
last = timcnt + x ;
/* wait */
while ( timcnt < last ) ;
}
void delay_ms(UWORD x)
{
ULONG last ;
/* calculate */
last = timcnt + 10 * x ;
/* wait */
while ( timcnt < last ) ;
}
void init_cam(UBYTE x)
{
/* reset camera */
send_cam(REG_COM7,COM7_RESET);
rs_puts("Complete reset"); crlf();
/* delay 200ms */
delay_ms(200);
send_cam(REG_COM7,0x00);
//send_cam(REG_RGB444,R444_ENABLE | R444_RGBX); //0x8c:RGB 444 control
send_cam(REG_COM1,0x40); //0x04:COM1(CCIR656,AEC) //0) //0x40)
send_cam(REG_COM15,COM15_R01FE|COM15_RGB565); //0x40:COM15
send_cam(REG_COM9, 0x38); //0x14:COM9=max AGC gain ceiling, Freeze AGC/AEC
//c(0x3d,0xc3) //(REG_COM13, COM13_GAMMA|COM13_UVSAT|0x2) //0x3d:COM13
send_cam(REG_HAECC7,0x94); //0xaa:Hist AEC/AGC control 7 c(0xAA,0x94) //AEC algorithm
send_cam(REG_TSLB,0x04); //0x3a:Neg,UVval,YUYV,window TSLB_YLAST) //0x04) //0x0C) //0x80) //0x00) //0x04)
send_cam(0x20,0x0f); //ADCCTR0, A/D range&ref, mu0102
send_cam(REG_COM8,0x9a); //mu0103
send_cam(0x10,0x01); //mu0103
if ( x ) {
/* CLKRC divide /4 -> 5MHz */
send_cam(0x11,0x83);
}
/* COM1 */
send_cam(0x3b,0x0a); /* disable night mode */
/* TSLB
3 bit => select YUV format
*/
send_cam(0x3a,0x05);
/* COM7
2 bit , 0 bit => select YUV format
*/
send_cam(REG_COM7,0x00);
/* COM15 */
send_cam(0x40,0xd0);
/* HSTART */
send_cam(0x17,0x16);
/* HSTOP */
send_cam(0x18,0x04);
/* HREF */
send_cam(0x32,0x24);
/* VSTRT */
send_cam(0x19,0x02);
/* VSTOP */
send_cam(0x1a,0x7a);
/* VREF */
send_cam(0x03,0x0a);
/* COM10 */
send_cam(0x15,0x02);
/* COM3 */
send_cam(0x0c,0x04);
/* COM4 */
send_cam(0x3e,0x1a);
/* SCALING_DCWCTR */
send_cam(0x72,0x22);
/* SCALING_PCLK_DIV */
send_cam(0x73,0xf2);
/* DM_LNL */
send_cam(0x92,0x00);
/* DM_LNH */
send_cam(0x93,0x00);
/* decrease frame rate
put dummy lines 510 (0x01FE)
*/
if ( x ) {
rs_puts("insert dummy line");
crlf();
/* DM_LNL */
send_cam(0x92,0xFE);
/* DM_LNH */
send_cam(0x93,0x01);
}
rs_puts("Exit initialize"); crlf();
}
void send_fpga(MOTORP x)
{
volatile UBYTE loop;
volatile UBYTE i;
volatile UBYTE dir ;
volatile UBYTE dat ;
for ( loop = 0 ; loop < 2 ; loop++ ) {
/* get front direction and dat */
dir = (x.xfront >> 8) & MASK03 ;
dat = x.xfront & MASKFF ;
if ( loop ) {
dir = (x.xrear >> 8) & MASK03 ;
dat = x.xrear & MASKFF ;
}
/* impress data */
GP3DAT &= 0xff00ffff ;
GP3DAT |= (dat << 16) ;
/* impress direction */
GP2DAT &= 0xff07ffff ; /* DIR = 00 , OE = 0 */
GP2DAT |= (dir << S_DIR);
/* send latch pulse */
GP2DAT |= (1 << S_TRG); /* trigger H */
for ( i = 0 ; i < 10 ; i++ ); /* delay */
GP2DAT &= ~(1 << S_TRG); /* trigger L */
}
}
UBYTE get_fpga(UBYTE lx)
{
UBYTE result ;
/* send line number */
GP2DAT &= 0xfff0ffff ;
GP2DAT |= (lx << 16) ;
/* GP3 inputs */
GP3DAT &= 0x00ffffff ;
/* enable FPGA output */
GP2DAT |= (1 << S_OE) ;
/* data */
result = GP3DAT & MASKFF ;
/* disable FPGA output */
GP2DAT &= ~(1 << S_OE) ;
/* GP3 outputs */
GP3DAT |= 0xff000000 ;
return result ;
}
UBYTE get_sensor(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( x < 5 ) { result = get_fpga(x) ; }
return result ;
}
void send_cam_trigger(void)
{
GP2DAT |= (1 << S_CTRG);
GP2DAT &= ~(1 << S_CTRG);
}
void show_duty(void)
{
UBYTE i ;
UBYTE msg[7] ;
/* clear buffer */
for ( i = 0 ; i < 7 ; i++ ) { *(msg+i) = '\0' ; }
*(msg+1) = ' ' ;
*(msg+4) = ' ' ;
/* front */
*(msg+0) = 'F' ;
*(msg+2) = (fduty / 10) + '0' ;
*(msg+3) = (fduty % 10) + '0' ;
*(msg+5) = fdir + '0' ;
rs_puts( msg );
crlf();
/* rear */
*(msg+0) = 'R' ;
*(msg+2) = (rduty / 10) + '0' ;
*(msg+3) = (rduty % 10) + '0' ;
*(msg+5) = rdir + '0' ;
rs_puts( msg );
crlf();
}
void show_data(UBYTE x)
{
int i ;
UBYTE tmp ;
/* */
tmp = x ;
/* show */
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar( ((tmp >> i) & 1) + '0');
}
crlf();
}
UBYTE is_center(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x18 ) { result = YES ; }
if ( x == 0x38 ) { result = YES ; }
if ( x == 0x1C ) { result = YES ; }
if ( x == 0x3C ) { result = YES ; }
return result ;
}
UBYTE is_left_white(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0xF0 ) { result = YES ; }
if ( x == 0xF8 ) { result = YES ; }
if ( x == 0xFC ) { result = YES ; }
return result ;
}
UBYTE is_right_white(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x0F ) { result = YES ; }
if ( x == 0x1F ) { result = YES ; }
if ( x == 0x3F ) { result = YES ; }
return result ;
}
UBYTE is_tiny_right(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x0C ) { result = YES ; }
return result ;
}
UBYTE is_right(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x06 ) { result = YES ; }
if ( x == 0x0E ) { result = YES ; }
return result ;
}
UBYTE is_big_right(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x03 ) { result = YES ; }
if ( x == 0x07 ) { result = YES ; }
return result ;
}
UBYTE is_tiny_left(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x30 ) { result = YES ; }
return result ;
}
UBYTE is_left(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0x60 ) { result = YES ; }
if ( x == 0x70 ) { result = YES ; }
return result ;
}
UBYTE is_big_left(UBYTE x)
{
UBYTE result ;
/* default */
result = NO ;
/* judge */
if ( x == 0xC0 ) { result = YES ; }
if ( x == 0xE0 ) { result = YES ; }
return result ;
}
/* turn off all LEDs */
void turn_off_led(void)
{
GP1DAT |= 0x00f00000 ;
}
/* turn on target LED */
void turn_on_led(UBYTE x)
{
/* judge */
if ( x > LED_LANE ) { return ; }
/* turn off all LEDs */
turn_off_led();
/* turn on target LED */
GP1DAT &= ~(1 << (x+20));
}
このファームウエアで一度動作確認後、RTOSを利用
したファームウエアを書き換えました。
#include <ADuC7026.h>
#define OFF 0
#define ON OFF+1
#define NO 0
#define YES NO+1
/* data definitions */
typedef unsigned char UBYTE ;
typedef signed char SBYTE ;
typedef unsigned short UWORD ;
typedef signed short SWORD ;
typedef unsigned long ULONG ;
typedef signed long SLONG ;
void IRQ_Handler(void) __irq;
void init_usr(void);
#define MASKFF 0xFF
#define MASK0F 0x0F
#define MASK80 0x80
#define MASK40 0x40
#define MASK20 0x20
#define MASK10 0x10
#define MASK08 0x08
#define MASK04 0x04
#define MASK02 0x02
#define MASK03 0x03
#define MASK01 0x01
#define MASKF0 0xF0
#define MASK07 0x07
#define SCCB_ID_WR 0x42
#define SCCB_ID_RD 0x43
#define S_SCL 17
#define S_SDA 16
#define S_CTRG 23
#define S_TRG 22
#define S_DIR 20
#define S_OE 19
#define TSK_ID_MAX 9
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TSK_ID4 4
#define TSK_ID5 5
#define TSK_ID6 6
#define TSK_ID7 7
#define TSK_ID8 8
typedef struct {
void (*tsk)(void);
UWORD wcount ;
} TCBP ;
TCBP tcb[TSK_ID_MAX];
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
volatile UWORD ready ;
volatile UWORD suspend;
volatile UWORD waitq ;
volatile UWORD run_tsk;
volatile UBYTE tflag ;
volatile UWORD tsk_state[3] ;
/*-----------------------*/
/* system call prototype */
/*-----------------------*/
void init_os(void);
void cre_tsk(UBYTE tid,void (*tsk)(void));
void sta_tsk(UBYTE tid,UBYTE sta);
void rsm_tsk(UBYTE tid);
void sus_tsk(UBYTE tid);
void slp_tsk(void);
void wai_tsk(UWORD x);
UBYTE is_tsk_ready(UBYTE tid);
void timer_handler(void);
void push_tsk_state(void);
void pop_tsk_state(void);
void rs_putchar(UBYTE x);
void crlf(void);
void rs_puts(UBYTE *x);
UBYTE get_hex(UBYTE x);
void show_help(void);
void init_cam(UBYTE x);
void put_sccb_start(void);
void put_sccb_stop(void);
void put_sccb_data(UBYTE x);
UBYTE get_sccb_data(void);
UBYTE load_cam(UBYTE adr);
void show_reg(void);
void show_sensor(UBYTE x);
void send_cam(UBYTE adr,UBYTE dat);
void delay_sccb(UBYTE x);
void delay_100us(UWORD x);
void delay_ms(UWORD x);
/*---------------*/
/* FPGA handling */
/*---------------*/
typedef struct {
UWORD xfront ;
UWORD xrear ;
} MOTORP ;
typedef struct {
UBYTE topx ;
UBYTE midx ;
UBYTE bottomx ;
} LINEP ;
typedef struct {
UBYTE topx ;
UBYTE midx ;
UBYTE bottomx ;
UBYTE top_dat ;
UBYTE mid_dat ;
UBYTE bottom_dat ;
} SJP ;
LINEP tline ;
MOTORP xmotor ;
SJP xsjp ;
void send_fpga_duty(MOTORP x);
void send_fpga_line(LINEP x);
UBYTE get_fpga(UBYTE lx);
void show_duty(void);
void show_data(UBYTE x);
UBYTE get_sensor(UBYTE x);
void send_cam_trigger(void);
void turn_off_led(void);
void turn_on_led(UBYTE x);
UBYTE perform_judge(UBYTE x);
void perform_sensing(SJP x);
/* global variables */
volatile UBYTE uflag ;
volatile UBYTE sbuf[8] ;
volatile UBYTE sindex ;
volatile UBYTE cmd ;
volatile UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
volatile ULONG timcnt ;
volatile UBYTE state ;
volatile UWORD vscnt ;
volatile UBYTE vtrg ;
volatile UBYTE ptrg ;
volatile UBYTE str_sft ;
volatile UBYTE str_trg ;
volatile UBYTE fduty ;
volatile UBYTE rduty ;
volatile UBYTE fdir ;
volatile UBYTE rdir ;
volatile UWORD icount ;
#define ILAST 30000
#define S_MODE_BIT 18
#define REG_GAIN 0x00 /* Gain lower 8 bits (rest in vref) */
#define REG_BLUE 0x01 /* blue gain */
#define REG_RED 0x02 /* red gain */
#define REG_VREF 0x03 /* Pieces of GAIN, VSTART, VSTOP */
#define REG_COM1 0x04 /* Control 1 */
#define COM1_CCIR656 0x40 /* CCIR656 enable */
#define REG_BAVE 0x05 /* U/B Average level */
#define REG_GbAVE 0x06 /* Y/Gb Average level */
#define REG_AECHH 0x07 /* AEC MS 5 bits */
#define REG_RAVE 0x08 /* V/R Average level */
#define REG_COM2 0x09 /* Control 2 */
#define COM2_SSLEEP 0x10 /* Soft sleep mode */
#define REG_PID 0x0a /* Product ID MSB */
#define REG_VER 0x0b /* Product ID LSB */
#define REG_COM3 0x0c /* Control 3 */
#define COM3_SWAP 0x40 /* Byte swap */
#define COM3_SCALEEN 0x08 /* Enable scaling */
#define COM3_DCWEN 0x04 /* Enable downsamp/crop/window */
#define REG_COM4 0x0d /* Control 4 */
#define REG_COM5 0x0e /* All "reserved" */
#define REG_COM6 0x0f /* Control 6 */
#define REG_AECH 0x10 /* More bits of AEC value */
#define REG_CLKRC 0x11 /* Clocl control */
#define CLK_EXT 0x40 /* Use external clock directly */
#define CLK_SCALE 0x3f /* Mask for internal clock scale */
#define REG_COM7 0x12 /* Control 7 */
#define COM7_RESET 0x80 /* Register reset */
#define COM7_FMT_MASK 0x38
#define COM7_FMT_VGA 0x00
#define COM7_FMT_CIF 0x20 /* CIF format */
#define COM7_FMT_QVGA 0x10 /* QVGA format */
#define COM7_FMT_QCIF 0x08 /* QCIF format */
#define COM7_RGB 0x04 /* bits 0 and 2 - RGB format */
#define COM7_YUV 0x00 /* YUV */
#define COM7_BAYER 0x01 /* Bayer format */
#define COM7_PBAYER 0x05 /* "Processed bayer" */
#define REG_COM8 0x13 /* Control 8 */
#define COM8_FASTAEC 0x80 /* Enable fast AGC/AEC */
#define COM8_AECSTEP 0x40 /* Unlimited AEC step size */
#define COM8_BFILT 0x20 /* Band filter enable */
#define COM8_AGC 0x04 /* Auto gain enable */
#define COM8_AWB 0x02 /* White balance enable */
#define COM8_AEC 0x01 /* Auto exposure enable */
#define REG_COM9 0x14 /* Control 9 - gain ceiling */
#define REG_COM10 0x15 /* Control 10 */
#define COM10_HSYNC 0x40 /* HSYNC instead of HREF */
#define COM10_PCLK_HB 0x20 /* Suppress PCLK on horiz blank */
#define COM10_HREF_REV 0x08 /* Reverse HREF */
#define COM10_VS_LEAD 0x04 /* VSYNC on clock leading edge */
#define COM10_VS_NEG 0x02 /* VSYNC negative */
#define COM10_HS_NEG 0x01 /* HSYNC negative */
#define REG_HSTART 0x17 /* Horiz start high bits */
#define REG_HSTOP 0x18 /* Horiz stop high bits */
#define REG_VSTART 0x19 /* Vert start high bits */
#define REG_VSTOP 0x1a /* Vert stop high bits */
#define REG_PSHFT 0x1b /* Pixel delay after HREF */
#define REG_MIDH 0x1c /* Manuf. ID high */
#define REG_MIDL 0x1d /* Manuf. ID low */
#define REG_MVFP 0x1e /* Mirror / vflip */
#define MVFP_MIRROR 0x20 /* Mirror image */
#define MVFP_FLIP 0x10 /* Vertical flip */
#define REG_AEW 0x24 /* AGC upper limit */
#define REG_AEB 0x25 /* AGC lower limit */
#define REG_VPT 0x26 /* AGC/AEC fast mode op region */
#define REG_HSYST 0x30 /* HSYNC rising edge delay */
#define REG_HSYEN 0x31 /* HSYNC falling edge delay */
#define REG_HREF 0x32 /* HREF pieces */
#define REG_TSLB 0x3a /* lots of stuff */
#define TSLB_YLAST 0x04 /* UYVY or VYUY - see com13 */
#define REG_COM11 0x3b /* Control 11 */
#define COM11_NIGHT 0x80 /* NIght mode enable */
#define COM11_NMFR 0x60 /* Two bit NM frame rate */
#define COM11_HZAUTO 0x10 /* Auto detect 50/60 Hz */
#define COM11_50HZ 0x08 /* Manual 50Hz select */
#define COM11_EXP 0x02
#define REG_COM12 0x3c /* Control 12 */
#define COM12_HREF 0x80 /* HREF always */
#define REG_COM13 0x3d /* Control 13 */
#define COM13_GAMMA 0x80 /* Gamma enable */
#define COM13_UVSAT 0x40 /* UV saturation auto adjustment */
#define COM13_UVSWAP 0x01 /* V before U - w/TSLB */
#define REG_COM14 0x3e /* Control 14 */
#define COM14_DCWEN 0x10 /* DCW/PCLK-scale enable */
#define REG_EDGE 0x3f /* Edge enhancement factor */
#define REG_COM15 0x40 /* Control 15 */
#define COM15_R10F0 0x00 /* Data range 10 to F0 */
#define COM15_R01FE 0x80 /* 01 to FE */
#define COM15_R00FF 0xc0 /* 00 to FF */
#define COM15_RGB565 0x10 /* RGB565 output */
#define COM15_RGB555 0x30 /* RGB555 output */
#define REG_COM16 0x41 /* Control 16 */
#define COM16_AWBGAIN 0x08 /* AWB gain enable */
#define REG_COM17 0x42 /* Control 17 */
#define COM17_AECWIN 0xc0 /* AEC window - must match COM4 */
#define COM17_CBAR 0x08 /* DSP Color bar */
#define REG_RGB444 0x8c /* RGB 444 control */
#define R444_ENABLE 0x02 /* Turn on RGB444, overrides 5x5 */
#define R444_RGBX 0x01 /* Empty nibble at end */
#define REG_HAECC1 0x9f /* Hist AEC/AGC control 1 */
#define REG_HAECC2 0xa0 /* Hist AEC/AGC control 2 */
#define REG_HAECC7 0xaa /* Hist AEC/AGC control 7 */
#define FRONT_CENTER 0x000
#define FRONT_RIGHT 0x100
#define FRONT_LEFT 0x200
#define REAR_STOP 0x080
#define REAR_FORWARD 0x180
#define REAR_REVERSE 0x280
#define STATE_WAIT 0
#define STATE_NORMAL 1
#define STATE_CRANK 2
#define STATE_LANEC 3
#define SSTART TSK_ID0
#define BLIND_RUN TSK_ID1
#define NORMAL_RUN TSK_ID2
#define CRANK_RUN TSK_ID3
#define LANE_CHANGE TSK_ID4
#define SENSING TSK_ID5
#define MEASURE TSK_ID6
#define SDEBUG TSK_ID7
#define LEDFLASH TSK_ID8
#define WHITE_STATE_NONE 0
#define WHITE_STATE_RIGHT 1
#define WHITE_STATE_LEFT 2
#define WHITE_STATE_ALL 3
#define ALL_BLACK 0x00
#define ALL_WHITE 0xff
#define STATE_ALL_BLACK 0
#define STATE_BIG_RIGHT 1
#define STATE_RIGHT 2
#define STATE_TINY_RIGHT 3
#define STATE_CENTER 4
#define STATE_TINY_LEFT 5
#define STATE_LEFT 6
#define STATE_BIG_LEFT 7
#define STATE_RIGHT_WHITE 8
#define STATE_LEFT_WHITE 9
#define STATE_ALL_WHITE 10
#define FD_ADR 0
#define FP_ADR 1
#define RD_ADR 2
#define RP_ADR 3
#define SENX0 8
#define SENX1 0
#define SENX2 10
#define LTOP 11
#define LMID 12
#define LBOT 13
#define ROTU 14
#define ROTL 15
#define LED_BLIND 0
#define LED_NORMAL 1
#define LED_CRANK 2
#define LED_LANE 3
#define LED_NONE 4
volatile system_status ;
volatile UBYTE bstate ;
volatile UBYTE cstate ;
volatile UBYTE ccnt ;
volatile UBYTE lstate ;
volatile UBYTE lcnt ;
volatile UBYTE sensor[3] ;
volatile UBYTE direction ;
volatile UBYTE white_state ;
volatile UWORD cur_rcnt ;
volatile UWORD pre_rcnt ;
volatile UWORD dif_rcnt ;
volatile UBYTE cyclic[6] = {LED_BLIND,LED_NORMAL,LED_CRANK,LED_LANE,LED_CRANK,LED_NORMAL};
volatile UBYTE fstate ;
void tsk0_proc(void);
void tsk1_proc(void);
void tsk2_proc(void);
void tsk3_proc(void);
void tsk4_proc(void);
void tsk5_proc(void);
void tsk6_proc(void);
void tsk7_proc(void);
void tsk8_proc(void);
#define IDLE 0
#define RUN IDLE+1
volatile UBYTE duty ;
int main(void)
{
volatile TCBP pcur_tsk ;
/* initialize user */
init_usr();
/* */
init_os();
/* regist task */
cre_tsk(SSTART ,tsk0_proc);
cre_tsk(BLIND_RUN ,tsk1_proc);
cre_tsk(NORMAL_RUN ,tsk2_proc);
cre_tsk(CRANK_RUN ,tsk3_proc);
cre_tsk(LANE_CHANGE,tsk4_proc);
cre_tsk(SENSING ,tsk5_proc);
cre_tsk(MEASURE ,tsk6_proc);
cre_tsk(SDEBUG ,tsk7_proc);
cre_tsk(LEDFLASH ,tsk8_proc);
/* initialize task states */
sta_tsk(SSTART ,TTS_READY);
sta_tsk(BLIND_RUN ,TTS_SUSPEND);
sta_tsk(NORMAL_RUN ,TTS_SUSPEND);
sta_tsk(CRANK_RUN ,TTS_SUSPEND);
sta_tsk(LANE_CHANGE,TTS_SUSPEND);
sta_tsk(SENSING ,TTS_READY);
sta_tsk(MEASURE ,TTS_READY);
sta_tsk(SDEBUG ,TTS_READY);
sta_tsk(LEDFLASH ,TTS_READY);
/* show message */
rs_puts("Hello"); crlf();
/* endless loop */
while (ON)
{
/* get target task state */
pcur_tsk = tcb[run_tsk] ;
/* perform task */
if ( is_tsk_ready( run_tsk ) == YES ) { (*(pcur_tsk.tsk))(); }
/* change next task */
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
/* timer handling */
if ( tflag ) {
/* clear flag */
tflag = OFF ;
/* update */
timer_handler();
}
}
/* dummy return */
return (0);
}
void IRQ_Handler(void) __irq
{
volatile UBYTE ch ;
/* judge UART receive interruption */
if ( (IRQSTA & UART_BIT) == UART_BIT ) {
/* judge */
if ( COMSTA0 & 1 ) {
/* clear flag */
ch = COMRX ;
*(sbuf+sindex) = ch ;
sindex++ ;
if ( ch == '\r' ) {
sindex = 0 ;
uflag = ON ;
}
}
}
/* judge timer0 interruption (100us) */
if ( (IRQSTA & RTOS_TIMER_BIT) == RTOS_TIMER_BIT ) {
/* clear timer0 interrupt flag */
T0CLRI = 0xff ;
/* increment */
timcnt++ ;
/* judge */
if ( (timcnt & 0x7f) == 0 ) { tflag = ON ; icount++ ;}
if ( (timcnt & 0xfff) == 0 ) { GP4DAT ^= (1 &tl;&tl; 23); }
/* judge time up */
if ( icount == ILAST ) {
icount = 0 ;
state = 0 ;
xmotor.xfront = 0 ;
xmotor.xrear = REAR_STOP ;
send_fpga_duty( xmotor );
}
}
}
void init_usr(void)
{
/* select clock 10.44MHz
initialized in start up routine
*/
PLLKEY1 = 0xaa ;
PLLCON = 0x01 ;
PLLKEY2 = 0x55 ;
/* power control
initialized in start up routine
*/
/* clear flag */
uflag = OFF ;
str_trg = OFF ;
vtrg = OFF ;
ptrg = OFF ;
/* clear counter */
sindex = 0 ;
state = 0 ;
vscnt = 0 ;
str_sft = 0 ;
icount = 0 ;
white_state = WHITE_STATE_NONE ;
bstate = 0 ; /* blinde run */
//nstate = 0 ; /* normal run */
cstate = 0 ; /* crank run */
ccnt = 0 ;
lstate = 0 ; /* lane change */
lcnt = 0 ;
fstate = 0 ; /* flashing LED */
/* initialize UART */
{
/* set baud rate 19200 bps CD = 2 */
COMCON0 = 0x80 ; /* select COMDIV1 and COMDIV0 */
COMDIV0 = 0x11 ;
COMDIV1 = 0x00 ;
/* set conditions */
COMCON0 = 0x03 ; /* select COMRX and COMTX , 8bit data , 1 stop bit , no parity */
/* enable interrupt */
COMIEN0 = 0x01 ; /* ERBFI */
}
/* P0 */
{
/* use GPIO */
GP0CON = 0x00000000 ;
/* */
GP0DAT = 0xDF1F0000 ;
}
/* P1 */
{
/* use UART */
GP1CON = 0x00000011 ;
/* */
GP1DAT = 0xfef00000 ;
}
/* P2 */
{
/* all bits outputs */
GP2DAT = 0xff000000 ;
}
/* P3 */
{
/* all bits outputs */
GP3DAT = 0xff000000 ;
}
/* P4 */
{
GP4DAT = 0xe3030000 ;
}
/* initialize timer 0 (10kHz) */
{
T0LD = 1044 ; /* (10.44MHz / 1) / 10kHz */
T0CON = 0xc0 ; /* enable , cyclic , 1/1 */
}
timcnt = 0 ;
fduty = 0 ;
rduty = 0 ;
fdir = 0 ;
rdir = 0 ;
/* clear sensor data */
*(sensor+0) = 0 ;
*(sensor+1) = 0 ;
*(sensor+2) = 0 ;
send_cam_trigger();
/* clear rotary counter */
cur_rcnt = 0 ;
pre_rcnt = 0 ;
dif_rcnt = cur_rcnt - pre_rcnt ;
/* target line */
tline.topx = 10 ;
tline.midx = 30 ;
tline.bottomx = 50 ;
send_fpga_line(tline);
/* enable timer 0 and UART interrupt */
IRQEN = RTOS_TIMER_BIT | UART_BIT ;
}
/* UART putchar */
void rs_putchar(UBYTE x)
{
/* ? transmmit buffer empty */
while( (COMSTA0 & 0x40) == 0 ) ;
/* set value */
COMTX = x ;
}
/* carriage return and line feed */
void crlf(void)
{
rs_putchar('\r');
rs_putchar('\n');
}
/* UART puts */
void rs_puts(UBYTE *x)
{
while ( *x != '\0' ) {
rs_putchar( *x ) ;
x++ ;
}
}
/* convert ASCII to number */
UBYTE get_hex(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
/* show help */
void show_help(void)
{
rs_puts("? help") ; crlf();
rs_puts("I initialize camera") ; crlf();
rs_puts("D show data") ; crlf();
rs_puts("F set front duty and direction") ; crlf();
rs_puts("R set rear duty and direction") ; crlf();
rs_puts("P pulse count") ; crlf();
rs_puts("S show duty ratio") ; crlf();
rs_puts("V show registers") ; crlf();
}
void show_reg(void)
{
UBYTE i ;
volatile UBYTE tmp ;
volatile UBYTE msg[2] ;
/* horizontal ruler */
for ( i = 0 ; i &tl; 4 ; i++ ) { rs_putchar(' ') ; }
/* 0 -> F */
for ( i = 0 ; i &tl; 16 ; i++ ) {
rs_putchar('+');
rs_putchar( asc_hex[i] ) ;
rs_putchar(' ');
}
/* new line */
crlf();
/* show */
for ( i = 0 ; i &tl; 208 ; i++ ) {
/* vertical ruler */
if ( (i % 16) == 0 ) {
rs_putchar('+');
rs_putchar( asc_hex[ i >> 4 ] ) ;
rs_putchar('0');
rs_putchar(' ');
}
/* get register data */
tmp = load_cam(i) ;
/* separate */
*(msg+0) = ((tmp >> 4) & MASK0F) ;
*(msg+1) = (tmp & MASK0F) ;
/* show */
rs_putchar( asc_hex[*(msg+0)] ) ;
rs_putchar( asc_hex[*(msg+1)] ) ;
/* add space */
rs_putchar(' ');
/* new line */
tmp = i & MASK0F ;
if ( tmp == 15 ) { crlf(); }
}
}
void show_sensor(UBYTE x)
{
int i ;
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar('0'+((x >> i) & 1));
}
crlf();
}
#define SCCB_DELAY_CNT 1
void put_sccb_start(void)
{
/* both high level */
GP4DAT |= (1 &tl;&tl; S_SCL) | (1 &tl;&tl; S_SDA) ; /* SCL = ON ; SDA = ON */
delay_100us(SCCB_DELAY_CNT);
/* 0 -> SDA */
GP4DAT &= ~(1 &tl;&tl; S_SDA) ; /* SCL = ON ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* 0 -> SCL */
GP4DAT &= ~(1 &tl;&tl; S_SCL) ; /* SCL = OFF ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
}
void put_sccb_stop(void)
{
/* both low level */
GP4DAT &= ~((1 &tl;&tl; S_SCL) | (1 &tl;&tl; S_SDA)) ; /* SCL = OFF ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 &tl;&tl; S_SCL) ; /* SCL = ON ; SDA = OFF */
delay_100us(SCCB_DELAY_CNT);
/* both high level */
GP4DAT |= (1 &tl;&tl; S_SDA) ; /* SCL = ON ; SDA = ON */
delay_100us(SCCB_DELAY_CNT);
}
void put_sccb_data(UBYTE x)
{
volatile UBYTE tmp ;
volatile UBYTE i ;
/* */
tmp = x ;
/* transfer data with write code */
for ( i = 0 ; i &tl; 8 ; i++ ) {
/* send bit datum */
GP4DAT &= ~(1 &tl;&tl; S_SDA) ;
if ( tmp & MASK80 ) { GP0DAT |= (1 &tl;&tl; S_SDA) ; }
/* dummy */
delay_100us(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* shift */
tmp <= 1 ;
/* 0 -> SCL */
GP4DAT &= ~(1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
}
/* change input */
GP4DAT &= ~(1 &tl;&tl; 24) ; delay_sccb(SCCB_DELAY_CNT);
/* 1 -> SCL */
GP4DAT |= (1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* get acknowledge */
tmp = OFF ;
if ( (GP4DAT & MASK01) ) { tmp |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* change output */
GP4DAT |= (1 &tl;&tl; 24) ;
/* delay */
delay_100us(5);
}
UBYTE get_sccb_data(void)
{
volatile UBYTE tmp ;
volatile UBYTE i ;
/* change inupt */
GP4DAT &= ~(1 &tl;&tl; 24) ;
/* default */
tmp = 0 ;
/* loop */
for ( i = 0 ; i &tl; 8 ; i++ ) {
/* shift */
tmp &tl;<= 1 ;
/* 1 -> SCL */
GP4DAT |= (1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* get bit datum */
if ( (GP4DAT & MASK01) ) { tmp |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
}
/* 1 -> SCL */
GP4DAT |= (1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* dummy shift */
i = OFF ;
if ( (GP4DAT & MASK01) ) { i |= ON ; }
/* 0 -> SCL */
GP4DAT &= ~(1 &tl;&tl; S_SCL) ; delay_100us(SCCB_DELAY_CNT);
/* change output */
GP4DAT |= (1 &tl;&tl; 24) ;
return tmp ;
}
void send_cam(UBYTE adr,UBYTE dat)
{
/* start conditon */
put_sccb_start() ;
/* send ID */
put_sccb_data(SCCB_ID_WR);
/* send address */
put_sccb_data(adr);
/* send parameter */
put_sccb_data(dat);
/* stop condition */
put_sccb_stop();
/* delay */
delay_100us(SCCB_DELAY_CNT);
}
UBYTE load_cam(UBYTE adr)
{
UBYTE result ;
/* start conditon */
put_sccb_start() ;
/* send ID (write) */
put_sccb_data(SCCB_ID_WR);
/* send address */
put_sccb_data(adr);
/* stop condition */
put_sccb_stop();
/* start conditon */
put_sccb_start() ;
/* send ID (read) */
put_sccb_data(SCCB_ID_RD);
/* get parameter */
result = get_sccb_data();
/* stop condition */
put_sccb_stop();
return result ;
}
void delay_sccb(UBYTE x)
{
UBYTE i ;
for ( i = 0 ; i &tl; x ; i++ ) ;
}
void delay_100us(UWORD x)
{
ULONG last ;
/* calculate */
last = timcnt + x ;
/* wait */
while ( timcnt &tl; last ) ;
}
void delay_ms(UWORD x)
{
ULONG last ;
/* calculate */
last = timcnt + 10 * x ;
/* wait */
while ( timcnt &tl; last ) ;
}
void init_cam(UBYTE x)
{
/* reset camera */
send_cam(REG_COM7,COM7_RESET);
rs_puts("Complete reset"); crlf();
/* delay 200ms */
delay_ms(200);
send_cam(REG_COM7,0x00);
//send_cam(REG_RGB444,R444_ENABLE | R444_RGBX); //0x8c:RGB 444 control
send_cam(REG_COM1,0x40); //0x04:COM1(CCIR656,AEC) //0) //0x40)
send_cam(REG_COM15,COM15_R01FE|COM15_RGB565); //0x40:COM15
send_cam(REG_COM9, 0x38); //0x14:COM9=max AGC gain ceiling, Freeze AGC/AEC
//c(0x3d,0xc3) //(REG_COM13, COM13_GAMMA|COM13_UVSAT|0x2) //0x3d:COM13
send_cam(REG_HAECC7,0x94); //0xaa:Hist AEC/AGC control 7 c(0xAA,0x94) //AEC algorithm
send_cam(REG_TSLB,0x04); //0x3a:Neg,UVval,YUYV,window TSLB_YLAST) //0x04) //0x0C) //0x80) //0x00) //0x04)
send_cam(0x20,0x0f); //ADCCTR0, A/D range&ref, mu0102
send_cam(REG_COM8,0x9a); //mu0103
send_cam(0x10,0x01); //mu0103
if ( x ) {
/* CLKRC divide /4 -> 5MHz */
send_cam(0x11,0x83);
}
/* COM1 */
send_cam(0x3b,0x0a); /* disable night mode */
/* TSLB
3 bit => select YUV format
*/
send_cam(0x3a,0x05);
/* COM7
2 bit , 0 bit => select YUV format
*/
send_cam(REG_COM7,0x00);
/* COM15 */
send_cam(0x40,0xd0);
/* HSTART */
send_cam(0x17,0x16);
/* HSTOP */
send_cam(0x18,0x04);
/* HREF */
send_cam(0x32,0x24);
/* VSTRT */
send_cam(0x19,0x02);
/* VSTOP */
send_cam(0x1a,0x7a);
/* VREF */
send_cam(0x03,0x0a);
/* COM10 */
send_cam(0x15,0x02);
/* COM3 */
send_cam(0x0c,0x04);
/* COM4 */
send_cam(0x3e,0x1a);
/* SCALING_DCWCTR */
send_cam(0x72,0x22);
/* SCALING_PCLK_DIV */
send_cam(0x73,0xf2);
/* DM_LNL */
send_cam(0x92,0x00);
/* DM_LNH */
send_cam(0x93,0x00);
/* decrease frame rate
put dummy lines 510 (0x01FE)
*/
if ( x ) {
rs_puts("insert dummy line");
crlf();
/* DM_LNL */
send_cam(0x92,0xFE);
/* DM_LNH */
send_cam(0x93,0x01);
}
rs_puts("Exit initialize"); crlf();
}
void send_fpga_duty(MOTORP x)
{
volatile UBYTE i;
volatile UBYTE j;
volatile UBYTE adr[4] ;
volatile UBYTE dat[4] ;
/* set address */
*(adr+0) = 0x20+FD_ADR ;
*(adr+1) = 0x20+FP_ADR ;
*(adr+2) = 0x20+RD_ADR ;
*(adr+3) = 0x20+RP_ADR ;
/* set address */
*(dat+0) = (x.xfront >> 8) & MASK03 ;
*(dat+1) = x.xfront & MASKFF ;
*(dat+2) = (x.xrear >> 8) & MASK03 ;
*(dat+3) = x.xrear & MASKFF ;
/* send loop */
for ( i = 0 ; i < 4 ; i++ ) {
/* send front direction */
GP2DAT &= 0xff00ffff ;
GP2DAT |= (*(adr+i) << 16) ;
/* send data */
GP3DAT &= 0xff00ffff ;
GP3DAT |= (*(dat+i) << 16) ;
/* pulse */
GP2DAT |= (1 << S_TRG); /* trigger H */
for ( j = 0 ; j < 10 ; i++ ); /* delay */
GP2DAT &= ~(1 << S_TRG); /* trigger L */
}
}
void send_fpga_line(LINEP x)
{
volatile UBYTE i;
volatile UBYTE j;
volatile UBYTE adr[3] ;
volatile UBYTE dat[3] ;
/* set address */
*(adr+0) = 0x20+LTOP ;
*(adr+1) = 0x20+LMID ;
*(adr+2) = 0x20+LBOT ;
/* set data */
*(dat+0) = x.topx ;
*(dat+1) = x.midx ;
*(dat+2) = x.bottomx ;
/* send loop */
for ( i = 0 ; i < 3 ; i++ ) {
/* send front direction */
GP2DAT &= 0xff00ffff ;
GP2DAT |= (*(adr+i) << 16) ;
/* send data */
GP3DAT &= 0xff00ffff ;
GP3DAT |= (*(dat+i) << 16) ;
/* pulse */
GP2DAT |= (1 << S_TRG); /* trigger H */
for ( j = 0 ; j < 10 ; i++ ); /* delay */
GP2DAT &= ~(1 << S_TRG); /* trigger L */
}
}
UBYTE get_fpga(UBYTE lx)
{
UBYTE result ;
/* send line number */
GP2DAT &= 0xff00ffff ;
GP2DAT |= (lx << 16) ;
/* GP3 inputs */
GP3DAT &= 0x00ffffff ;
/* enable FPGA output */
GP2DAT |= (1 << S_OE) ;
/* data */
result = GP3DAT & MASKFF ;
/* disable FPGA output */
GP2DAT &= ~(1 << S_OE) ;
/* GP3 outputs */
GP3DAT |= 0xff000000 ;
return result ;
}
UBYTE get_sensor(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( x < 5 ) {
result = get_fpga(x) ;
}
return result ;
}
void send_cam_trigger(void)
{
GP2DAT |= (1 << S_CTRG);
GP2DAT &= ~(1 << S_CTRG);
}
void show_duty(void)
{
UBYTE i ;
UBYTE msg[7] ;
/* clear buffer */
for ( i = 0 ; i < 7 ; i++ ) { *(msg+i) = '\0' ; }
*(msg+1) = ' ' ;
*(msg+4) = ' ' ;
/* front */
*(msg+0) = 'F' ;
*(msg+2) = (fduty / 10) + '0' ;
*(msg+3) = (fduty % 10) + '0' ;
*(msg+5) = fdir + '0' ;
rs_puts( msg );
crlf();
/* rear */
*(msg+0) = 'R' ;
*(msg+2) = (rduty / 10) + '0' ;
*(msg+3) = (rduty % 10) + '0' ;
*(msg+5) = rdir + '0' ;
rs_puts( msg );
crlf();
}
void show_data(UBYTE x)
{
int i ;
UBYTE tmp ;
/* */
tmp = x ;
/* show */
for ( i = 7 ; i > -1 ; i-- ) {
rs_putchar( ((tmp >> i) & 1) + '0');
}
crlf();
}
/* turn off all LEDs */
void turn_off_led(void)
{
GP1DAT |= 0x00f00000 ;
}
/* turn on target LED */
void turn_on_led(UBYTE x)
{
/* judge */
if ( x > LED_LANE ) { return ; }
/* turn off all LEDs */
turn_off_led();
/* turn on target LED */
GP1DAT &= ~(1 << (x+20));
}
UBYTE perform_judge(UBYTE x)
{
volatile UBYTE result ;
/* judge */
switch ( x ) {
case 0x00 : result = STATE_ALL_BLACK ; break ;
case 0x03 :
case 0x07 : result = STATE_BIG_RIGHT ; break ;
case 0x06 :
case 0x0E : result = STATE_RIGHT ; break ;
case 0x0C : result = STATE_TINY_RIGHT ; break ;
case 0x18 :
case 0x38 :
case 0x3C :
case 0x1C : result = STATE_CENTER ; break ;
case 0x30 : result = STATE_TINY_LEFT ; break ;
case 0x60 :
case 0x70 : result = STATE_LEFT ; break ;
case 0xC0 :
case 0xE0 : result = STATE_BIG_LEFT ; break ;
case 0x3F :
case 0x1F :
case 0x0F : result = STATE_RIGHT_WHITE ; break ;
case 0xF0 :
case 0xF8 :
case 0xFC : result = STATE_LEFT_WHITE ; break ;
case 0xFF : result = STATE_ALL_WHITE ; break ;
default : result = 0x0f ; break ;
}
return result ;
}
void perform_sensing(SJP x)
{
/* judge TOP */
x.top_dat = perform_judge(x.topx);
/* judge MIDDLE */
x.mid_dat = perform_judge(x.midx);
/* judge BOTTOM */
x.bottom_dat = perform_judge(x.bottomx);
}
/* trigger handling */
void tsk0_proc(void)
{
/* get data */
str_sft <<= 1 ;
str_sft &= MASK07 ;
if ( GP0DAT & MASK20 ) { str_sft |= ON ; }
/* judge */
if ( str_sft == 3 ) {
if ( system_status == IDLE ) {
system_status = RUN ;
rsm_tsk(BLIND_RUN);
sus_tsk(LEDFLASH);
turn_off_led();
} else {
system_status = IDLE ;
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
rsm_tsk(LEDFLASH);
/* stop */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = REAR_FORWARD ;
send_fpga_duty( xmotor );
}
}
/* cycle 20ms */
wai_tsk( 2 );
}
/* BLIND_RUN */
void tsk1_proc(void)
{
/* show mode */
turn_on_led(LED_BLIND);
/* select CENTER */
xmotor.xfront = FRONT_CENTER ;
/* sequencer */
switch ( bstate ) {
/* duty 5% */
case 0 : xmotor.xrear = (REAR_FORWARD | 5) ;
break ;
/* duty 15% */
case 1 : xmotor.xrear = (REAR_FORWARD | 15) ;
break ;
/* duty 25% */
case 2 : xmotor.xrear = (REAR_FORWARD | 25) ;
break ;
/* duty 45% */
case 3 : xmotor.xrear = (REAR_FORWARD | 45) ;
break ;
/* duty 65% */
case 4 : xmotor.xrear = (REAR_FORWARD | 65) ;
break ;
/* duty 75% */
case 5 : xmotor.xrear = (REAR_FORWARD | 75) ;
break ;
/* others */
default : break ;
}
send_fpga_duty( xmotor );
/* increment */
bstate++ ;
/* judge */
if ( bstate < 6 ) {
wai_tsk(50) ; /* 500ms interval */
} else {
bstate = 0 ;
/* rsm_tsk(SENSING) ; */
rsm_tsk(NORMAL_RUN) ;
slp_tsk();
turn_off_led();
}
}
/* NORMAL_RUN */
void tsk2_proc(void)
{
volatile UBYTE topx ;
volatile UBYTE midx ;
volatile UBYTE sensorx ;
volatile UBYTE flag2 ;
/* show mode */
turn_on_led(LED_NORMAL);
/* show sensor data */
//show_sensor(sensorx);
/* default */
flag2 = OFF ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 50) ;
topx = xsjp.top_dat ;
midx = xsjp.mid_dat ;
sensorx = xsjp.bottom_dat ;
/* judge */
if ( sensorx == STATE_CENTER ) {
xmotor.xfront = ( FRONT_CENTER );
xmotor.xrear = ( REAR_FORWARD | 50 ) ;
}
if ( sensorx == STATE_TINY_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 50);
xmotor.xrear = (REAR_FORWARD | 75);
}
if ( sensorx == STATE_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 75);
xmotor.xrear = (REAR_FORWARD | 55);
}
if ( sensorx == STATE_BIG_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 90);
xmotor.xrear = (REAR_FORWARD | 45);
}
if ( sensorx == STATE_TINY_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 50);
xmotor.xrear = (REAR_FORWARD | 75);
}
if ( sensorx == STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 75);
xmotor.xrear = (REAR_FORWARD | 55);
}
if ( sensorx == STATE_BIG_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 90);
xmotor.xrear = (REAR_FORWARD | 45);
}
send_fpga_duty( xmotor ) ;
/* get CRANK mark */
if ( topx == STATE_ALL_WHITE ||
midx == STATE_ALL_WHITE ||
sensorx == STATE_ALL_WHITE ) {
white_state = WHITE_STATE_ALL ;
rsm_tsk( CRANK_RUN );
flag2 = ON ;
}
/* get LANE LEFT CHANGE mark */
if ( topx == STATE_LEFT_WHITE ||
midx == STATE_LEFT_WHITE ||
sensorx == STATE_LEFT_WHITE ) {
white_state = WHITE_STATE_LEFT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* get LANE RIGHT CHANGE mark */
if ( topx == STATE_RIGHT_WHITE ||
midx == STATE_RIGHT_WHITE ||
sensorx == STATE_RIGHT_WHITE ) {
white_state = WHITE_STATE_RIGHT ;
rsm_tsk( LANE_CHANGE );
flag2 = ON ;
}
/* delay */
if ( flag2 == ON ) {
slp_tsk() ;
turn_off_led();
} else {
wai_tsk(1) ; /* 10ms interval */
}
}
/* CRANK_RUN */
void tsk3_proc(void)
{
volatile UBYTE topx ;
volatile UBYTE midx ;
/* show mode */
turn_on_led(LED_CRANK);
/* show sensor data */
//show_sensor(sensorx);
topx = xsjp.top_dat ;
midx = xsjp.mid_dat ;
/* sequencer */
switch ( cstate ) {
/* blind run until minimum distance */
case 0 : cstate = 1 ;
ccnt = 30 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga_duty( xmotor );
break ;
/* delay */
case 1 : if ( ccnt == 0 ) {
cstate = 2 ;
} else {
ccnt-- ;
}
break ;
/* until CRANK location */
case 2 : if ( topx == STATE_LEFT_WHITE ||
midx == STATE_LEFT_WHITE ) {
cstate = 3 ;
white_state = WHITE_STATE_LEFT ;
}
if ( topx == STATE_RIGHT_WHITE ||
midx == STATE_RIGHT_WHITE ) {
cstate = 3 ;
white_state = WHITE_STATE_RIGHT ;
}
break ;
/* judge turn location */
case 3 : if ( topx == STATE_ALL_WHITE ||
topx == STATE_ALL_BLACK ||
midx == STATE_ALL_WHITE ||
midx == STATE_ALL_BLACK ) {
cstate = 4 ;
}
break ;
/* turn */
case 4 : cstate = 5 ;
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 50);
}
if ( white_state == WHITE_STATE_RIGHT ) {
xmotor.xfront = (FRONT_RIGHT | 50);
}
xmotor.xrear = (REAR_FORWARD | 50);
send_fpga_duty( xmotor ) ;
break ;
/* judge center */
case 5 : if ( topx == STATE_CENTER || midx == STATE_CENTER ) {
cstate = 6 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga_duty( xmotor ) ;
}
break ;
/* return first state */
case 6 : break ;
/* */
default : break ;
}
/* delay */
if ( cstate < 6 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
cstate = 0 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga_duty( xmotor );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
turn_off_led();
rsm_tsk( NORMAL_RUN );
}
}
/* LANE_CHANGE */
void tsk4_proc(void)
{
volatile UBYTE xtop ;
volatile UBYTE xmid ;
/* show mode */
turn_on_led(LED_LANE);
/* show sensor data */
//show_sensor(sensorx);
/* */
xtop = xsjp.top_dat ;
xmid = xsjp.mid_dat ;
/* sequencer */
switch ( lstate ) {
/* forward until ALL_BLACK */
case 0 : if ( xtop == STATE_ALL_BLACK || xmid == STATE_ALL_BLACK ) {
lstate = 1 ;
}
break ;
/* judge turn */
case 1 : lstate = 2 ;
lcnt = 10 ;
xmotor.xfront = (FRONT_RIGHT | 80);
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_LEFT | 80);
}
send_fpga_duty( xmotor );
break ;
/* turn delay */
case 2 : if ( lcnt == 0 ) {
lstate = 3 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga_duty( xmotor );
} else {
lcnt-- ;
}
break ;
/* blind run */
case 3 : lstate = 4 ;
lcnt = 10 ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga_duty( xmotor );
break ;
/* blind run delay */
case 4 : if ( lcnt == 0 ) {
lstate = 5 ;
} else {
lcnt-- ;
}
break ;
/* judge center */
case 5 : if ( xtop == STATE_CENTER || xmid == STATE_CENTER ) {
lstate = 6 ;
}
break ;
/* judge turn (reverse) */
case 6 : lstate = 7 ;
lcnt = 10 ;
xmotor.xfront= (FRONT_LEFT | 80);
if ( white_state == WHITE_STATE_LEFT ) {
xmotor.xfront = (FRONT_RIGHT | 80);
}
send_fpga_duty( xmotor );
break ;
/* turn (reverse) delay */
case 7 : if ( lcnt == 0 ) {
lstate = 8 ;
xmotor.xfront = FRONT_CENTER ;
send_fpga_duty( xmotor );
} else {
lcnt-- ;
}
break ;
/* return first state */
case 8 : break ;
/* */
default : break ;
}
/* delay */
if ( lstate < 8 ) {
wai_tsk(1) ; /* 10ms interval */
} else {
lstate = 0 ;
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = (REAR_FORWARD | 75);
send_fpga_duty( xmotor );
white_state = WHITE_STATE_NONE ;
slp_tsk() ;
turn_off_led();
rsm_tsk( NORMAL_RUN );
}
}
/* SENSING */
void tsk5_proc(void)
{
/* get data */
*(sensor+0) = get_fpga(SENX0) ;
*(sensor+1) = get_fpga(SENX1) ;
*(sensor+2) = get_fpga(SENX2) ;
/* judge */
xsjp.topx = *(sensor+0) ;
xsjp.midx = *(sensor+1) ;
xsjp.bottomx = *(sensor+2) ;
perform_sensing(xsjp);
/* send trigger */
send_cam_trigger();
/* */
wai_tsk( 3 ) ; /* 30ms */
}
#define MAX_RUN_PULSE 28000
/* MEASURE */
void tsk6_proc(void)
{
volatile UWORD xmes ;
/* update */
pre_rcnt = cur_rcnt ;
/* get upper byte */
xmes = get_fpga(ROTU);
xmes <<= 8 ;
/* get lower byte */
xmes += get_fpga(ROTL);
/* update */
cur_rcnt = xmes ;
/* differencial pulse count */
dif_rcnt = cur_rcnt - pre_rcnt ;
/* judge */
if ( cur_rcnt == MAX_RUN_PULSE ) {
/* stop RUN */
sus_tsk(NORMAL_RUN);
sus_tsk(CRANK_RUN);
sus_tsk(LANE_CHANGE);
/* stop motors */
xmotor.xfront = FRONT_CENTER ;
xmotor.xrear = REAR_FORWARD ;
send_fpga_duty( xmotor );
}
/* interval */
wai_tsk( 10 ) ; /* 100ms */
}
/* SDEBUG */
void tsk7_proc(void)
{
volatile UBYTE tmp7 ;
volatile UBYTE loop7 ;
/* default */
tmp7 = 0 ;
/* judge flag */
if ( uflag == OFF ) return ;
/* new line */
crlf();
/* clear flag */
uflag = OFF ;
/* judge */
cmd = *(sbuf+0) ;
if ( cmd == '?' ) { show_help(); }
/* initialize CAMERA */
if ( cmd == 'I' ) {
push_tsk_state();
turn_off_led();
init_cam( *(sbuf+1)-'0' ) ;
pop_tsk_state();
}
/* set front parameters */
if ( cmd == 'F' || cmd == 'R' ) {
/* duty */
duty = get_hex( *(sbuf+1) ) ;
duty *= 10 ;
duty += get_hex( *(sbuf+2) ) ;
/* direction */
direction = get_hex( *(sbuf+3) ) ;
/* rear or front */
if ( cmd == 'R' ) {
rdir = direction ;
rduty = duty ;
xmotor.xrear = (rdir << 8) | rduty ;
} else {
fdir = direction ;
fduty = duty ;
xmotor.xfront = (fdir << 8) | fduty ;
}
/* impress */
send_fpga_duty( xmotor );
}
/* set encoder pulse */
if ( cmd == 'P' ) {
for ( loop7 = 0 ; loop7 < 2 ; loop7++ ) {
/* get byte */
tmp7 = get_fpga(ROTU);
if ( loop7 ) { tmp7 = get_fpga(ROTL); }
/* convert and show */
rs_putchar( asc_hex[(tmp7 >> 4) & MASK0F] );
rs_putchar( asc_hex[tmp7 & MASK0F] );
}
/* new line */
crlf() ;
}
/* show duty */
if ( cmd == 'S' ) { show_duty(); }
/* show sensor data */
if ( cmd == 'D' ) {
/* get sensor loacation */
tmp7 = get_hex( *(sbuf+1) ) - '0' ;
/* */
show_data( get_fpga(tmp7) );
}
/* show register values */
if ( cmd == 'V' ) {
push_tsk_state();
turn_off_led();
show_reg() ;
pop_tsk_state();
}
}
/* LED flash */
void tsk8_proc(void)
{
/* impress */
turn_on_led( cyclic[fstate] );
/* update state */
fstate++ ;
/* judge */
if ( fstate == 6 ) { fstate = 0 ; }
/* interval 300ms */
wai_tsk(30);
}
/*------------------*/
/* system call body */
/*------------------*/
void init_os(void)
{
ready = 0 ;
suspend = 0 ;
waitq = 0 ;
tflag = OFF ;
}
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
tcb[tid].tsk = tsk;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
volatile UWORD tmp ;
tmp = (1 << tid);
if ( sta == TTS_READY ) { ready |= tmp; }
if ( sta == TTS_SUSPEND ) { suspend |= tmp; }
if ( sta == TTS_WAIT ) { waitq |= tmp; }
}
void rsm_tsk(UBYTE tid)
{
volatile UWORD tmp ;
tmp = (1 << tid);
ready |= tmp;
suspend &= ~tmp;
waitq &= ~tmp;
}
void sus_tsk(UBYTE tid)
{
volatile UWORD tmp ;
tmp = (1 << tid);
ready &= ~tmp;
suspend |= tmp;
waitq &= ~tmp;
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
volatile UWORD tmp ;
tmp = (1 << run_tsk);
ready &= ~tmp;
suspend &= ~tmp;
waitq |= tmp;
tcb[run_tsk].wcount = x ;
}
UBYTE is_tsk_ready(UBYTE tid)
{
return( (ready >> tid) & 1 ) ;
}
void timer_handler(void)
{
volatile UWORD xtmp;
volatile UBYTE loop;
/* call timer handling */
xtmp = waitq ;
for ( loop = 0 ; loop < TSK_ID_MAX ; loop++ ) {
if ( xtmp & ON ) {
tcb[loop].wcount-- ;
if ( tcb[loop].wcount == 0 ) { rsm_tsk(loop); }
}
xtmp >>= 1 ;
}
}
void push_tsk_state(void)
{
/* store */
*(tsk_state+0) = ready ;
*(tsk_state+1) = suspend ;
*(tsk_state+2) = waitq ;
/* resume */
ready = (1 << SDEBUG) ;
suspend = 0 ;
waitq = 0 ;
}
void pop_tsk_state(void)
{
/* resume */
ready = *(tsk_state+0) ;
suspend = *(tsk_state+1) ;
waitq = *(tsk_state+2) ;
}
変更内容は、以下です。
- FPGAとのバスインタフェース
- 画像データの利用ラインを３ラインとする
- ３ラインは、０〜５９の任意の３ラインを指定できる
- ３ラインの組合せで、走行パターンを変更
- ラインから生成するセンサーデータは８ビット
赤LEDあるいは緑LEDを17本並べて、輝度を稼げる
ようにしました。赤LED、緑LEDは順方向降下電圧
が２Ｖ程度なので、昇圧で高くなっている電圧で
より輝度を上げられると考えられます。
回路図は、以下。
手持ちの部品を利用したので、以前の昇圧回路で
使った値の素子とは多少異なっています。厳密な
計算を必要としない部位なので、10%程度誤差は
目を瞑っておきます。
LEDを載せられるだけ入れると、２６本でした。
（写真では、２４本LEDを載せています。）
LEDの足を切っていないときは、次のように
なります。移動中に外れないように、リード
を切ってから、走らせます。
昇圧に使う電源電圧は4V程度なので、２倍に昇圧
されると8Vになります。緑LEDは順方向の降下電圧
は2V程度なので、電流制限抵抗に流れる電流は
6V/330=1.8mA。これを26本使うと、47mA程度になり
ます。白色LEDを利用した場合の電流と同じ程度でも
LEDの本数が増えているので、光量は増え、到達距離
は長くなります。
FPGAとのバスインタフェース部分は、次のVHDLコード
としました。
-- write register file
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iDSTATE <= "00" ; -- state control
-- data
iDDATA <= (others => '0') ;
-- control
iDCONT <= (others => '0') ;
-- direction
iDIRF <= "00" ; -- forward
iDIRR <= "00" ; -- reverse
-- duty ratio
iDUTYF <= (others => '0') ;
iDUTYR <= (others => '0') ;
-- target line
iTLTOP <= (others => '0') ;
iTLMIDDLE <= (others => '0') ;
iTLBOTTOM <= (others => '0') ;
elsif rising_edge( CLOCK ) then
case conv_integer(iDSTATE) is
-- wait trigger
when 0 => if ( iTRG = '1' ) then
iDSTATE <= "01" ;
else
iDSTATE <= "00" ;
end if ;
-- latch
when 1 => iDSTATE <= "11" ;
-- data
iDDATA <= GP3 ;
-- control
iDCONT <= GP2(5 downto 0) ;
-- deliver
when 3 => iDSTATE <= "10" ;
if ( iDCONT(5 downto 4) = "10" ) then
-- direction forword
if ( iDCONT(3 downto 0) = "0000" ) then
iDIRF <= iDDATA(1 downto 0) ;
-- forword duty
elsif ( iDCONT(3 downto 0) = "0001" ) then
iDUTYF <= iDDATA(6 downto 0) ;
-- direction reverse
elsif ( iDCONT(3 downto 0) = "0010" ) then
iDIRR <= iDDATA(1 downto 0) ;
-- reverse duty
elsif ( iDCONT(3 downto 0) = "0011" ) then
iDUTYF <= iDDATA(6 downto 0) ;
-- target line TOP
elsif ( iDCONT(3 downto 0) = "1011" ) then
iTLTOP <= iDDATA ;
-- target line MIDDLE
elsif ( iDCONT(3 downto 0) = "1100" ) then
iTLMIDDLE <= iDDATA ;
-- target line BOTTOM
elsif ( iDCONT(3 downto 0) = "1101" ) then
iTLBOTTOM <= iDDATA ;
end if ;
end if ;
-- return first state
when 2 => iDSTATE <= "00" ;
-- default
when others =>
iDSTATE <= "00" ;
end case ;
end if ;
end process ;
-- BUS interface data
GP3 <= iDAT when ( GP2(5 downto 4) = "01" ) else (others => 'Z');
-- read register file
iDAT <= "000000" & iDIRF when ( GP2(3 downto 0) = "0000" ) else -- front direction
'0' & iDUTYF when ( GP2(3 downto 0) = "0001" ) else -- front duty
"000000" & iDIRR when ( GP2(3 downto 0) = "0010" ) else -- rear direction
'0' & iDUTYR when ( GP2(3 downto 0) = "0011" ) else -- rear duty
iLTOP when ( GP2(3 downto 0) = "1000" ) else -- TOP
iLMIDDLE when ( GP2(3 downto 0) = "1001" ) else -- MIDDLE
iLBOTTOM when ( GP2(3 downto 0) = "1010" ) else -- BOTTOM
iTLTOP when ( GP2(3 downto 0) = "1011" ) else -- target line TOP
iTLMIDDLE when ( GP2(3 downto 0) = "1100" ) else -- target line MIDDLE
iTLBOTTOM when ( GP2(3 downto 0) = "1101" ) else -- target line BOTTOM
iECNT(15 downto 8) when ( GP2(3 downto 0) = "1110" ) else -- encoder counter(high)
iECNT( 7 downto 0) when ( GP2(3 downto 0) = "1111" ) else -- encoder counter(low)
"11111111" ; -- dummy
レジスタファイルは１６バイトと、次のように
レジスタ機能を割当てました。
- register_0 前輪の回転方向（左右）
- register_1 前輪のPWMデューティ比
- register_2 後輪の回転方向（前後）
- register_3 後輪のPWMデューティ比
- register_4 （空）
- register_5 （空）
- register_6 （空）
- register_7 （空）
- register_8 ラインTOPのセンサーデータ
- register_9 ラインMIDDLEのセンサーデータ
- register_A ラインBOTTOMのセンサーデータ
- register_B ラインTOPの位置
- register_C ラインMIDDLEの位置
- register_D ラインBOTTOMの位置
- register_E エンコーダカウンタの上位８ビット
- register_F エンコーダカウンタの下位８ビット
- BIT_7 CENA
- BIT_6 TRG
- BIT_5 WE
- BIT_4 OE
- BIT_3 LSEL3
- BIT_2 LSEL2
- BIT_1 LSEL1
- BIT_0 LSEL0
目次 前 次