目次
前
次
移動制御プログラム
移動するには、パルスモータにパルス数を与えます。
マイコンのファームウエアで必要になる関数を考えます。
マイコンとのインタフェースは、8ビットx2の
2ポートを使うことにします。
動作テストには、AVRマイコンを利用します。
AVRマイコンには、XC95108を接続します。
モータが回転中か停止しているかをフラグで提示する
ので、2ビットフラグを入手する関数を定義します。
typedef unsigned char UBYTE ;
UBYTE get_status(void)
{
return( PINC & 3 ) ;
}
左右のモータパルス数と回転方向を与える関数を定義します。
パルス数は12ビット(0〜4095)とし、16ビットの最上位
ビットに1を与えたとき、左回り(counter clockwise)とします。
typedef unsigned short UWORD ;
#define MASK0F 0x0f
#define MASK80 0x80
#define MASK8000 0x8000
void put_pulse_count(UWORD xlp,UWORD xrp)
{
UBYTE xdir ;
UBYTE xcnt[3] ;
UBYTE i ;
/* stop motor */
xdir = 0 ;
*(xcnt+0) = xdir ;
*(xcnt+1) = xdir | 0x80 ;
*(xcnt+2) = xdir ;
for ( i = 0 ; i < 3 ; i++ ) { PORTB = *(xcnt+i) ; }
/* left motor pulse count */
*(xcnt+0) = ((xlp >> 8) & MASK0F) | 0x40 ;
*(xcnt+1) = ((xlp >> 4) & MASK0F) | 0x50 ;
*(xcnt+2) = (xlp & MASK0F) | 0x60 ;
for ( i = 0 ; i < 3 ; i++ ) { impress_portb( *(xcnt+i) ) ; }
PORTB = 0 ;
/* right motor pulse count */
*(xcnt+0) = ((xrp >> 8) & MASK0F) ;
*(xcnt+1) = ((xrp >> 4) & MASK0F) | 0x10 ;
*(xcnt+2) = (xrp & MASK0F) | 0x20 ;
for ( i = 0 ; i < 3 ; i++ ) { impress_portb( *(xcnt+i) ) ; }
PORTB = 0 ;
/* send command */
xdir = 4 ;
if ( xlp & MASK8000 ) { xdir |= 1 ; }
if ( xrp & MASK8000 ) { xdir |= 2 ; }
*(xcnt+0) = xdir ;
*(xcnt+1) = xdir | 0x80 ;
*(xcnt+2) = xdir ;
for ( i = 0 ; i < 3 ; i++ ) { PORTB = *(xcnt+i) ; }
}
void impress_portb(UBYTE x)
{
PORTB = x ;
PORTB = x | MASK80 ;
}
ハードウエアと直接やりとりする関数を定義したので
中間、上位の処理を考えます。
シリアルインタフェースを使い、PersonalComputerから
パルスモータのパルス数を与えてテストします。
シリアルインタフェースを利用するコマンドを
決めます。
- ? ヘルプ
- W 左右の方向とパルス数指定
- R 状態表示
- F 周波数選択
Wコマンドのフォーマットを更に細かく決めます。
左パルス数、右パルス数を10進4けたで与え
回転方向をF、Rで挟み込みます。
W0123F4321R{enter}というように使います。
Fコマンドは、0〜3のいずれかの値を指定します。
F2{enter}というように使います。
この仕様で動くAVRファームウエアは、以下です。
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#define OFF 0
#define ON OFF+1
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
volatile UBYTE sflag ;
#define BUFSIZE 16
volatile UBYTE sbuf[BUFSIZE] ;
volatile UBYTE sindex ;
volatile UBYTE cmd ;
volatile UWORD xleft ;
volatile UWORD xright ;
volatile UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'} ;
#define MASK0F 0x0f
#define MASK80 0x80
#define MASK8000 0x8000
const prog_char msg_h[] PROGMEM = "? help" ;
const prog_char msg_w[] PROGMEM = "W write pulse counts" ;
const prog_char msg_r[] PROGMEM = "R read status" ;
const prog_char msg_f[] PROGMEM = "F select frequency" ;
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void rs_putchar(UBYTE x);
void rs_puts(UBYTE *ptr);
void crlf(void);
UBYTE get_hex(UBYTE x);
void show_help(void) ;
UWORD get_word(UBYTE *ptr);
UBYTE get_status(void);
void put_pulse_count(UWORD xlp,UWORD xrp);
void impress_portb(UBYTE x);
/*------*/
/* main */
/*------*/
int main(void)
{
UBYTE tmp ;
/* initialize */
user_initialize();
/* enable interrupt */
sei();
/* endless loop */
while (ON) {
/* handling ladder diagram infomation */
if ( sflag == ON ) {
/* clear flag */
sflag = OFF ;
/* get command */
cmd = *(sbuf+0) ;
/* help */
if ( cmd == '?' ) { show_help() ; }
/* set pulse counts */
if ( cmd == 'W' ) {
/* get left pulse count */
xleft = get_word( (UBYTE *)(sbuf+1) );
if ( *(sbuf+5) == 'R' ) { xleft |= MASK8000 ; }
/* get right pulse count */
xright = get_word( (UBYTE *)(sbuf+6) );
if ( *(sbuf+10) == 'R' ) { xright |= MASK8000 ; }
/* send */
put_pulse_count(xleft,xright);
}
/* get status */
if ( cmd == 'R' ) {
tmp = get_status();
switch ( tmp ) {
case 1 : rs_puts((UBYTE *)"LEFT idel / RIGHT run") ; break ;
case 2 : rs_puts((UBYTE *)"LEFT run / RIGHT idle") ; break ;
case 3 : rs_puts((UBYTE *)"BOTH run") ; break ;
default : rs_puts((UBYTE *)"BOTH idle") ; break ;
}
}
/* select frequency */
if ( cmd == 'F' ) {
tmp = get_hex( *(sbuf+1) ) | 0x70 ;
PORTB = tmp ;
PORTB = tmp | 0x80 ;
PORTB = tmp ;
}
}
}
/* dummy */
return 0 ;
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
#define FOSC 8000000
#define BAUD 9600
#define MYUBRR (FOSC/16/BAUD)-1
void user_initialize(void)
{
/* PORT A */
PORTA = 0b00000000 ; /* 00000000 */
DDRA = 0b00000000 ; /* iiiiiiii */
/* PORT B */
PORTB = 0b00000000 ; /* 00000000 */
DDRB = 0b11111111 ; /* oooooooo */
/* PORT C */
PORTC = 0b00000011 ; /* 00000011 */
DDRC = 0b00000000 ; /* iiiiiiii */
/* PORT D */
PORTD = 0b00000000 ; /* 00010000 */
DDRD = 0b11111110 ; /* oooooooi */
/* initialize registers */
sflag = OFF ;
/* initialize serial */
{
/* clear index */
sindex = 0 ;
/* clear buffer */
*(sbuf+0) = 0 ;
/* set Baud Rate Registers */
UBRR = MYUBRR ;
/* Enable receive interrupt , receive module and transmit module */
UCR = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE);
}
/* others */
xleft = 0 ;
xright = 0 ;
}
/* UART receive interrupt */
ISR(SIG_UART_RECV)
{
UBYTE ch ;
/* get 1 charactoer */
ch = UDR ;
/* store */
*(sbuf+sindex) = ch ;
sindex++ ;
/* judge */
if ( ch == '\r' ) {
sflag = ON ;
sindex = 0 ;
}
}
void rs_putchar(UBYTE x)
{
while ( !(USR & (1 << UDRE)) ) {}
UDR = x ;
}
void rs_puts(UBYTE *ptr)
{
while ( *ptr != '\0' ) {
rs_putchar( *ptr ) ;
ptr++ ;
}
crlf();
}
void crlf(void)
{
rs_putchar('\r');
rs_putchar('\n');
}
UBYTE get_hex(UBYTE x)
{
volatile UBYTE result ;
/* default */
result = 0 ;
/* select */
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[16];
strcpy_P(msg,msg_h); rs_puts((UBYTE *)msg);
strcpy_P(msg,msg_w); rs_puts((UBYTE *)msg);
strcpy_P(msg,msg_r); rs_puts((UBYTE *)msg);
strcpy_P(msg,msg_f); rs_puts((UBYTE *)msg);
}
UWORD get_word(UBYTE *ptr)
{
UWORD result ;
UBYTE i ;
/* clear */
result = 0 ;
for ( i = 0 ; i < 4 ; i++ ) {
result *= 10 ;
result += get_hex( *ptr ) ;
ptr++ ;
}
return result ;
}
UBYTE get_status(void)
{
return( PINC & 3 ) ;
}
void put_pulse_count(UWORD xlp,UWORD xrp)
{
UBYTE xdir ;
UBYTE xcnt[3] ;
UBYTE i ;
/* stop motor */
xdir = 0 ;
*(xcnt+0) = xdir ;
*(xcnt+1) = xdir | 0x80 ;
*(xcnt+2) = xdir ;
for ( i = 0 ; i < 3 ; i++ ) { PORTB = *(xcnt+i) ; }
/* left motor pulse count */
*(xcnt+0) = ((xlp >> 8) & MASK0F) | 0x40 ;
*(xcnt+1) = ((xlp >> 4) & MASK0F) | 0x50 ;
*(xcnt+2) = (xlp & MASK0F) | 0x60 ;
for ( i = 0 ; i < 3 ; i++ ) { impress_portb( *(xcnt+i) ) ; }
PORTB = 0 ;
/* right motor pulse count */
*(xcnt+0) = ((xrp >> 8) & MASK0F) ;
*(xcnt+1) = ((xrp >> 4) & MASK0F) | 0x10 ;
*(xcnt+2) = (xrp & MASK0F) | 0x20 ;
for ( i = 0 ; i < 3 ; i++ ) { impress_portb( *(xcnt+i) ) ; }
PORTB = 0 ;
/* send command */
xdir = 4 ;
if ( xlp & MASK8000 ) { xdir |= 1 ; }
if ( xrp & MASK8000 ) { xdir |= 2 ; }
*(xcnt+0) = xdir ;
*(xcnt+1) = xdir | 0x80 ;
*(xcnt+2) = xdir ;
for ( i = 0 ; i < 3 ; i++ ) { PORTB = *(xcnt+i) ; }
}
void impress_portb(UBYTE x)
{
PORTB = x ;
PORTB = x | MASK80 ;
}
動作シーケンス
マシンを移動させるには、次のシーケンスを利用します。
- 左右の移動距離を計算
- 距離をパルス数に換算
- 左右のモータが停止していることを確認
- 左右のパルス数を転送
- 左右のモータの回転を確認
シーケンスから、移動距離の計算とパルス数に
変換する関数を定義しなければならないと理解
できます。
もう少し上位の動作シーケンスを考えた方が
わかりやすいと思い、次の3動作を簡単定義
できるアプリケーションを記述します。
Tcl/Tkを利用し、C言語のステートメントを
生成するスクリプトを記述しました。
スクリプトは、200行程度です。
#!/usr/local/bin/wish
wm title . "Make move script"
set theFileName ""
set fd_in ""
set fd_out ""
set xcondition ""
set xdistance ""
set xdegree "0"
set xfname ""
set xsensor ""
#######################################
# define objects
#######################################
#----- label -----
label .lblFileNameLabel -text "FileName"
label .lblFileName -textvariable theFileName
label .lblDummy -text "control"
label .lblCondition -text "condition"
label .lblDistance -text "distance"
label .lblDegree -text "degree"
label .lblSensor -text "sensor"
#----- entry -----
entry .txtCond -textvariable xcondition
entry .txtDistance -textvariable xdistance
entry .txtDegree -textvariable xdegree
entry .txtSensor -textvariable xsensor
entry .txtFName -textvariable xfname
#----- button (script) -----
button .btnWhere -text "where" -width 8 -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr "if ( cond == [.txtCond get]) {"
.lstScript insert [expr $ptr+1] " "
.lstScript insert [expr $ptr+2] "}"
}
button .btnForward -text "forward" -width 8 -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr " forward( [.txtDistance get] );"
}
button .btnReverse -text "reverse" -width 8 -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr " reverse( [.txtDistance get] );"
}
button .btnRotate -text "rotate" -width 8 -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr " rotate( [.txtDegree get] );"
}
button .btnGetSensor -text "sensor" -width 8 -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr "cond = [.txtSensor get] ;"
}
#----- list box -----
listbox .lstScript -font "utf-8" -yscrollcommand ".scbLadderV set" -width 50
#----- scroll bar -----
scrollbar .scbLadderV -orient vertical -command ".lstScript yview"
#----- button (system) -----
button .btnExit -text "Exit" -command "exit"
button .btnLoad -text "Load" -command {
# clear list box
.lstScript delete 0 end
# get file name
set theFileName [tk_getOpenFile -filetypes {{"assembly code file" {.asm}}}]
# set file handle
set fd_in [open $theFileName "r"]
# read context
while { [gets $fd_in sbuf] >= 0 } {
# store lines to listbox
.lstScript insert end $sbuf
}
# close
close $fd_in
}
button .btnSave -text "Save" -command {
# get file name
set theFileName [tk_getSaveFile -filetypes {{"text code file" {.txt}}}]
# add extention
set theFileName "$theFileName.txt"
# set file handle
set fd_out [open $theFileName "w"]
# write context
set last [.lstScript index end]
for {set i 0} {$i < $last} {incr i} {
set tmp [.lstScript get $i]
puts $fd_out $tmp
}
# close
close $fd_out
}
button .btnSpace -text "Space" -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript insert $ptr " "
}
button .btnDelete -text "Delete" -command {
# get active line number
set ptr [.lstScript index active]
# store strings
.lstScript delete $ptr
}
button .btnClear -text "Clear" -command {
.lstScript delete 0 end
}
button .btnClearCondition -text "clear condition" -command {
set xcondition ""
}
button .btnClearDistance -text "clear distance" -command {
set xdistance ""
}
button .btnClearDegree -text "clear degree" -command {
set xdegree "0"
}
button .btnClearSensor -text "clear sensor" -command {
set xsensor ""
}
#######################################
# window area placing
#######################################
grid .lblFileNameLabel -column 0 -row 0
grid .lblFileName -column 0 -row 1
grid .lstScript -column 0 -row 2
grid .lblDummy -column 0 -row 6
grid .btnWhere -column 0 -row 7
grid .btnForward -column 0 -row 8
grid .btnReverse -column 0 -row 9
grid .btnRotate -column 0 -row 10
grid .btnGetSensor -column 0 -row 11
grid .scbLadderV -column 1 -row 2 -sticky ns
grid .btnLoad -column 1 -row 0
grid .btnSave -column 1 -row 1
grid .btnSpace -column 3 -row 0
grid .btnDelete -column 3 -row 1
grid .lblCondition -column 1 -row 7
grid .txtCond -column 2 -row 7
grid .lblDistance -column 1 -row 8
grid .txtDistance -column 2 -row 8
grid .lblDegree -column 1 -row 10
grid .txtDegree -column 2 -row 10
grid .lblSensor -column 1 -row 11
grid .txtSensor -column 2 -row 11
grid .btnClearCondition -column 5 -row 7
grid .btnClearDistance -column 5 -row 8
grid .btnClearDegree -column 5 -row 10
grid .btnClearSensor -column 5 -row 11
grid .btnExit -column 6 -row 14
このアプリケーションで、センサー値に対する
処理を次々に定義し、テキストファイルに出力
します。
移動は、前進後退と左右の向き変更だけに限定
してあります。
コマンドは、次のように定義。
- 前進 forward
- 後退 reverse
- 向き変更 rotate
forward、reverseにはパラメータをひとつ
だけ指定します。パラメータ値は0から4095
の整数。
rotateはパラメータをひとつだけ指定。
パラメータ値は-90から+90の整数。
移動処理を3つのコマンドに集約したなら
センサー値との対応を、コマンドwhereで
指定します。
コマンドwhereは、プログラムコードに変換
するとif文になるようにしてあります。
if文の条件式で使う変数はcmdに。
ボタンwhereで変換すると、以下。
if ( cmd == 0x10 ) {
}
変数cmdの値範囲は、8ビットで表現できる
0あるいは自然数に。
変数値の範囲限定で、MCRで利用している
センサー出力値をそのまま使えます。
センサー値は8ビットとして与えます。
入力センサー値をsensorというボタンで
指定。
センサー値は、変数sensorに入れることに。
センサー値は、センサーにより8ビットの範囲では
ない可能性があるとし、変換後cmdに代入。
cmd = convert(sensor) を実際のプログラムで
入れて対応します。
テキストファイルに出力後、実際の動作コードに
合体し、移動制御プログラムに仕立てます。
目次
前
次