目次
前
次
試走前チェック（移動関連）
走行パターンの作成ができたので、試走前チェックに入ります。
試走前に過去に半田付けした制御基板を利用して
ファームウエアの動作を確認。
JICA研修の基板の余りがあったので、必要な部品と
ケーブルを半田付け。
ファームウエアは、ATmega644でRTOSを利用。
ソースコードは、以下。
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long ULONG ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
#define NORMAL 1
#define CRANK 2
#define ROTATE 3
#define LANE 4
#define CHANGE 5
#define BLIND 6
#define DIR_CENTER 0
#define DIR_RIGHT 1
#define DIR_LEFT 2
const prog_char msg_h[] PROGMEM = "? help" ;
const prog_char msg_d[] PROGMEM = "D set duty ratio" ;
const prog_char msg_p[] PROGMEM = "P motor DC power" ;
const prog_char msg_s[] PROGMEM = "S get switch state" ;
const prog_char msg_r[] PROGMEM = "R show road state" ;
const prog_char msg_a[] PROGMEM = "A show active state" ;
volatile UBYTE sbuf[8] ;
volatile UBYTE sindex ;
volatile UBYTE cmd ;
volatile UBYTE dccnt ;
volatile UBYTE leftx ;
volatile UBYTE left ;
volatile UBYTE rightx ;
volatile UBYTE right ;
volatile UBYTE mstate ;
volatile UBYTE dir[2] ;
volatile UBYTE xcnt0 ;
volatile UBYTE xcnt3 ;
volatile UBYTE xcnt4 ;
volatile UBYTE xcnt5 ;
volatile UBYTE xcnt6 ;
void rs_putchar(char x)
{
while ( !(UCSR0A & (1 << UDRE0)) ) {}
UDR0 = x ;
}
void rs_puts(char *x)
{
while ( *x != '\0' ) {
rs_putchar( *x );
x++ ;
}
}
void crlf(void)
{
rs_putchar('\r');
rs_putchar('\n');
}
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 ;
}
void show_help(void)
{
char msg[32];
strcpy_P(msg,msg_h); rs_puts((char *)msg); crlf();
strcpy_P(msg,msg_d); rs_puts((char *)msg); crlf();
strcpy_P(msg,msg_p); rs_puts((char *)msg); crlf();
strcpy_P(msg,msg_s); rs_puts((char *)msg); crlf();
strcpy_P(msg,msg_r); rs_puts((char *)msg); crlf();
strcpy_P(msg,msg_a); rs_puts((char *)msg); crlf();
}
#define FOSC 16000000
#define BAUD 9600
#define MYUBRR (FOSC/16/BAUD)-1
volatile UWORD ndx[13];
volatile UWORD cdx[13];
volatile UWORD rdx[13];
volatile UWORD ldx[13];
volatile UWORD chdx[13];
volatile UWORD bldx[13];
typedef union {
struct {
unsigned B0:1;
unsigned B1:1;
unsigned B2:1;
unsigned B3:1;
unsigned B4:1;
unsigned B5:1;
unsigned B6:1;
unsigned B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP xflags ;
#define WFLAG xflags.BIT.B0
#define UFLAG xflags.BIT.B1
#define EFLAG xflags.BIT.B2
#define OFF 0
#define ON OFF+1
#define NO 0
#define YES NO+1
#define MASKFFFF 0xffff
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define TSK_ID_MAX 8
#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
typedef struct {
void (*tsk)(void);
UWORD wcount ;
} TCBP ;
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
volatile UWORD xready ;
volatile UWORD xsuspend;
volatile UWORD xwaitq ;
volatile UBYTE run_tsk;
volatile TCBP tcb[TSK_ID_MAX];
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void init_usr(void);
void init_road_array(void);
void send_road_white(UBYTE x);
void send_road(UBYTE x);
UBYTE get_start(void);
UBYTE get_dir(void);
void put_motor_power(UBYTE x);
void send_dc(UBYTE ldx,UBYTE rdx);
UBYTE get_sensor(void);
void show_state(UBYTE x);
void show_duty(UBYTE ldx,UBYTE rdx);
/*-------------------------*/
/* RTOS functions protoype */
/*-------------------------*/
void init_os(void);
void update_state(UBYTE tid,UBYTE sta);
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);
UWORD is_tsk_ready(UBYTE tid);
void timer_handler(void);
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);
/*------*/
/* main */
/*------*/
int main(void)
{
TCBP pcur_tsk ;
/* disable interrupt */
cli() ;
/* initialize */
init_usr();
/* initialize monitor */
init_os();
/* enable interrupt */
sei() ;
/* message */
rs_puts((char *)"Hello");
crlf();
/* loop */
run_tsk = TSK_ID0 ;
while ( ON ) {
/* task dispacher and scheduler */
pcur_tsk = tcb[run_tsk] ;
if ( is_tsk_ready( run_tsk ) != NO ) { (*(pcur_tsk.tsk))(); }
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
/* timer handler */
if ( WFLAG == ON ) {
/* clear flag */
WFLAG = OFF ;
/* perform */
timer_handler();
}
}
/* dummy */
return 0 ;
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void init_usr(void)
{
UWORD tmp ;
/* PORT A */
PORTA = 0xff ; /* 11111111 */
DDRA = 0xff ; /* oooooooo */
/* PORT B */
PORTB = 0xc0 ; /* 11000000 */
DDRB = 0xcf ; /* ooiioooo */
/* PORT C */
PORTC = 0xff ; /* 11111111 */
DDRC = 0xff ; /* oooooooo */
/* PORT D */
PORTD = 0x01 ; /* 11110001 */
DDRD = 0x0e ; /* iiiioooi */
/* clear flags */
xflags.DR = 0 ;
/* initialize timer0 */
{
/* clear counter */
TCNT0 = 0 ;
/* set compare value */
OCR0A = 249 ;
OCR0B = 252 ;
/* select compare match */
TCCR0A = (1 << WGM01);
/* prescaler /64 => 250kHz */
TCCR0B = (1 << WGM12) | (3 << CS00) ;
/* enable compare match */
TIMSK0 = (1 << OCIE0A) ;
}
/* initialize timer1 */
{
/* clear counter */
TCNT1 = 0 ;
/* set compare value */
OCR1A = 249 ;
OCR1B = 320 ;
/* prescaler /64 => 250kHz */
TCCR1B = (1 << WGM12) | (3 << CS10) ;
/* enable compare match */
TIMSK1 = (1 << OCIE1A) ;
}
/* initialize SCI_0 */
{
/* clear index */
sindex = 0 ;
/* clear buffer */
*(sbuf+0) = '\0' ;
/* set Baud Rate Registers */
tmp = MYUBRR ;
UBRR0H = tmp / 256 ;
UBRR0L = tmp % 256 ;
/* Enable receive interrupt , receive module and transmit module */
UCSR0B = (1 << RXEN0) |( 1 << TXEN0) | (1 << RXCIE0);
/* 8bits 1 stop bit */
UCSR0C = (3 << UCSZ00);
}
/* others */
dccnt = 0 ;
leftx = 0 ;
left = 0 ;
rightx = 0 ;
right = 0 ;
xcnt0 = 0 ;
init_road_array();
}
void init_road_array(void)
{
/* NORMAL */
*(ndx+ALL_BLACK) = (10 << 8) | 10 ;
*(ndx+ALL_WHITE) = (30 << 8) | 30 ;
*(ndx+LEFT_WHITE) = (10 << 8) | 10 ;
*(ndx+RIGHT_WHITE) = (10 << 8) | 10 ;
*(ndx+CENTER) = (50 << 8) | 50 ;
*(ndx+TINY_RIGHT) = (50 << 8) | 55 ;
*(ndx+RIGHT) = (50 << 8) | 60 ;
*(ndx+BIG_RIGHT) = (50 << 8) | 65 ;
*(ndx+TINY_LEFT) = (55 << 8) | 50 ;
*(ndx+LEFT) = (60 << 8) | 50 ;
*(ndx+BIG_LEFT) = (65 << 8) | 50 ;
*(ndx+BOTH_WHITE) = (10 << 8) | 10 ;
*(ndx+ILLEAGAL) = (10 << 8) | 10 ;
/* CRANK */
*(cdx+ALL_BLACK) = (10 << 8) | 10 ;
*(cdx+ALL_WHITE) = (10 << 8) | 10 ;
*(cdx+LEFT_WHITE) = (10 << 8) | 10 ;
*(cdx+RIGHT_WHITE) = (10 << 8) | 10 ;
*(cdx+CENTER) = (30 << 8) | 30 ;
*(cdx+TINY_RIGHT) = (30 << 8) | 35 ;
*(cdx+RIGHT) = (30 << 8) | 40 ;
*(cdx+BIG_RIGHT) = (30 << 8) | 45 ;
*(cdx+TINY_LEFT) = (35 << 8) | 30 ;
*(cdx+LEFT) = (40 << 8) | 30 ;
*(cdx+BIG_LEFT) = (45 << 8) | 30 ;
*(cdx+BOTH_WHITE) = (10 << 8) | 10 ;
*(cdx+ILLEAGAL) = (10 << 8) | 10 ;
/* ROTATE */
*(rdx+ALL_BLACK) = (10 << 8) | 10 ;
*(rdx+ALL_WHITE) = (30 << 8) | 30 ;
*(rdx+LEFT_WHITE) = (10 << 8) | 50 ;
*(rdx+RIGHT_WHITE) = (50 << 8) | 10 ;
*(rdx+CENTER) = (30 << 8) | 30 ;
*(rdx+TINY_RIGHT) = (30 << 8) | 35 ;
*(rdx+RIGHT) = (30 << 8) | 40 ;
*(rdx+BIG_RIGHT) = (30 << 8) | 45 ;
*(rdx+TINY_LEFT) = (35 << 8) | 30 ;
*(rdx+LEFT) = (40 << 8) | 30 ;
*(rdx+BIG_LEFT) = (45 << 8) | 30 ;
*(rdx+BOTH_WHITE) = (25 << 8) | 25 ;
*(rdx+ILLEAGAL) = (10 << 8) | 10 ;
/* LANE */
*(ldx+ALL_BLACK) = (10 << 8) | 10 ;
*(ldx+ALL_WHITE) = (30 << 8) | 30 ;
*(ldx+LEFT_WHITE) = (10 << 8) | 50 ;
*(ldx+RIGHT_WHITE) = (50 << 8) | 10 ;
*(ldx+CENTER) = (30 << 8) | 30 ;
*(ldx+TINY_RIGHT) = (30 << 8) | 35 ;
*(ldx+RIGHT) = (30 << 8) | 40 ;
*(ldx+BIG_RIGHT) = (30 << 8) | 45 ;
*(ldx+TINY_LEFT) = (35 << 8) | 30 ;
*(ldx+LEFT) = (40 << 8) | 30 ;
*(ldx+BIG_LEFT) = (45 << 8) | 30 ;
*(ldx+BOTH_WHITE) = (10 << 8) | 10 ;
*(ldx+ILLEAGAL) = (10 << 8) | 10 ;
/* CHANGE */
*(chdx+ALL_BLACK) = (10 << 8) | 10 ;
*(chdx+ALL_WHITE) = (50 << 8) | 50 ;
*(chdx+LEFT_WHITE) = (10 << 8) | 50 ;
*(chdx+RIGHT_WHITE) = (50 << 8) | 10 ;
*(chdx+CENTER) = (50 << 8) | 50 ;
*(chdx+TINY_RIGHT) = (55 << 8) | 50 ;
*(chdx+RIGHT) = (60 << 8) | 50 ;
*(chdx+BIG_RIGHT) = (65 << 8) | 50 ;
*(chdx+TINY_LEFT) = (50 << 8) | 55 ;
*(chdx+LEFT) = (50 << 8) | 60 ;
*(chdx+BIG_LEFT) = (50 << 8) | 65 ;
*(chdx+BOTH_WHITE) = (10 << 8) | 10 ;
*(chdx+ILLEAGAL) = (10 << 8) | 10 ;
/* BLIND */
*(bldx+ALL_BLACK) = (10 << 8) | 10 ;
*(bldx+ALL_WHITE) = (50 << 8) | 50 ;
*(bldx+LEFT_WHITE) = (10 << 8) | 50 ;
*(bldx+RIGHT_WHITE) = (50 << 8) | 10 ;
*(bldx+CENTER) = (50 << 8) | 50 ;
*(bldx+TINY_RIGHT) = (55 << 8) | 50 ;
*(bldx+RIGHT) = (60 << 8) | 50 ;
*(bldx+BIG_RIGHT) = (65 << 8) | 50 ;
*(bldx+TINY_LEFT) = (50 << 8) | 55 ;
*(bldx+LEFT) = (50 << 8) | 60 ;
*(bldx+BIG_LEFT) = (50 << 8) | 65 ;
*(bldx+BOTH_WHITE) = (10 << 8) | 10 ;
*(bldx+ILLEAGAL) = (10 << 8) | 10 ;
}
/*
Timer0 interrupt
generate 10ms
*/
ISR(TIMER0_COMPA_vect)
{
/* increment */
xcnt0++ ;
/* judge */
if ( xcnt0 == 10 ) {
/* clear */
xcnt0 = 0 ;
/* set flag */
WFLAG = ON ;
}
}
/*
Timer1 interrupt
generate PWM with 10ms periode
*/
ISR(TIMER1_COMPA_vect)
{
UBYTE bport ;
/* now state */
bport = PORTB ;
/* impress */
bport &= ~0x05 ;
if ( dccnt < leftx ) { bport |= 0x04 ; }
if ( dccnt < rightx ) { bport |= 0x01 ; }
PORTB = bport ;
/* increment */
dccnt++ ;
/* judge */
if ( dccnt == 100 ) {
dccnt = 0 ;
leftx = left ;
rightx = right ;
}
}
/* UART interrupt */
ISR(USART0_RX_vect)
{
UBYTE ch ;
/* get 1 charactor */
ch = UDR0 ;
/* store */
*(sbuf+sindex) = ch ;
/* update index */
sindex++ ;
/* judge */
if ( ch == '\r' ) {
UFLAG = ON ;
sindex = 0 ;
}
}
void send_road_white(UBYTE x)
{
UBYTE bport ;
/* get now value */
bport = PORTB ;
/* initialize */
bport |= 0xc0 ;
/* judge */
if ( x == ALL_WHITE ) { bport &= ~0xc0 ; }
if ( x == LEFT_WHITE ) { bport &= ~0x80 ; }
if ( x == RIGHT_WHITE ) { bport &= ~0x40 ; }
/* impress */
PORTB = bport ;
}
void send_road(UBYTE x)
{
UBYTE cport ;
/* judge */
switch ( x ) {
case ALL_BLACK : cport = 0x00 ; break;
case ALL_WHITE : cport = 0xff ; break;
case LEFT_WHITE : cport = 0xf0 ; break;
case RIGHT_WHITE : cport = 0x0f ; break;
case CENTER : cport = 0x18 ; break;
case TINY_RIGHT : cport = 0x0c ; break;
case RIGHT : cport = 0x06 ; break;
case BIG_RIGHT : cport = 0x03 ; break;
case TINY_LEFT : cport = 0x30 ; break;
case LEFT : cport = 0x60 ; break;
case BIG_LEFT : cport = 0xc0 ; break;
case BOTH_WHITE : cport = 0x81 ; break;
default : cport = 0xaa ; break;
}
/* impress */
PORTC = cport ^ MASKFF ;
/* judge */
send_road_white(x);
}
UBYTE get_start(void)
{
return( !(PINB & 0x10) );
}
UBYTE get_dir(void)
{
return( !(PINB & 0x20) );
}
void put_motor_power(UBYTE x)
{
if ( x ) { PORTB |= 0x02 ; }
else { PORTB &= ~0x02 ; }
}
void send_dc(UBYTE ldx,UBYTE rdx)
{
left = ldx ;
right = rdx ;
}
UBYTE get_sensor(void)
{
UBYTE result ;
/* get data */
result = PIND ;
/* shift */
result >>= 4 ;
/* mask */
result &= MASK0F ;
return result ;
}
#define TDAT_BIT 2
#define TCLK_BIT 3
void show_state(UBYTE x)
{
UBYTE aport ;
/* shift */
aport = (1 << x) ;
/* inverse */
aport ^= MASKFF ;
/* impress */
PORTA = aport ;
}
void show_duty(UBYTE ldx,UBYTE rdx)
{
UBYTE result ;
UBYTE dh;
UBYTE dl;
/* calculate */
dh = (ldx >> 3);
dl = (rdx >> 3);
/* concatenate */
result = (dh << 4) | dl ;
/* inverse */
result ^= MASKFF ;
/* impress */
for ( dl = 0 ; dl < 8 ; dl++ ) {
/* data */
PORTD &= ~(1 << TDAT_BIT) ;
if (result & 0x80 ) { PORTD |= (1 << TDAT_BIT) ; }
/* clock : H */
PORTD |= (1 << TCLK_BIT) ;
/* shift */
result <<= 1 ;
/* clock : L */
PORTD &= ~(1 << TCLK_BIT) ;
}
/* default */
PORTD &= ~(1 << TDAT_BIT) ;
}
/*-------------------------*/
/* RTOS functions protoype */
/*-------------------------*/
void init_os(void)
{
/* clear */
xready = 0 ;
xsuspend = 0 ;
xwaitq = 0 ;
/* create task*/
cre_tsk(TSK_ID0,tsk0_proc);
cre_tsk(TSK_ID1,tsk1_proc);
cre_tsk(TSK_ID2,tsk2_proc);
cre_tsk(TSK_ID3,tsk3_proc);
cre_tsk(TSK_ID4,tsk4_proc);
cre_tsk(TSK_ID5,tsk5_proc);
cre_tsk(TSK_ID6,tsk6_proc);
cre_tsk(TSK_ID7,tsk7_proc);
/* set state */
sta_tsk(TSK_ID0,TTS_READY);
sta_tsk(TSK_ID1,TTS_SUSPEND);
sta_tsk(TSK_ID2,TTS_SUSPEND);
sta_tsk(TSK_ID3,TTS_SUSPEND);
sta_tsk(TSK_ID4,TTS_SUSPEND);
sta_tsk(TSK_ID5,TTS_SUSPEND);
sta_tsk(TSK_ID6,TTS_SUSPEND);
sta_tsk(TSK_ID7,TTS_READY);
/* others */
}
void update_state(UBYTE tid,UBYTE sta)
{
UWORD tmp ;
UWORD tmpx ;
/* generate bit code */
tmp = (1 << tid);
tmpx = tmp ^ MASKFFFF ;
/* READY */
if ( sta == TTS_READY ) {
xready |= tmp;
xsuspend &= tmpx;
xwaitq &= tmpx;
}
/* SUSPEND */
if ( sta == TTS_SUSPEND ) {
xready &= tmpx;
xsuspend |= tmp;
xwaitq &= tmpx;
}
/* WAIT */
if ( sta == TTS_WAIT ) {
xready |= tmp;
xsuspend &= tmpx;
xwaitq |= tmp;
}
}
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
tcb[tid].tsk = tsk ;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
update_state(tid,sta);
}
void rsm_tsk(UBYTE tid)
{
update_state(tid,TTS_READY);
}
void sus_tsk(UBYTE tid)
{
update_state(tid,TTS_SUSPEND);
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
tcb[run_tsk].wcount = x ;
update_state(run_tsk,TTS_WAIT);
}
UWORD is_tsk_ready(UBYTE tid)
{
return( xready & (1 << tid) );
}
void timer_handler(void)
{
UBYTE i ;
UWORD wtmp ;
/* loop */
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
if ( xwaitq & (1 << i) ) {
wtmp = tcb[i].wcount ;
wtmp-- ;
tcb[i].wcount = wtmp ;
if ( wtmp == 0 ) { rsm_tsk(i); }
}
}
}
void tsk0_proc(void)
{
UBYTE xsensor ;
/* show state */
mstate = 0 ;
show_state( mstate );
/* wait trigger */
if ( get_start() == ON ) {
/* enable */
put_motor_power( ON );
}
/* get sensor data */
xsensor = get_sensor();
/* show sensor data */
send_road( xsensor ) ;
/* delay 100ms */
wai_tsk( 10 );
}
void tsk1_proc(void)
{
/* UBYTE xmul ; */
UBYTE flag ;
UBYTE xsensor ;
UWORD tmp ;
/* default */
flag = OFF ;
/* show state */
mstate = 1 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* update motor duty ratio */
tmp = *(ndx+xsensor);
left = (tmp >> 8) & MASKFF ;
right = tmp & MASKFF ;
/* default */
*(dir+0) = DIR_CENTER ;
*(dir+1) = DIR_CENTER ;
/* get sensor */
/*
if ( xsensor == CENTER ) {
climb hill or down hill
if ( sloap ) {
xmul = *(sloapt+sloap) ;
left = (UBYTE)( xmul * left * 1.0 / 100 );
right = (UBYTE)( xmul * right * 1.0 / 100 );
}
}
*/
/* update */
send_dc(left,right);
/* judge */
if ( xsensor == ALL_WHITE ) {
rsm_tsk(CRANK);
flag = ON ;
}
if ( xsensor == LEFT_WHITE ) {
*(dir+0) = DIR_LEFT ;
*(dir+1) = DIR_RIGHT ;
rsm_tsk(LANE);
flag = ON ;
}
if ( xsensor == RIGHT_WHITE ) {
*(dir+0) = DIR_RIGHT ;
*(dir+0) = DIR_LEFT ;
rsm_tsk(LANE);
flag = ON ;
}
/* cyclic */
if ( flag == ON ) {
flag = OFF ;
slp_tsk();
} else {
wai_tsk( 10 );
}
}
void tsk2_proc(void)
{
UBYTE flag ;
UBYTE xsensor ;
UWORD tmp ;
/* default */
flag = OFF ;
/* show state */
mstate = 2 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* update motor duty ratio */
tmp = *(cdx+xsensor);
left = (tmp >> 8) & MASKFF ;
right = tmp & MASKFF ;
/* update */
send_dc(left,right);
/* judge */
if ( xsensor == LEFT_WHITE ) {
*(dir+0) = DIR_LEFT ;
*(dir+1) = DIR_RIGHT ;
}
if ( xsensor == RIGHT_WHITE ) {
*(dir+0) = DIR_RIGHT ;
*(dir+1) = DIR_LEFT ;
}
if ( xsensor == ALL_BLACK ) {
/* initialize sequence counter */
xcnt3 = 0 ;
/* update state */
rsm_tsk(ROTATE);
flag = ON ;
}
/* cyclic */
if ( flag == ON ) {
flag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
/* ROTATE */
void tsk3_proc(void)
{
UBYTE flag ;
UBYTE xsensor ;
/* default */
flag = OFF ;
/* show state */
mstate = 3 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* sequence */
switch ( xcnt3 ) {
/* rotate */
case 0 :
xcnt3 = 1 ;
left = 50 ;
right = 10 ;
if ( *(dir+0) == DIR_RIGHT ) {
left = 10 ;
right = 50 ;
}
break ;
/* judge */
case 1 :
xcnt3 = 1 ;
if ( xsensor == CENTER ) {
left = 30 ;
right = 30 ;
xcnt3 = 2 ;
}
break ;
/* straight and move */
case 2 :
xcnt3 = 3 ;
break ;
/* exit */
case 3 :
rsm_tsk(NORMAL);
flag = ON ;
break ;
/* others */
default:
break;
}
/* update */
send_dc(left,right);
/* cyclic */
if ( flag == ON ) {
xcnt3 = 0 ;
flag = OFF ;
slp_tsk();
} else {
wai_tsk( 100 );
}
}
/* LANE */
void tsk4_proc(void)
{
UBYTE flag ;
UBYTE xsensor ;
UWORD tmp ;
/* default */
flag = OFF ;
/* show state */
mstate = 4 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* update motor duty ratio */
tmp = *(ldx+xsensor);
left = (tmp >> 8) & MASKFF ;
right = tmp & MASKFF ;
/* update */
send_dc(left,right);
/* judge */
if ( xsensor == ALL_BLACK ) {
/* initialize sequence counter */
xcnt5 = 0 ;
/* update state */
rsm_tsk(CHANGE);
flag = ON ;
}
if ( xsensor == LEFT_WHITE ) {
*(dir+0) = DIR_LEFT ;
*(dir+1) = DIR_RIGHT ;
}
if ( xsensor == RIGHT_WHITE ) {
*(dir+0) = DIR_RIGHT ;
*(dir+1) = DIR_LEFT ;
}
/* cyclic */
if ( flag == ON ) {
flag = OFF ;
slp_tsk();
} else {
wai_tsk( 10 );
}
}
/* CHANGE */
void tsk5_proc(void)
{
UBYTE flag ;
UBYTE xsensor ;
/* default */
flag = OFF ;
/* show state */
mstate = 5 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* sequence */
switch ( xcnt5 ) {
/* rotate */
case 0 :
xcnt5 = 1 ;
/* set rotate direction */
left = 50 ;
right = 30 ;
if ( *(dir+0) == DIR_LEFT ) {
left = 30 ;
right = 50 ;
}
break ;
/* straight */
case 1 :
xcnt5 = 2 ;
left = 30 ;
right = 30 ;
break ;
/* move */
case 2 :
xcnt5 = 3 ;
break ;
/* exit */
case 3 :
rsm_tsk(BLIND);
flag = ON ;
break ;
/* others */
default:
break;
}
/* update */
send_dc(left,right);
/* cyclic */
if ( flag == ON ) {
flag = OFF ;
xcnt5 = 0 ;
xcnt6 = 0 ;
slp_tsk();
} else {
wai_tsk( 10 );
}
}
/* BLIND */
void tsk6_proc(void)
{
UBYTE flag ;
UBYTE xsensor ;
/* default */
flag = OFF ;
/* show state */
mstate = 6 ;
show_state( mstate );
/* get sensor */
xsensor = get_sensor();
send_road( xsensor );
/* sequence */
switch ( xcnt6 ) {
/* straight */
case 0 :
xcnt6 = 1 ;
left = 30 ;
right = 30 ;
break ;
/* judge and move */
case 1 :
xcnt6 = 1 ;
if ( xsensor == CENTER ) { xcnt6 = 2 ; }
if ( xsensor == TINY_LEFT ) { xcnt6 = 2 ; }
if ( xsensor == TINY_RIGHT ) { xcnt6 = 2 ; }
break ;
/* rotate */
case 2 :
xcnt6 = 3 ;
/* set rotate direction */
left = 50 ;
right = 30 ;
if ( *(dir+1) == DIR_LEFT ) {
left = 30 ;
right = 50 ;
}
break ;
/* move */
case 3 :
xcnt6 = 4 ;
left = 30 ;
right = 30 ;
break ;
/* exit */
case 4 :
rsm_tsk(NORMAL);
flag = ON ;
break ;
/* others */
default:
break;
}
/* update */
send_dc(left,right);
/* cyclic */
if ( flag == ON ) {
flag = OFF ;
xcnt6 = 0 ;
slp_tsk();
} else {
wai_tsk( 10 );
}
}
void tsk7_proc(void)
{
UBYTE tmp[4] ;
if ( UFLAG == ON ) {
/* clear flag */
UFLAG = OFF ;
/* get command */
cmd = *(sbuf+0);
/* new line */
crlf();
/* help */
if ( cmd == '?' ) { show_help() ; }
/* enable */
if ( cmd == 'P' ) {
/* get code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* impress */
put_motor_power( *(tmp+0) );
}
/* set duty ratio */
if ( cmd == 'D' ) {
/* get left duty */
*(tmp+0) = get_hex( *(sbuf+1) );
*(tmp+1) = get_hex( *(sbuf+2) );
*(tmp+2) = *(tmp+0) * 10 + *(tmp+1) ;
/* get right duty */
*(tmp+0) = get_hex( *(sbuf+3) );
*(tmp+1) = get_hex( *(sbuf+4) );
*(tmp+3) = *(tmp+0) * 10 + *(tmp+1) ;
/* send */
send_dc( *(tmp+2) , *(tmp+3) );
show_duty( *(tmp+2) , *(tmp+3) );
}
/* show switch state */
if ( cmd == 'S' ) {
/* get parameter */
*(tmp+0) = *(sbuf+1) ;
/* perform */
if ( *(tmp+0) == '0' ) {
*(tmp+1) = get_start();
/* show */
rs_putchar( *(tmp+1) + '0' );
}
if ( *(tmp+0) == '1' ) {
*(tmp+3) = get_dir();
rs_putchar( *(tmp+3) + '0' );
}
/* new line */
crlf();
}
/* show road state */
if ( cmd == 'R' ) {
/* get command code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* perform */
send_road( *(tmp+0) ) ;
}
/* show active state */
if ( cmd == 'A' ) {
/* get command code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* perform */
show_state( *(tmp+0) ) ;
}
}
}
API関数とRTOSを使ったので、I/O部分を利用マイコンに合わせて
他の部分はほぼ同じにしています。
ATmega644は、フラッシュROMが64kバイト、SRAMが8kバイトありますが
どちらも１０％くらいしか利用していません。
画像処理は、カメラとマイコンの組み合わせで別ユニット
にしたので、この程度のプログラムサイズで済んでいます。
机上でのテストは、次のモータユニットを使いました。
実際にコースを走るマシンは、DCモータとして産業用を
取り付けてあります。
制御基板は構成が同じなので、着脱するだけで済ませられます。
シリアルインタフェースを接続し、１文字コマンドを使い
利用してる関数とハードウエアをチェック
モータを回転させる制御信号は、基板上のモニタLEDを使って
確認できるようにしてあります。
モータを接続する前にLEDの点灯状態で、DUTY比の設定値も
ある程度判断できました。
RTOSを利用している関係で、どのタスクが実行されているかを
８個のLEDのうち、どこが点灯するかを判断できます。
８個のLEDの点灯状態をチェック。
制御基板には２ブロックの８個のLEDがあるので、緑のLED
ブロックを利用しました。
４ビットのセンサー情報から、路面状態をモニタする
LEDブロックは、赤のLEDを利用。
全白線、右片側白線、左片側白線をモニタできるかテスト。
RTOSのタスクの中に、デバッグ専用タスクを入れて対応できました。
タスクは、コマンドインタプリタで構成。
void tsk7_proc(void)
{
UBYTE tmp[4] ;
if ( UFLAG == ON ) {
/* clear flag */
UFLAG = OFF ;
/* get command */
cmd = *(sbuf+0);
/* new line */
crlf();
/* help */
if ( cmd == '?' ) { show_help() ; }
/* enable */
if ( cmd == 'P' ) {
/* get code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* impress */
put_motor_power( *(tmp+0) );
}
/* set duty ratio */
if ( cmd == 'D' ) {
/* get left duty */
*(tmp+0) = get_hex( *(sbuf+1) );
*(tmp+1) = get_hex( *(sbuf+2) );
*(tmp+2) = *(tmp+0) * 10 + *(tmp+1) ;
/* get right duty */
*(tmp+0) = get_hex( *(sbuf+3) );
*(tmp+1) = get_hex( *(sbuf+4) );
*(tmp+3) = *(tmp+0) * 10 + *(tmp+1) ;
/* send */
send_dc( *(tmp+2) , *(tmp+3) );
show_duty( *(tmp+2) , *(tmp+3) );
}
/* show switch state */
if ( cmd == 'S' ) {
/* get parameter */
*(tmp+0) = *(sbuf+1) ;
/* perform */
if ( *(tmp+0) == '0' ) {
*(tmp+1) = get_start();
/* show */
rs_putchar( *(tmp+1) + '0' );
}
if ( *(tmp+0) == '1' ) {
*(tmp+3) = get_dir();
rs_putchar( *(tmp+3) + '0' );
}
/* new line */
crlf();
}
/* show road state */
if ( cmd == 'R' ) {
/* get command code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* perform */
send_road( *(tmp+0) ) ;
}
/* show active state */
if ( cmd == 'A' ) {
/* get command code */
*(tmp+0) = get_hex( *(sbuf+1) );
/* perform */
show_state( *(tmp+0) ) ;
}
}
}
目次
前
次