目次
前
次
ファームウエアソースコード
ATmega644Pを利用した場合のプログラムは、以下です。
通信速度は、57600bpsでカメラとデバッグ用PCに接続します。
割込みは、以下の種類を利用しています。
- タイマー０割込み（PWM波形生成に利用）
- タイマー１割込み（1msの時間待ちに利用）
- USART0受信割込み（カメラからの応答に利用）
- USART1受信割込み（PCからの応答に利用）
- INT2割込み （スタート、ストップトリガーに利用）
ポートの各ビット割当ては、次のようにしています。
Port B SPIインタフェースと外部割込み
- PB7 SCLK (output)
- PB6 MISO (input)
- PB5 MOSI (output)
- PB4 nSS (output)
- PB3 -- (output)
- PB2 INT2 (input)
- PB1 -- (output)
- PB0 -- (output)
Port C PWM波形出力
- PC7 -- (output)
- PC6 -- (output)
- PC5 -- (output)
- PC4 -- (output)
- PC3 LED (output)
- PC2 servo (output)
- PC1 l_motor (output)
- PC0 r_motor (output)
Port D USART通信
- PD7 monitor (output)
- PD6 -- (output)
- PD5 -- (output)
- PD4 -- (output)
- PD3 TxD1 (output)
- PD2 RxD1 (input)
- PD1 TxD0 (output)
- PD0 RxD0 (input)
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#define FOSC 16000000
#define BAUD 57600
#define MYUBRR (FOSC/16/BAUD)-1
#define OFF 0
#define ON OFF+1
#define BUFSIZE 8
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long ULONG ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
typedef union {
struct {
unsigned char B0:1;
unsigned char B1:1;
unsigned char B2:1;
unsigned char B3:1;
unsigned char B4:1;
unsigned char B5:1;
unsigned char B6:1;
unsigned char B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP x_flags ;
#define SFLAG x_flags.BIT.B0
#define BFLAG x_flags.BIT.B1
#define CFLAG x_flags.BIT.B2
#define ZFLAG x_flags.BIT.B3
#define GFLAG x_flags.BIT.B4
#define LFLAG x_flags.BIT.B5
#define DFLAG x_flags.BIT.B7
volatile FLAGSP y_flags ;
#define UFLAG y_flags.BIT.B0
#define XFLAG y_flags.BIT.B1
volatile FLAGSP s_flags ;
#define SBUSY s_flags.BIT.B0
#define SESTABLISH s_flags.BIT.B1
#define SINITIALIZE s_flags.BIT.B2
#define STHRESOLD s_flags.BIT.B3
#define SZFLAG s_flags.BIT.B4
#define SCRANK s_flags.BIT.B5
#define SLANERIGHT s_flags.BIT.B6
#define SLANELEFT s_flags.BIT.B7
#define RING_BSIZE 512
typedef struct {
UBYTE data[RING_BSIZE];
UWORD capacity;
UWORD rdp;
UWORD wrp;
} RINGP ;
volatile RINGP rings;
volatile UBYTE sbuf[BUFSIZE] ;
volatile UBYTE sindex;
#define DAT_MAX 480
#define LINE_MAX 60
volatile UBYTE dummy[12];
volatile UBYTE gdat[DAT_MAX] ;
volatile UBYTE sdat[LINE_MAX] ;
volatile UBYTE gindex ;
volatile UBYTE threshold ;
volatile ULONG tickcount ;
#define NO 0
#define YES NO+1
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
volatile UBYTE cport ;
volatile UBYTE pcnt ;
volatile UBYTE lcnt ;
volatile UBYTE duty[4];
volatile UBYTE dutyx[4];
const prog_char asc_code[] PROGMEM = "0123456789ABCDEF" ;
const prog_char msg_h[] PROGMEM = "?:help" ;
const prog_char msg_c[] PROGMEM = "C:establish" ;
const prog_char msg_i[] PROGMEM = "I:initialize" ;
const prog_char msg_g[] PROGMEM = "G:get image" ;
const prog_char msg_a[] PROGMEM = "A:sensor data" ;
const prog_char msg_s[] PROGMEM = "S:status" ;
volatile UBYTE mode ;
volatile UWORD dtim ;
volatile UBYTE state ;
volatile UBYTE dc_left ;
volatile UBYTE dc_right ;
volatile UBYTE do_left ;
volatile UBYTE do_right ;
volatile UBYTE lane_direction ;
#define RMOTOR 0
#define LMOTOR 1
#define SERVO 2
#define LED 3
#define PMAX 200
#define SERVO_MID 15
#define LED_MAX 180
#define IDLE 0
#define RUN IDLE+1
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define LITTLE_LEFT 5
#define LEFT 6
#define BIG_LEFT 7
#define LITTLE_RIGHT 8
#define RIGHT 9
#define BIG_RIGHT 10
#define CRANK_RIGHT 11
#define CRANK_LEFT 12
#define LANE_RIGHT 0
#define LANE_LEFT 1
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void rs_putchar(UBYTE x);
void rs_put_txd1(UBYTE x);
void rs_puts_txd1(UBYTE *x);
void rs_crlf(void);
void usart_interpret(void);
UBYTE establish_c328(void);
void send_c328(UBYTE x,UWORD p0,UWORD p1);
void send_sync(void);
void send_ack(UBYTE x);
void send_init(void);
void send_snapshot(void);
void send_get_img(void);
void delay_ms(UWORD x);
void put_ring(UBYTE x);
UBYTE get_ring(void);
UWORD get_ring_cap(void);
void init_ring(void);
UBYTE init_c328(void);
UBYTE get_c328(void);
UBYTE generate_threshold(void);
UBYTE is_middle(UWORD x);
UBYTE is_edge(UWORD x);
UBYTE get_sensor(void);
void update_motor(UBYTE ld,UBYTE rd);
void rotate(SBYTE x);
/*------*/
/* main */
/*------*/
int main(void)
{
UBYTE sensor_data ;
SBYTE angle0 ;
SBYTE angle1 ;
/* disable interrupt */
cli();
/* initialize port and variables */
user_initialize();
/* enable interrupt */
sei();
/* establish communication bitween c328 and mega328 */
SESTABLISH = establish_c328() ;
/* initialzie c328 (dummy shot) */
SINITIALIZE = init_c328() ;
/* generate threshold (dummy shot) */
STHRESOLD = generate_threshold() ;
/* initialzie c328 */
SINITIALIZE = init_c328() ;
/* generate threshold */
STHRESOLD = generate_threshold() ;
/* */
BFLAG = get_c328();
/* endless loop */
while ( ON ) {
/* INT2 */
if ( SFLAG == ON ) {
SFLAG = OFF ;
/* change mode */
if ( mode == IDLE ) { mode = RUN ; }
else { mode = IDLE ; }
}
/* monitor */
if ( LFLAG == ON ) {
LFLAG = OFF ;
/* update counter */
lcnt++ ;
PORTD &= ~(1 << 7) ;
if ( lcnt & 1 ) { PORTD |= (1 << 7) ; }
}
/* command interpreter */
if ( UFLAG == ON ) {
UFLAG = OFF ;
/* USART communication process */
usart_interpret();
}
/* ? IDLE or RUN */
if ( mode == IDLE ) {
update_motor(0,0);
XFLAG = OFF ;
} else {
dtim = 1000 ;
/* initial moving */
if ( XFLAG == OFF ) {
dc_left = 50 ;
dc_left = 50 ;
update_motor(dc_left,dc_left);
delay_ms(3000);
XFLAG = ON ;
}
/* get sensor data */
sensor_data = get_sensor();
/* state machine */
switch ( state ) {
case 0 : /* judge */
state = 0 ;
if ( sensor_data == ALL_WHITE ) { state = 100 ; }
if ( sensor_data == LEFT_WHITE ) { state = 200 ; }
if ( sensor_data == RIGHT_WHITE ) { state = 200 ; }
if ( sensor_data == CENTER ) { state = 10 ; }
if ( sensor_data == LITTLE_LEFT ) { state = 20 ; }
if ( sensor_data == LEFT ) { state = 30 ; }
if ( sensor_data == BIG_LEFT ) { state = 40 ; }
if ( sensor_data == LITTLE_RIGHT ) { state = 50 ; }
if ( sensor_data == RIGHT ) { state = 60 ; }
if ( sensor_data == BIG_RIGHT ) { state = 70 ; }
break ;
/* CENTER */
case 10 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
dc_left <<= 3 ; dc_left /= 10 ;
dc_right <<= 3 ; dc_right /= 10 ;
update_motor(dc_left,dc_left);
break ;
/* LITTLE_LEFT (move center) */
case 20 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
dc_left *= 95 ; dc_left /= 100 ;
update_motor(dc_left,dc_left);
break ;
/* LEFT (move center) */
case 30 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
do_right *= 9 ; do_right /= 10 ;
update_motor(dc_left,dc_left);
break ;
/* BIG_LEFT (move center) */
case 40 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
do_right <<= 3 ; do_right /= 10 ;
update_motor(dc_left,dc_left);
break ;
/* LITTLE_RIGHT (move center) */
case 50 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
dc_left *= 95 ; dc_left /= 100 ;
update_motor(dc_left,dc_left);
break ;
/* RIGHT (move center) */
case 60 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
dc_left *= 9 ; dc_left /= 10 ;
update_motor(dc_left,dc_left);
break ;
/* BIG_RIGHT (move center) */
case 70 :
state = 80 ;
do_left = dc_left ;
do_right = dc_right ;
do_left <<= 3 ; do_left /= 10 ;
update_motor(dc_left,dc_left);
break ;
/* delay */
case 80 :
state = 90 ;
delay_ms( dtim ) ;
break ;
/* return */
case 90 :
state = 0 ;
dc_left = do_left ;
dc_right = do_right ;
break ;
/* CRANK */
case 100 : /* slow move */
state = 100 ;
update_motor(20,20);
delay_ms(1000);
if ( sensor_data == CRANK_RIGHT ) {
state = 110 ;
angle0 = -60 ;
angle1 = -5 ;
}
if ( sensor_data == CRANK_LEFT ) {
state = 110 ;
angle0 = 60 ;
angle1 = 5 ;
}
break ;
case 110 : /* rotate right */
state = 120 ;
rotate(angle0);
delay_ms(3000);
break ;
case 120 : /* rotate right (bit by bit) */
state = 120 ;
if ( sensor_data == CENTER ) { state = 130 ; }
else { rotate(angle1) ; delay_ms(100); }
break ;
case 130 : /* slow move */
state = 140 ;
update_motor(20,20);
delay_ms(1000);
break ;
case 150 : /* return */
state = 0 ;
break ;
/* LANE */
case 200 : /* slow move */
state = 200 ;
update_motor(20,20);
delay_ms(1000);
if ( sensor_data == ALL_BLACK ) {
state = 210 ;
angle0 = -30 ;
angle1 = 30 ;
if ( lane_direction == LANE_LEFT ) {
angle0 *= -1 ;
angle1 *= -1 ;
}
}
break ;
case 210 : /* rotate */
state = 220 ;
rotate(angle0);
delay_ms(1000);
break ;
case 220 : /* move */
state = 220 ;
if ( sensor_data == CENTER ) { state = 230 ; }
else {
update_motor(20,20);
delay_ms(1000);
}
break ;
case 230 : /* roate (reverse) */
state = 240 ;
rotate(angle1);
delay_ms(1000);
break ;
case 240 : /* slow move */
state = 250 ;
update_motor(20,20);
delay_ms(1000);
break ;
case 250 : /* return */
state = 0 ;
break ;
}
/* establish communication bitween c328 and mega328 */
SESTABLISH = establish_c328() ;
/* initialzie c328 (dummy shot) */
SINITIALIZE = init_c328() ;
/* prepare next state processing */
BFLAG = get_c328();
}
}
/* dummy */
return 0 ;
}
#/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
UWORD tmp ;
/* PORT A */
PORTA = 0b00000000 ; /* 00000000 */
DDRA = 0b11111111 ; /* oooooooo */
/* PORT B */
PORTB = 0b00010100 ; /* 00000000 */
DDRB = 0b10111011 ; /* oioooioo */
/* PORT C */
PORTC = 0b00000000 ; /* 00000000 */
DDRC = 0b00111111 ; /* iioooooo */
/* PORT D */
PORTD = 0b00000000 ; /* 00000000 */
DDRD = 0b11111010 ; /* oooooioi */
/* */
x_flags.DR = 0 ;
y_flags.DR = 0 ;
s_flags.DR = 0 ;
sindex = 0 ;
gindex = 0 ;
tickcount = 0 ;
mode = IDLE ;
/* initialize timer0 */
{
/* clear counter */
TCNT0 = 0 ;
/* set compare value */
OCR0A = 199 ;
OCR0B = 250 ;
/* inhibit compare match output , select CTC mode */
TCCR0A = (1 << WGM01) ;
/* prescaler /8 => 2MHz */
TCCR0B = (1 << CS01) ;
/* enable compare match */
TIMSK0 = (1 << OCIE0A) ;
}
/* initialzie duty ratio */
pcnt = 0 ;
lcnt = 0 ;
/* initialize duty */
*(duty+RMOTOR) = 0 ;
*(duty+LMOTOR) = 0 ;
*(duty+SERVO) = SERVO_MID ;
*(duty+LED) = LED_MAX;
/* initialize duty (interrupt) */
*(dutyx+RMOTOR) = 0 ;
*(dutyx+LMOTOR) = 0 ;
*(dutyx+SERVO) = SERVO_MID ;
*(dutyx+LED) = LED_MAX;
/* initialize timer1 */
{
/* clear counter */
TCNT1 = 0 ;
/* set compare value */
OCR1A = 1999 ;
OCR1B = 3200 ;
/* prescaler /8 => 2MHz */
TCCR1B = (1 << WGM12) | (1 << CS11) ;
/* enable compare match */
TIMSK1 = (1 << OCIE1A) ;
}
/* clear index */
sindex = 0 ;
/* clear buffer */
*(sbuf+0) = 0 ;
/* initialize SCI0 */
{
/* set Baud Rate Registers */
tmp = MYUBRR ;
UBRR0H = (UBYTE)((tmp >> 8) & MASKFF) ;
UBRR0L = (UBYTE)(tmp & MASKFF) ;
/* Enable receive interrupt , receive module and transmit module */
UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0) ;
/* 8 bits , 1 stop bit , no parity */
UCSR0C = (3 << UCSZ00);
}
/* */
init_ring();
/* initialize SCI1 */
{
/* set Baud Rate Registers */
tmp = MYUBRR ;
UBRR1H = (UBYTE)((tmp >> 8) & MASKFF) ;
UBRR1L = (UBYTE)(tmp & MASKFF) ;
/* Enable receive interrupt , receive module and transmit module */
UCSR1B = (1 << RXCIE1) | (1 << RXEN1) | (1 << TXEN1) ;
/* 8 bits , 1 stop bit , no parity */
UCSR1C = (3 << UCSZ10);
}
}
/* UART receive interrupt */
ISR(USART0_RX_vect)
{
volatile UBYTE ch ;
if ( !(UCSR0B & (1 << FE0)) ) {
/* get 1 charactoer */
ch = UDR0 ;
/* store */
put_ring( ch );
}
}
/* UART receive interrupt */
ISR(USART1_RX_vect)
{
volatile UBYTE ch ;
if ( !(UCSR1B & (1 << FE1)) ) {
/* get 1 charactoer */
ch = UDR1 ;
/* store */
*(sbuf+sindex) = ch ;
/* update */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sindex = 0 ;
UFLAG = ON ;
}
}
}
void rs_putchar(UBYTE x)
{
while ( !(UCSR0A & (1 << UDRE0)) ) {}
UDR0 = x ;
}
void rs_put_txd1(UBYTE x)
{
while ( !(UCSR1A & (1 << UDRE1)) ) {}
UDR1 = x ;
}
void rs_puts_txd1(UBYTE *x)
{
while ( *x != '\0' ){
rs_put_txd1(*x) ;
x++ ;
}
rs_crlf();
}
void rs_crlf(void)
{
rs_put_txd1('\r') ;
rs_put_txd1('\n') ;
}
/* timer0 comare match interrupt */
ISR(TIMER0_COMPA_vect)
{
/* initialize */
cport = 0 ;
/* judge */
if ( pcnt <= *(dutyx+RMOTOR) ) { cport |= 1 ; }
if ( pcnt <= *(dutyx+LMOTOR) ) { cport |= 2 ; }
if ( pcnt <= *(dutyx+SERVO) ) { cport |= 4 ; }
if ( pcnt <= *(dutyx+LED) ) { cport |= 8 ; }
/* set */
PORTC = cport ;
/* increment */
pcnt++ ;
if ( pcnt == PMAX ) {
pcnt = 0 ;
/* update */
*(dutyx+RMOTOR) = *(duty+RMOTOR) ;
*(dutyx+LMOTOR) = *(duty+LMOTOR) ;
*(dutyx+SERVO) = *(duty+SERVO) ;
*(dutyx+LED) = *(duty+LED) ;
/* monitor */
LFLAG = ON ;
}
}
/* INT2 interrupt */
ISR(INT2_vect)
{
SFLAG = ON ;
}
#define SEL_BIT 5
#define LT_BIT 4
#define FLG_BIT 3
#define KEEP_INDEX 30
#define MASK0F 0x0f
/* USART interpreter */
void usart_interpret(void)
{
UBYTE cmd ;
UBYTE i ;
UBYTE tmp ;
char msg[20];
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) {
strcpy_P(msg,msg_h); rs_puts_txd1((UBYTE *)msg) ;
strcpy_P(msg,msg_c); rs_puts_txd1((UBYTE *)msg) ;
strcpy_P(msg,msg_i); rs_puts_txd1((UBYTE *)msg) ;
strcpy_P(msg,msg_g); rs_puts_txd1((UBYTE *)msg) ;
strcpy_P(msg,msg_a); rs_puts_txd1((UBYTE *)msg) ;
strcpy_P(msg,msg_s); rs_puts_txd1((UBYTE *)msg) ;
}
/* establish communication */
if ( cmd == 'C' ) { establish_c328(); }
/* initialize c328 */
if ( cmd == 'I' ) { init_c328(); }
/* get image from c328 and convert data to sensor data */
if ( cmd == 'G' ) { get_c328(); }
/* show sensor data */
if ( cmd == 'A' ) {
for ( i = 0 ; i < 60 ; i++ ) {
/* get code */
tmp = *(sdat+i) ;
/* separate */
rs_put_txd1( pgm_read_byte(&asc_code[((tmp >> 4) & MASK0F)]) );
rs_put_txd1( pgm_read_byte(&asc_code[(tmp & MASK0F)]) );
rs_crlf();
}
}
/* show status */
if ( cmd == 'S' ) {
if ( SBUSY ) { rs_puts_txd1((UBYTE *)"BUSY"); }
if ( SESTABLISH ) { rs_puts_txd1((UBYTE *)"connected"); }
else { rs_puts_txd1((UBYTE *)"disconnected"); }
rs_puts_txd1((UBYTE *)"threshold ");
if ( STHRESOLD ) { rs_puts_txd1((UBYTE *)" exists"); }
else { rs_puts_txd1((UBYTE *)" not existed"); }
if ( SCRANK ) { rs_puts_txd1((UBYTE *)"crank mode"); }
if ( SLANERIGHT ) { rs_puts_txd1((UBYTE *)"right lane"); }
if ( SLANELEFT ) { rs_puts_txd1((UBYTE *)"left lane"); }
}
}
UBYTE establish_c328(void)
{
UWORD i ;
UWORD length ;
UBYTE result ;
UBYTE gflag ;
UBYTE yflag0 ;
UBYTE yflag1 ;
/* default */
gflag = OFF ;
yflag0 = OFF ;
yflag1 = OFF ;
if ( DFLAG ) { rs_puts_txd1((UBYTE *)" _SYNC") ; }
/* flush */
length = get_ring_cap() ;
if ( length ) {
for ( i = 0 ; i < length ; i++ ) { result = get_ring() ; }
}
/* looping */
for ( i = 0 ; i < 60 ; i++ ) {
/* send SYNC command */
send_sync();
/* wait */
delay_ms( 10 ) ;
/* judge */
if ( get_ring_cap() < 12 ) continue ;
/* get data from buffer */
length = get_ring_cap() ;
for ( i = 0 ; i < length ; i++ ) {
if ( i > 11 ) { result = get_ring() ; }
else { *(dummy+i) = get_ring() ; }
}
/* */
gflag = ON ;
if ( gflag ) break ;
}
/* judge */
if ( *(dummy+1) == 0x0E && *(dummy+2) == 0x0D ) { yflag0 = ON ; }
if ( *(dummy+7) == 0x0D && *(dummy+8) == 0x00 ) { yflag1 = ON ; }
/* send ACK */
result = gflag & yflag0 & yflag1 ;
if ( result ) {
send_ack( 0x0D ) ;
if ( DFLAG ) { rs_puts_txd1((UBYTE *)" _established") ; }
if ( yflag0 ) { rs_puts_txd1((UBYTE *)" _ACK OK! ") ; }
if ( yflag1 ) { rs_puts_txd1((UBYTE *)" _SYNC send ") ; }
}
return result ;
}
void send_c328(UBYTE x,UWORD p0,UWORD p1)
{
rs_putchar( 0xAA );
rs_putchar( x );
rs_putchar( (UBYTE)((p0 >> 8) & MASKFF) );
rs_putchar( (UBYTE)(p0 & MASKFF) );
rs_putchar( (UBYTE)((p1 >> 8) & MASKFF) );
rs_putchar( (UBYTE)(p1 & MASKFF) );
}
void send_sync(void)
{
send_c328(0x0D,0x0000,0x0000);
}
void send_ack(UBYTE x)
{
send_c328(0x0D,(x << 8) | 0x00,0x0000);
}
/* Timer1 interrupt */
ISR(TIMER1_COMPA_vect)
{
/* increment */
tickcount++ ;
}
void delay_ms(UWORD x)
{
ULONG target ;
/* set target */
target = tickcount + (ULONG)x ;
/* wait */
while( target > tickcount ) ;
}
void put_ring(UBYTE x)
{
UWORD ptmp ;
/* store data */
rings.data[rings.wrp] = x ;
/* pointer increment */
ptmp = rings.wrp ;
ptmp++ ;
if ( ptmp == RING_BSIZE ) { rings.wrp = 0 ; }
/* capacity increment */
ptmp = rings.capacity ;
ptmp++ ;
rings.capacity = ptmp ;
}
UBYTE get_ring(void)
{
UWORD ptmp ;
UBYTE result ;
/* default */
result = 0 ;
/* judge */
ptmp = rings.rdp ;
if ( rings.capacity == 0 ) { return result ; }
/* read data */
result = rings.data[ptmp] ;
/* pointer increment */
ptmp++ ;
if ( ptmp == RING_BSIZE ) { rings.rdp = 0 ; }
/* capacity decrement */
ptmp = rings.capacity ;
if ( ptmp > 0 ) { rings.capacity = ptmp - 1 ; }
return result ;
}
UWORD get_ring_cap(void)
{
return rings.capacity ;
}
void init_ring(void)
{
/* clear capacity */
rings.capacity = 0 ;
/* initialize read pointer */
rings.rdp = 0 ;
/* initialize write pointer */
rings.wrp = 0 ;
}
UBYTE init_c328(void)
{
UWORD i ;
UWORD length ;
UBYTE yflag0 ;
UBYTE yflag1 ;
/* set BUSY */
BFLAG = ON ;
/* default */
yflag0 = OFF ;
/* send SYNC command */
send_init();
/* wait response */
delay_ms(20) ;
/* get data from buffer */
length = get_ring_cap() ;
for ( i = 0 ; i < 6 ; i++ ) { *(dummy+i) = get_ring() ; }
if ( *(dummy+1) == 0x0E && *(dummy+2) == 0x01 ) { yflag0 = ON ; }
/* send snap shot */
send_snapshot();
/* wait response */
delay_ms(20) ;
/* get data from buffer */
length = get_ring_cap() ;
for ( i = 6 ; i < 12 ; i++ ) { *(dummy+i) = get_ring() ; }
if ( *(dummy+7) == 0x0E && *(dummy+8) == 0x05 ) { yflag1 = ON ; }
/* reset BUSY */
BFLAG = OFF ;
return yflag1 & yflag0 ;
}
void send_init(void)
{
send_c328(0x01,0x0003,0x0100);
}
void send_snapshot(void)
{
send_c328( 0x05 , 0x0100 , 0x0000 );
}
void send_get_img(void)
{
send_c328(0x04,0x0100,0x0000);
}
UBYTE get_c328(void)
{
UWORD i ;
UWORD j ;
ULONG length ;
UBYTE k ;
UBYTE resx ;
UBYTE result ;
UBYTE yflag0 ;
UBYTE yflag1 ;
/* set BUSY */
BFLAG = ON ;
/* set status */
SBUSY = ON ;
/* sweap */
length = get_ring_cap();
if ( length ) {
for ( i = 0 ; i < (UWORD)length ; i++ ) { result = get_ring() ;}
}
/* send get picture command */
send_get_img();
/* wait */
while ( ON ) {
result = get_ring_cap() ;
if ( result >= 12 ) break ;
delay_ms(1);
}
/* copy */
for ( i = 0 ; i < 12 ; i++ ) { *(dummy+i) = get_ring() ; }
/* judge */
yflag0 = OFF ;
yflag1 = OFF ;
if ( *(dummy+1) == 0x0E && *(dummy+2) == 0x04 ) { yflag0 = ON ; }
if ( *(dummy+7) == 0x0A && *(dummy+8) == 0x01 ) { yflag1 = ON ; }
/* calculate size */
length = *(dummy+11) ; length <<= 8 ;
length += *(dummy+10) ; length <<= 8 ;
length += *(dummy+9 ) ;
if ( length > 32767 ) {
/* flush */
length = get_ring_cap();
for ( i = 0 ; i < length ; i++ ) { result = get_ring(); }
return OFF ;
}
/* get image data */
k = 0 ;
resx = 0 ;
for ( i = 0 ; i < (UWORD)length ; i++ ) {
/* wait receive graphic data */
while ( get_ring_cap() <= 0 ) ;
/* get 1 pixel */
result = get_ring();
/* judge */
if ( result > threshold ) { resx++ ; }
/* block summuation */
j = i % 10 ;
if ( j == 9 ) {
/* store summuation */
*(gdat+k) = resx ;
/* clear block summuation */
resx = 0 ;
/* pointer increment */
k++ ;
}
}
/* send ACK */
send_ack( 0x0A );
/* generate sensor data */
SCRANK = OFF ;
SLANERIGHT = OFF ;
SLANELEFT = OFF ;
for ( i = 0 ; i < LINE_MAX ; i++ ) {
/* index */
j = (i << 3) ;
/* summuation */
resx = 0 ;
resx += *(gdat+j+0) ;
resx += *(gdat+j+1) ;
resx += *(gdat+j+2) ;
resx += *(gdat+j+3) ;
resx += *(gdat+j+4) ;
resx += *(gdat+j+5) ;
resx += *(gdat+j+6) ;
resx += *(gdat+j+7) ;
/* average */
resx >>= 3 ;
/* convert */
result = 0 ;
if ( *(gdat+j+0) > result ) { result |= 0x80 ; }
if ( *(gdat+j+1) > result ) { result |= 0x40 ; }
if ( *(gdat+j+2) > result ) { result |= 0x20 ; }
if ( *(gdat+j+3) > result ) { result |= 0x10 ; }
if ( *(gdat+j+4) > result ) { result |= 0x08 ; }
if ( *(gdat+j+5) > result ) { result |= 0x04 ; }
if ( *(gdat+j+6) > result ) { result |= 0x02 ; }
if ( *(gdat+j+7) > result ) { result |= 0x01 ; }
/* reverse store */
*(sdat+59-i) = result ;
/* judge CRANK */
if ( result == 0xff ) { SCRANK = ON ; }
if ( result == 0xf0 || result == 0xf8 ) { SLANELEFT = ON ; }
if ( result == 0x0f || result == 0x1f ) { SLANERIGHT = ON ; }
}
/* */
BFLAG = OFF ;
/* reset status */
SBUSY = OFF ;
return ON ;
}
UBYTE generate_threshold(void)
{
UWORD i ;
ULONG length ;
UBYTE resx ;
UBYTE resy ;
UBYTE result ;
UBYTE yflag0 ;
UBYTE yflag1 ;
/* set BUSY */
BFLAG = ON ;
/* set status */
SBUSY = ON ;
/* sweap */
length = get_ring_cap();
if ( length ) {
for ( i = 0 ; i < (UWORD)length ; i++ ) { result = get_ring() ;}
}
/* send get picture command */
send_get_img();
/* wait */
while ( ON ) {
result = get_ring_cap() ;
if ( result >= 12 ) break ;
delay_ms(1);
}
/* copy */
for ( i = 0 ; i < 12 ; i++ ) { *(dummy+i) = get_ring() ; }
/* judge */
yflag0 = OFF ;
yflag1 = OFF ;
if ( *(dummy+1) == 0x0E && *(dummy+2) == 0x04 ) { yflag0 = ON ; }
if ( *(dummy+7) == 0x0A && *(dummy+8) == 0x01 ) { yflag1 = ON ; }
/* calculate size */
length = *(dummy+11) ; length <<= 8 ;
length += *(dummy+10) ; length <<= 8 ;
length += *(dummy+9 ) ;
if ( length > 32767 ) {
/* flush */
length = get_ring_cap();
for ( i = 0 ; i < length ; i++ ) { result = get_ring(); }
return OFF ;
}
/* get image data */
resx = 0 ;
resy = 0 ;
for ( i = 0 ; i < (UWORD)length ; i++ ) {
/* wait receive graphic data */
while ( get_ring_cap() <= 0 ) ;
/* get 1 pixel */
result = get_ring();
/* judge */
if ( is_middle(i) ) { resx += result ; }
if ( is_edge(i) ) { resy += result ; }
/* calculate threshold and store it */
if ( i == 4799 ) {
resx >>= 3 ;
resy >>= 3 ;
threshold = ((resx+resy) >> 1) ;
}
}
/* send ACK */
send_ack( 0x0A );
/* */
BFLAG = OFF ;
/* reset status */
SBUSY = OFF ;
return ON ;
}
UBYTE is_middle(UWORD x)
{
UBYTE result ;
result = OFF ;
if ( x == 1159 ) { result = ON ; }
if ( x == 1160 ) { result = ON ; }
if ( x == 2359 ) { result = ON ; }
if ( x == 2360 ) { result = ON ; }
if ( x == 3559 ) { result = ON ; }
if ( x == 3540 ) { result = ON ; }
if ( x == 4759 ) { result = ON ; }
if ( x == 4760 ) { result = ON ; }
return result ;
}
UBYTE is_edge(UWORD x)
{
UBYTE result ;
result = OFF ;
if ( x == 1120 ) { result = ON ; }
if ( x == 1179 ) { result = ON ; }
if ( x == 2320 ) { result = ON ; }
if ( x == 2399 ) { result = ON ; }
if ( x == 3520 ) { result = ON ; }
if ( x == 3599 ) { result = ON ; }
if ( x == 4720 ) { result = ON ; }
if ( x == 4799 ) { result = ON ; }
return result ;
}
UBYTE get_sensor(void)
{
return *(sdat+29) ;
}
void update_motor(UBYTE ld,UBYTE rd)
{
*(duty+LMOTOR) = (ld << 1);
*(duty+RMOTOR) = (rd << 1);
}
void rotate(SBYTE x)
{
UBYTE dx_left ;
UBYTE dx_right ;
UBYTE tmp ;
/* default */
dx_left = 10 ;
dx_right = 10 ;
if ( x < 0 ) {
tmp = x *(-1);
if ( tmp == 5 ) { dx_right = dx_left+5 ; }
if ( tmp == 30 ) { dx_right = dx_left+15 ; }
if ( tmp == 60 ) { dx_right = dx_left+30 ; }
} else {
tmp = x ;
if ( tmp == 5 ) { dx_left = dx_right+5 ; }
if ( tmp == 30 ) { dx_left = dx_right+15 ; }
if ( tmp == 60 ) { dx_left = dx_right+30 ; }
}
/* update */
update_motor(dx_left,dx_right);
}
目次
前
次
