目次
前
次
夏の試走会
ネットミーティングを通じて、夏の試走会の話が出ました。
FIFOなしカメラを接続して、何とか画像処理で走らせ
たいと、ファームウエアと電子回路を考えました。
まず、移動メカの特徴を掴むためのファーム
ウエアとVHDLコードを定義です。
ARMは、Spartan3は、バス接続します。
ポート２は、バスの制御信号を配置。
- P2.7 = CENA
- P2.6 = TRG
- P2.5 = DIR1
- P2.4 = DIR0
- P2.3 = OE
- P2.2 = LSEL2
- P2.1 = LSEL1
- P2.0 = LSEL0
前輪、後輪のモータDUTY比をTRGをトリガーとして
DIRで回転方向、ポート３に前輪、後輪の指定と値
を設定します。
センサーデータは、ライン番号をLSELで指定、OEで
バスの方向を変えて対応。
CENAは、カメラに撮影指示のトリガーとしてあります。
ポート３は、バスデータ信号を配置。
- P3.7 = BIO<7>
- P3.6 = BIO<6>
- P3.5 = BIO<5>
- P3.4 = BIO<4>
- P3.3 = BIO<3>
- P3.2 = BIO<2>
- P3.1 = BIO<1>
- P3.0 = BIO<0>
ポート１には、動作状態を示すLED制御信号を配置。
- P1.7 = stateLED3
- P1.6 = stateLED2
- P1.5 = stateLED1
- P1.4 = stateLED0
ARMのファームウエアは、以下です。
#include <ADuC7026.h>
#include "stdio.h"
#define OFF 0
#define ON OFF+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 MASK01 0x01
#define MASKF0 0xF0
#define MASK07 0x07
#define SCCB_ID_WR 0x42
#define SCCB_ID_RD 0x43
#define S_SCL 23
#define S_SDA 22
#define S_TRG 22
#define S_DIR 20
#define S_OE 19
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(void);
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 send_cam(UBYTE adr,UBYTE dat);
void delay_ms(UWORD x);
void delay_s(UBYTE x);
void send_fpga(UWORD x);
UBYTE get_fpga(UBYTE lx);
void show_duty(void);
void show_data(UBYTE x);
void show_state(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 UBYTE cstate ;
volatile UWORD vscnt ;
volatile UBYTE vtrg ;
volatile UBYTE ptrg ;
volatile UBYTE scnt ;
volatile UBYTE str_sft ;
volatile UBYTE str_trg ;
volatile UBYTE fduty ;
volatile UBYTE rduty ;
volatile UBYTE fdir ;
volatile UBYTE rdir ;
#define SCNT_MAX 20
#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
int main(void)
{
UBYTE duty ;
UBYTE direction ;
/* initialize user */
init_usr();
/* show message */
rs_puts("Hello"); crlf();
/* endless loop */
show_state(STATE_WAIT);
while(ON)
{
/* command interrpreter */
if ( uflag == ON ) {
/* clear flag */
uflag = OFF ;
/* judge */
cmd = *(sbuf+0) ;
if ( cmd == '?' ) { show_help(); }
/* initialize CAMERA */
if ( cmd == 'I' ) { crlf(); init_cam() ; }
/* set front parameters */
if ( cmd == 'F' || cmd == 'R' ) {
crlf();
/* 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 ;
} else {
fdir = direction ;
fduty = duty ;
duty &= ~MASK80 ;
}
/* impress */
send_fpga( (direction << 8) | duty );
}
/* show duty */
if ( cmd == 'S' ) {
crlf();
show_duty();
}
/* show sensor data */
if ( cmd == 'D' ) {
crlf();
direction = get_hex( *(sbuf+1) ) - '0' ;
show_data( get_fpga( direction ) );
}
}
/* state machine */
switch ( state ) {
/* wait trigger */
case 0 : state = 0 ;
if ( str_trg == ON ) {
str_trg = OFF ;
state = 1 ;
show_state(STATE_NORMAL);
}
break ;
/* duty 15% */
case 1 : state = 2 ;
send_fpga( REAR_FORWARD | 15 );
delay_s(1) ;
break ;
/* duty 20% */
case 2 : state = 3 ;
send_fpga( REAR_FORWARD | 20 );
delay_s(1) ;
break ;
/* duty 25% */
case 3 : state = 4 ;
send_fpga( REAR_FORWARD | 25 );
delay_s(1) ;
break ;
/* duty 35% */
case 4 : state = 5 ;
send_fpga( REAR_FORWARD | 35 );
delay_s(1) ;
break ;
/* duty 45% */
case 5 : state = 6 ;
send_fpga( REAR_FORWARD | 45 );
delay_s(2) ;
break ;
/* duty 50% */
case 6 : state = 7 ;
send_fpga( REAR_FORWARD | 50 );
delay_s(1) ;
break ;
/* left */
case 7 : state = 8 ;
send_fpga( FRONT_LEFT | 50 );
delay_ms(1000) ;
break ;
/* straight */
case 8 : state = 9 ;
send_fpga( FRONT_CENTER | 50 );
send_fpga( REAR_FORWARD | 50 );
delay_s(1) ;
break ;
/* right */
case 9 : state = 10 ;
send_fpga( FRONT_RIGHT | 50 );
delay_ms(1000) ;
break ;
/* straight */
case 10 : state = 11 ;
send_fpga( FRONT_CENTER );
send_fpga( REAR_FORWARD | 50 );
delay_s(2) ;
break ;
/* return first state */
case 11 : state = 0 ;
send_fpga( REAR_STOP );
show_state(STATE_WAIT);
break ;
/* others */
default : state = 0 ;
break ;
}
}
/* 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 timer3 interruption (1ms) */
if ( (IRQSTA & WATCHDOG_TIMER_BIT) == WATCHDOG_TIMER_BIT ) {
/* clear timer3 interrupt flag */
T3CLRI = 0xff ;
/* increment */
timcnt++ ;
/* blink */
if ( (timcnt & 0x3ff) == 1000 ) { GP4DAT ^= (1 << 23); }
/* increment */
scnt++ ;
if ( scnt == SCNT_MAX ) {
scnt = 0 ;
str_sft <<= 1 ;
str_sft &= MASK07 ;
if ( GP0DAT & MASK20 ) { str_sft |= ON ; }
str_trg = OFF ;
if ( str_sft == 3 ) { str_trg = ON ; }
}
}
}
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 ;
cstate = 0 ;
vscnt = 0 ;
scnt = 0 ;
str_sft = 0 ;
/* 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 */
{
/* */
GP0DAT = 0xDF1F0000 ;
}
/* P1 */
{
/* use UART */
GP1CON = 0x00000011 ;
/* */
GP1DAT = 0xfef00000 ;
}
/* P2 */
{
/* all bits outputs */
GP2DAT = 0xff000000 ;
}
/* P3 */
{
/* all bits outputs */
GP3DAT = 0xff000000 ;
}
/* P4 */
{
GP4DAT = 0xff000000 ;
}
/* initialize timer 3 (1s) */
{
T3LD = 33 ; /* (32.768kHz / 32) = about 1 kHz */
T3CON = 0xc0 ; /* enable , cyclic , 1/1 */
}
timcnt = 0 ;
fduty = 0 ;
rduty = 0 ;
fdir = 0 ;
rdir = 0 ;
/* enable timer 3 interrupt and UART interrupt */
IRQEN = WATCHDOG_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("S duty") ; crlf();
}
void put_sccb_start(void)
{
/* both high level */
GP0DAT |= ((1 << S_SCL) | (1 << S_SDA)) ; /* SCL = ON ; SDA = ON */
delay_ms(1);
/* 0 -> SDA */
GP0DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */
delay_ms(1);
/* 0 -> SCL */
GP0DAT &= ~(1 << S_SCL) ; /* SCL = OFF ; SDA = OFF */
delay_ms(1);
}
void put_sccb_stop(void)
{
/* both high level */
GP0DAT |= (1 << S_SCL) ; /* SCL = ON ; SDA = ? */
delay_ms(1);
/* 0 -> SDA */
GP0DAT &= ~(1 << S_SDA) ; /* SCL = ON ; SDA = OFF */
delay_ms(1);
/* both high level */
GP0DAT |= (1 << S_SDA) ; /* SCL = ON ; SDA = ON */
delay_ms(1);
}
void put_sccb_data(UBYTE x)
{
UBYTE tmp ;
UBYTE i ;
/* load raw data */
tmp = x ;
/* transfer data with write code */
for ( i = 0 ; i < 8 ; i++ ) {
/* send bit datum */
GP0DAT &= ~(1 << S_SDA) ;
if ( tmp & MASK80 ) { GP0DAT |= (1 << S_SDA) ; }
delay_ms(1) ;
/* 1 -> SCL */
GP0DAT |= (1 << S_SCL) ; delay_ms(1) ;
/* shift */
tmp <<= 1 ;
/* 0 -> SCL */
GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ;
}
/* data low */
GP0DAT &= ~(1 << S_SDA) ; delay_ms(1) ;
/* change input */
GP0DAT &= ~(1 << 30) ; delay_ms(1) ;
/* 1 -> SCL */
GP0DAT |= (1 << S_SCL) ; delay_ms(1) ;
/* get acknowledge */
tmp = OFF ;
if ( (GP0DAT & MASK40) == MASK40 ) { tmp = ON ; }
/* 0 -> SCL */
GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ;
/* change output */
GP0DAT |= (1 << 30) ;
}
UBYTE get_sccb_data(void)
{
UBYTE result ;
UBYTE i ;
UBYTE tmp ;
/* change inupt */
GP0DAT &= ~(1 << 30) ;
/* default */
result = 0 ;
/* loop */
for ( i = 0 ; i < 8 ; i++ ) {
/* shift */
result <<= 1 ;
/* 1 -> SCL */
GP0DAT |= (1 << S_SCL) ; delay_ms(1) ;
/* get bit datum */
if ( (GP0DAT & MASK40) == MASK40 ) { result |= ON ; }
/* 0 -> SCL */
GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ;
}
/* 1 -> SCL */
GP0DAT |= (1 << S_SCL) ; delay_ms(1) ;
/* get acknowledge */
tmp = OFF ;
if ( (GP0DAT & MASK40) == MASK40 ) { tmp = ON ; }
/* 0 -> SCL */
GP0DAT &= ~(1 << S_SCL) ; delay_ms(1) ;
/* change output */
GP0DAT |= (1 << 30) ;
return result ;
}
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_ms(1);
}
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_ms(UWORD x)
{
ULONG last ;
/* set */
last = timcnt + x ;
/* delay */
while ( timcnt < last ) ;
}
void delay_s(UBYTE x)
{
UWORD i ;
/* set */
for ( i = 0 ; i < x ; i++ ) {
delay_ms(1000);
}
}
void init_cam(void)
{
send_cam(REG_COM7,COM7_RESET); //0x12:COM7(Reset,QCIF,Cbar,RGBformat)
delay_ms(200); //200ms
rs_puts("Complete reset"); crlf();
send_cam(REG_COM7,COM7_RGB | COM7_RGB); //0x12:COM7 , 0x0C) //RGB
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
/* CLKRC */
send_cam(0x11,0x80);
/* COM1 */
send_cam(0x3b,0x0a); /* disable night mode */
/* TSLB */
send_cam(0x3a,0x04);
/* COM7 */
send_cam(0x12,0x04);
/* RGB444 */
send_cam(0x8c,0x02);
/* 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);
rs_puts("Exit initialize"); crlf();
}
void send_fpga(UWORD x)
{
UBYTE i;
UBYTE dir ;
UBYTE dat ;
/* get direction and dat */
dir = (x >> 8) & MASK0F ;
dat = x & MASKFF ;
/* impress data */
GP3DAT &= 0xff00ffff ;
GP3DAT |= (dat << 16) ;
/* impress direction */
GP2DAT &= 0xff07ffff ; /* 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 ;
}
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();
}
void show_state(UBYTE x)
{
UBYTE tmp ;
/* judge */
if ( x > STATE_LANEC ) return ;
/* generate code */
tmp = (1 << x) ;
tmp ^= MASK0F ;
tmp <<= 4 ;
/* impress LED */
GP1DAT &= 0xff00ffff ;
GP1DAT |= (tmp << 16);
}
スタートトリガーを、タイマー割込み＋シフトレジスタで
とらえて、シーケンサでモータを回転させました。
シリアルインタフェースにより、単独で前輪、後輪のモータ
回転を制御できるようにしてあります。
Spartan3のVHDLコードは、以下。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity camtst is
generic (
TOPX : integer := 4 ;
XMAX : integer := 10 ;
TOPY : integer := 11 ;
YMAX : integer := 1666 ;
TOPZ : integer := 6 ;
ZMAX : integer := 50 ;
TOPU : integer := 8 ;
UMAX : integer := 150 ;
LCNT_MAX : integer := 120 ;
PCNT_MAX : integer := 160 --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- camera control
--CENA : in std_logic ;
-- camera
PCLK : in std_logic ;
VSYNC : in std_logic ;
HREF : in std_logic ;
GDAT : in std_logic_vector( 7 downto 0) ;
-- BUS interface
TRG : in std_logic ;
DIR : in std_logic_vector( 1 downto 0) ;
OE : in std_logic ;
LSEL : in std_logic_vector( 2 downto 0) ;
BIO : inout std_logic_vector( 7 downto 0) ;
-- MOTOR control
FOUT : out std_logic_vector( 1 downto 0) ;
ROUT : out std_logic_vector( 1 downto 0) ;
-- monitor
GLED : out std_logic ;
RLED : out std_logic ;
MGDAT : out std_logic_vector(15 downto 0) --;
);
end camtst ;
architecture Behavioral of camtst is
-- component clock generator
component clkgenx is
generic (
TOPX : integer ;
RMAX : integer --;
);
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- output
CLKOUT : out std_logic -- ;
);
end component ;
-- component motor driver
component pwmax is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
DIR : in std_logic_vector(1 downto 0) ;
-- duty ratio
DUTY : in std_logic_vector(6 downto 0) ;
-- output
POUT : out std_logic_vector(1 downto 0) --;
);
end component;
-- clock
signal iMCLK : std_logic ;
signal iPWMCLK : std_logic ;
signal iSCLK : std_logic ;
signal iICLK : std_logic ;
-- camera state machine
signal iVS_SFT : std_logic_vector(2 downto 0) ;
signal iVSTRG : std_logic ;
signal iPCLK_SFT : std_logic_vector(2 downto 0) ;
signal iPTRG : std_logic ;
signal iHREF : std_logic ;
signal iLCNT : integer range 0 to LCNT_MAX ;
signal iPCNT : integer range 0 to PCNT_MAX ;
signal iCSTATE : std_logic_vector(3 downto 0) ;
signal iFINDEX : integer range 0 to 924 ;
signal iGDAT : std_logic_vector(15 downto 0) ;
-- BUS interface
signal iTRG : std_logic ;
signal iTRG_SFT : std_logic_vector(2 downto 0) ;
signal iDIR : std_logic_vector(1 downto 0) ;
signal iSEL : std_logic ;
signal iDUTY : std_logic_vector(6 downto 0) ;
signal iDIRR : std_logic_vector(1 downto 0) ;
signal iDIRF : std_logic_vector(1 downto 0) ;
signal iDUTYF : std_logic_vector(6 downto 0) ;
signal iDUTYR : std_logic_vector(6 downto 0) ;
signal iDSTATE : std_logic_vector(1 downto 0) ;
-- BUS data
signal iBIO : std_logic_vector(7 downto 0) ;
-- MOTOR control
signal iDIR_FRONT : std_logic_vector(1 downto 0) ;
signal iDIR_REAR : std_logic_vector(1 downto 0) ;
signal iDUTY_FRONT : std_logic_vector(6 downto 0) ;
signal iDUTY_REAR : std_logic_vector(6 downto 0) ;
signal iFOUT : std_logic_vector(1 downto 0) ;
signal iROUT : std_logic_vector(1 downto 0) ;
begin
-- component clock generator (20MHz/20 = 1MHz)
CLKX : clkgenx generic map (TOPX,XMAX) port map (nRESET,CLOCK,iMCLK);
-- component clock generator (1MHz/3333 = about 300Hz)
CLKY : clkgenx generic map (TOPY,YMAX) port map (nRESET,iMCLK,iSCLK);
-- component clock generator (1MHz/100 = 10kHz)
CLKZ : clkgenx generic map (TOPZ,ZMAX) port map (nRESET,iMCLK,iPWMCLK);
-- component clock generator (300Hz/300 = about 1HzHz)
CLKU : clkgenx generic map (TOPU,UMAX) port map (nRESET,iSCLK,iICLK);
-- rear motor control
REARX : pwmax port map (nRESET,iPWMCLK,iDIR_REAR,iDUTY_REAR,iROUT);
-- front motor control
FRONTX : pwmax port map (nRESET,iPWMCLK,iDIR_FRONT,iDUTY_FRONT,iFOUT);
-- input
iHREF <= HREF ;
-- output
GLED <= not iICLK ;
RLED <= iICLK ;
MGDAT <= not iGDAT ;
-- PWM output
FOUT <= iFOUT ;
ROUT <= iROUT ;
BIO <= iBIO when ( OE = '1' ) else (others => 'Z' );
-- internal
iBIO <= X"A1" when ( LSEL = "000" ) else
X"B2" when ( LSEL = "001" ) else
X"C4" when ( LSEL = "010" ) else
X"D8" when ( LSEL = "011" ) else
X"E9" when ( LSEL = "100" ) else
X"F6" when ( LSEL = "101" ) else
X"55" when ( LSEL = "110" ) else
X"00" ;
-- trigger
process (nRESET,iSCLK)
begin
if ( nRESET = '0' ) then
iVS_SFT <= "000" ;
elsif rising_edge(iSCLK) then
iVS_SFT <= iVS_SFT(1 downto 0) & VSYNC ;
end if ;
end process ;
iVSTRG <= '1' when ( iVS_SFT = "011" or iVS_SFT = "001" ) else '0' ;
--PCLK
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iPCLK_SFT <= "000" ;
elsif rising_edge(CLOCK) then
iPCLK_SFT <= iPCLK_SFT(1 downto 0) & PCLK ;
end if ;
end process ;
iPTRG <= '1' when ( iPCLK_SFT = "011" or iPCLK_SFT = "001" ) else '0' ;
-- camera sequencer
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCSTATE <= "0000" ;
iLCNT <= 0 ;
iPCNT <= 0 ;
iFINDEX <= 0 ;
elsif rising_edge(CLOCK) then
case conv_integer(iCSTATE) is
-- wait trigger
when 0 => if ( iVSTRG = '1' ) then
iCSTATE <= "0001" ;
iLCNT <= 0 ;
iPCNT <= 0 ;
iFINDEX <= 0 ;
else
iCSTATE <= "0000" ;
end if ;
-- judge line max
when 1 => if ( iLCNT = LCNT_MAX ) then
iCSTATE <= "1000" ;
iLCNT <= 0 ;
else
iCSTATE <= "0011" ;
end if ;
-- judge pixel max
when 3 => if ( iPCNT = PCNT_MAX ) then
iCSTATE <= "1100" ;
iPCNT <= 0 ;
iLCNT <= iLCNT + 1 ;
else
iCSTATE <= "0111" ;
end if ;
-- get upper data
when 7 => if ( iPTRG = '1' and iHREF = '1' ) then
iCSTATE <= "1111" ;
iGDAT(15 downto 8) <= GDAT ;
else
iCSTATE <= "0111" ;
end if ;
-- get lower data
when 15 => if ( iPTRG = '1' and iHREF = '1' ) then
iCSTATE <= "1110" ;
iGDAT(7 downto 0) <= GDAT ;
else
iCSTATE <= "1111" ;
end if ;
-- store
when 14 => iCSTATE <= "0011" ;
if ( 13 < iPCNT and iPCNT < 147 ) then
if ( iLCNT = 8 or iLCNT = 16 or iLCNT = 24 or iLCNT = 32 or
iLCNT = 40 or iLCNT = 48 or iLCNT = 56 ) then
--
iFINDEX <= iFINDEX + 1 ;
end if ;
end if ;
iPCNT <= iPCNT + 1 ;
-- pixel loop
when 12 => iCSTATE <= "0001" ;
-- return first state
when 8 => iCSTATE <= "0000" ;
-- default
when others =>
iCSTATE <= "0000" ;
end case ;
end if ;
end process ;
-- BUS interface
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iTRG_SFT <= "000" ;
elsif rising_edge( CLOCK ) then
iTRG_SFT <= iTRG_SFT(1 downto 0) & TRG ;
end if ;
end process ;
iTRG <= '1' when ( iTRG_SFT = "011" or iTRG_SFT = "001" ) else '0' ;
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iDSTATE <= "00" ;
iDIRF <= "00" ;
iDIRR <= "00" ;
iDIR <= "00" ;
iDUTY <= (others => '0') ;
iDUTYF <= (others => '0') ;
iDUTYR <= (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" ;
iDIR <= DIR ;
iSEL <= BIO(7) ;
iDUTY <= BIO(6 downto 0) ;
-- deliver
when 3 => iDSTATE <= "10" ;
if ( iSEL = '1' ) then
iDIRR <= iDIR ;
iDUTYR <= iDUTY ;
else
iDIRF <= iDIR ;
iDUTYF <= iDUTY ;
end if ;
-- return first state
when 2 => iDSTATE <= "00" ;
-- default
when others =>
iDSTATE <= "00" ;
end case ;
end if ;
end process ;
process (iMCLK)
begin
if rising_edge(iMCLK) then
iDIR_FRONT <= iDIRF ;
iDIR_REAR <= iDIRR ;
iDUTY_FRONT <= iDUTYF ;
iDUTY_REAR <= iDUTYR ;
end if ;
end process ;
end Behavioral;
VHDLコードの中で、BIOはバスデータの入出力に使って
います。ARMからのDUTY比を受取るときは、入力で動き
センサーデータを要求された場合は、出力になります。
モータへのPWM波形出力は、次のVHDLコードを使ってます。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity pwmax is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ;
-- input
DIR : in std_logic_vector(1 downto 0) ;
-- duty ratio
DUTY : in std_logic_vector(6 downto 0) ;
-- output
POUT : out std_logic_vector(1 downto 0) --;
);
end pwmax;
architecture Behavioral of pwmax is
signal iCNT : integer range 0 to 100 ;
signal iDUTY : integer range 0 to 100 ;
signal iDUTYX : integer range 0 to 100 ;
signal iOUT : std_logic_vector(1 downto 0) ;
signal iDIR : std_logic_vector(1 downto 0) ;
begin
-- output
POUT <= "00" when ( iDIR = "11" ) else (iOUT and iDIR) ;
-- input
iDIR <= DIR ;
-- internal clock
iOUT <= "11" when ( iCNT < iDUTY ) else "00" ;
-- counter
process(nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iCNT <= 0 ;
elsif rising_edge( CLOCK ) then
if ( iCNT = 100 ) then
iCNT <= 0 ;
iDUTY <= iDUTYX ;
else
iCNT <= iCNT + 1 ;
end if ;
end if;
end process;
-- BUS interface
process(nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iDUTYX <= 0 ;
elsif rising_edge( CLOCK ) then
iDUTYX <= conv_integer( DUTY ) ;
end if;
end process;
end Behavioral;
札幌エレクトロニクスセンターの研修室前に
周回コースを用意して、マシンを走らせます。
次の写真では下側から、陽光が入っています。
この陽光がコース上に縞をつくり、カメラでは
それを白ラインと判定し、マシンはコースを外
れていきました。
昼間よりも夜間の方が、カメラによる走行は楽
ということは、情報としてもっていましたが、
カラーのカメラでも、夜間の方が走行しやすい
と確認できました。
カメラを利用したセンサーでは、この縞をなくす
ために、LED照光器を利用します。今回も縞対策に
LED照光器を用意します。
コースの光反射でできる縞があると、レーンチェンジ
時に、本来のセンターラインではなく、縞をセンター
ラインと誤認することになります。
MCR-VCのコースは、昼間に大学の実験室内に設置
されるので、照明の具合はコース上で千差万別。
その対策は、LED照光器と偏光板を使います。
目次
前
次
