目次

移動制御プログラム

 移動するには、パルスモータにパルス数を与えます。
 マイコンのファームウエアで必要になる関数を考えます。

 マイコンとのインタフェースは、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コマンドのフォーマットを更に細かく決めます。

 左パルス数、右パルス数を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 ;
}


動作シーケンス  マシンを移動させるには、次のシーケンスを利用します。
  1. 左右の移動距離を計算
  2. 距離をパルス数に換算
  3. 左右のモータが停止していることを確認
  4. 左右のパルス数を転送
  5. 左右のモータの回転を確認
 シーケンスから、移動距離の計算とパルス数に  変換する関数を定義しなければならないと理解  できます。  もう少し上位の動作シーケンスを考えた方が  わかりやすいと思い、次の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にはパラメータをひとつ  だけ指定します。パラメータ値は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) を実際のプログラムで  入れて対応します。  テキストファイルに出力後、実際の動作コードに  合体し、移動制御プログラムに仕立てます。
目次

inserted by FC2 system