目次 前 次

滑らか走行のために

 
　今回利用のメカは、左右に独立したモータを
　持ちます。



　このタイプのマシンは、直進するときに、２つの
　モータの回転数に差があることから、首を振るよう
　になります。

　マシンが首を振らないように、直進時には
　左右のモータの回転数に差がでないように
　制御します。

　モータの回転数を検出し、PID制御で左右の
　モータの回転数差を小さくします。

　モータの回転数を検出するためには、ロータリー
　エンコーダを使います。エンコーダを、車輪内部
　に収容します。

　エンコーダは、ヨーグルトの容器の底をカット
　して自作します。



　カットするには、次のような治具を作りました。



　板に支柱をたてて、支柱にカッターの刃を固定します。
　ヨーグルトの容器を刃にあて、一回りさせて底だけを
　カットします。

　底を黒く塗装してから、フォトインタラプタでの
　赤外線反射をよくするよう、アルミホイルを両面
　テープで貼り付けます。





PID制御
　ロータリーエンコーダで、左右のモータの回転数が
　わかれば、PIDによる制御を適用します。

　左右のモータ回転数が、DUTY比によりどのくらいに
　なるのかを計測し、制御対象の挙動を把握します。

　DUTY比と回転数の関係を掴むために、次の処理系を
　考えました。



　モータドライバにPWM波形を与え、１秒当たり
　の回転数を測定します。

　必要な機能をリストして、構成を考えます。

　ファームウエアでの実現に必要な内容を検討します。

　PWM波形のDUTY比指定
　　シリアルインタフェースを利用し、端末ソフトから
　　DUTY比を10進数で与えます。
　　コマンドを用意しておきます。
　　１文字コマンド'D'とし、この後にDUTY比を10進数
　　の２桁で入力します。

　PWM波形生成
　　タイマー割込みを利用し、０〜９９までカウントし
　　記憶している値を比較します。記憶値より小さいと
　　１を出力し、記憶値以上だと０を出力します。

　１秒間計測
　　１ｍｓのシステムタイマーを作り、システムタイマー
　　の値を利用して1000msの待ち時間を生成します。

　カウント処理
　　フォトリフレクタからの変化信号を割込みで
　　受取り、内部に用意したカウンタの値を＋１
　　していきます。

　カウント値表示
　　シーケンサを用意して、次の動作を繰り返します。

    

  1. カウンタレジスタクリア

  2. 1000ms待ち

  3. カウンタ値を10進に変換

  4. 端末にカウンタの値を転送

  5. １にもどる


　　1000msの待ちの間に、外部割込みを使い
　　カウンタ値を＋１していきます。

　　表示は、必要なときだけにしたいので、コマンドを
　　用意します。'C'として、この後に１を入れると表示
　　０を入れると表示停止とします。

　今回は、Arudino基板を流用して計測します。



　ファームウエアは、次のように定義しました。

#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>

#define OFF 0
#define ON  OFF+1

#define FOSC   4000000
#define BAUD   9600
#define MYUBRR (FOSC/16/BAUD)-1

typedef unsigned char  UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long  ULONG ;
typedef   signed char  SBYTE ;
typedef   signed short SWORD ;

#define BSIZE    8
#define INTERVAL 499
#define PWM_MAX  100

const prog_char msg_h[] PROGMEM = "? help" ;
const prog_char msg_d[] PROGMEM = "D set duty ratio" ;
const prog_char msg_c[] PROGMEM = "C show counter" ;

/* variables */
volatile ULONG timcnt ; 
volatile UBYTE uflag ;
volatile UBYTE eflag ;
volatile UBYTE etrg  ;
volatile UBYTE cmd ;
volatile UBYTE sbuf[BSIZE] ;
volatile UBYTE sindex ;
volatile UBYTE pcnt   ;
volatile UBYTE fduty  ;
volatile UBYTE fdutyx ;
volatile UWORD mcount ;
volatile UBYTE state ;
volatile ULONG target ;
volatile UWORD xtmp ;

volatile UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'} ;
volatile UBYTE val[6] ;

void  user_initialize(void);
void  uart_putchar(UBYTE x);
void  crlf(void);
void  uart_puts(UBYTE *x);
UBYTE get_hex(UBYTE x) ;
void  show_help(void);
void  measure(void);

/*------*/
/* main */
/*------*/
int main(void)
{
  /* initialize */
  user_initialize();
  /* enable interrupt */
  sei();
  /* loop */
  while ( ON ) {
    /* command interpreter */
    if ( uflag == ON ) {
      /* clear flag */
      uflag = OFF ;
      /* new line */
      crlf();
      /* get command */
      cmd = *(sbuf+0) ;
      /* help */
      if ( cmd == '?' ) { show_help(); }
      /* set duty */
      if ( cmd == 'D' ) {
        fduty = get_hex( *(sbuf+1) ) ;
        fduty *= 10 ;
        fduty += get_hex( *(sbuf+2) ) ;
      }
      /* measure */
      if ( cmd == 'C' ) {
        /* default */
        state = 0 ;
        eflag = OFF ;
        /* judge */
        if ( *(sbuf+1) == '1' ) { eflag = ON ; }
      }
    }
    /* counter increment */
    if ( etrg == ON ) {
      /* clear flag */
      etrg = OFF ;
      /* increment */
      mcount++ ; 
   }
    /* measure and show */
    measure();
  }
  /* dummy */
  return 0 ;
}

/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
  /* PORT B */
  PORTB = 0b00000000 ; /* 00000000 */
  DDRB  = 0b11111111 ; /* oooooooo */
  /* PORT D */
  PORTD = 0b00000001 ; /* 00000101 */
  DDRD  = 0b11111110 ; /* oooooioi */
  /* initialize flags */
  uflag = OFF ;
  eflag = OFF ;
  etrg  = OFF ;
  /* initialize UART */
  {
    /* initialize pointer */
    sindex = 0 ; 
    /* set Baud Rate Registers */
    UBRRL = MYUBRR ;
    /* Enable receive interrupt , receive module and transmit module */
    UCSRB = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE);
  }
  /* initialize external interrupt */
  {
    MCUCR = (1 << ISC01) ;
  }
  /* initialize timer1 */
  {
    /* compare match (1/8) -> 4000kHz / 8 = 500kHz */
    TCCR1B = (1 << WGM12) | (1 << CS10) ;
    TCNT1 = 0 ;
    OCR1A = INTERVAL ;
    OCR1B = INTERVAL * 2 ;
    /* set TIMER1 interrupt */
    TIMSK = (1 << OCIE1A) ;
  }
  /* initialize duty ratio */
  fduty  = 0 ;
  fdutyx = 0 ;
  *(val+5) = 0 ;
  timcnt = 0 ;
}

/* receive interrupt */
ISR(USART_RX_vect)
{
  UBYTE tmp ;
  /* get 1 charactor */
  tmp = UDR ;
  /* store */
  *(sbuf+sindex) = tmp ;
  /* update counter */
  sindex++ ;
  /* judge */
  if ( tmp == '\r' ) {
    sindex = 0 ;  
    uflag  = ON ;
  }
}

/* INT0 interrupt */
ISR(INT0_vect)
{
  etrg = ON ;
}

/* timer1 interrupt */
ISR(TIMER1_COMPA_vect)
{
  volatile UBYTE tmp ;
  /* increment */
  timcnt++ ;
  /* default */
  tmp = 0 ;
  /* judge */
  if ( pcnt < fdutyx ) { tmp |= ON ; }
  /* impress */
  PORTB = tmp ;
  /* increment */
  pcnt++ ;
  /* judge */
  if ( pcnt == PWM_MAX ) {
    pcnt = 0 ;
    fdutyx = fduty ;
  }
}

void  uart_putchar(UBYTE x)
{
  while ( !(UCSRA & (1 << UDRE)) ) ;
  UDR = x ;
}

void  crlf(void)
{
  uart_putchar('\r');
  uart_putchar('\n');
}

void  uart_puts(UBYTE *x)
{
  while ( *x != '\0' ) {
    uart_putchar( *x ) ;
    x++ ;
  }
  crlf();
}

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] ;
  /* set message */
  strcpy_P(msg,msg_h); uart_puts((UBYTE *)msg);
  strcpy_P(msg,msg_d); uart_puts((UBYTE *)msg);
  strcpy_P(msg,msg_c); uart_puts((UBYTE *)msg);
}

void  measure(void)
{
  /* judge */
  if ( eflag == OFF ) return ;
  /* state machine */
  switch ( state ) {
    /* clear internal counter */
    case 0 : mcount = 0 ;
             state = 1 ;
             break ;
    /* enable external interrupt */
    case 1 : GIMSK = (1 << INT0) ;
	         state = 2 ;
             break ;
    /* get now counter */
    case 2 : target = timcnt + 1000 ;
             state = 3 ;
             break ;
    /* ? past 1 second */
    case 3 : state = 3 ;
             if ( timcnt > target ) { state = 4 ; }
             break ;
    /* disable external interrupt */
    case 4 : GIMSK = 0 ;
	     state = 5 ;
             break ;
    /* convertion */
    case 5 : xtmp = mcount ;
             *(val+0) = xtmp / 10000 ; xtmp %= 10000 ;
             *(val+1) = xtmp / 1000  ; xtmp %= 1000  ;
             *(val+2) = xtmp / 100   ; xtmp %= 100   ;
             *(val+3) = xtmp / 10    ;
             *(val+4) = xtmp % 10    ;
             /* value -> ascii */
             *(val+0) = asc_hex[ *(val+0) ] ;
             *(val+1) = asc_hex[ *(val+1) ] ;
             *(val+2) = asc_hex[ *(val+2) ] ;
             *(val+3) = asc_hex[ *(val+3) ] ;
             *(val+4) = asc_hex[ *(val+4) ] ;
             state = 6 ;
             break ;
    /* show */
    case 6 : uart_puts((UBYTE *)val) ;
             state = 7 ;
             break ;
    /* return first state */
    case 7 :
             state = 0 ;
             break ;
  }
}

　仕事の関係で、ロボットトライアスロンの資料を
　読んでみました。

　モータを制御する場合、PID制御ではなくPI制御で
　充分という記述がありました。

　ロボットトライアスロンのマシンでは、センサーと
　して、ロータリーエンコーダと同じように、モータ
　の回転をA相、B相で取出しています。

　A相、B相でモータ回転を取得すると、回転方向と
　回転数を同時にセンシングできます。

　今回はロータリーエンコーダを使わずに、回転数
　だけを取得するためにエンコーダを用意してます。




　これまでのマシンで利用した、次の回路を
　利用し、２個のモータにエンコーダを接続
　します。



　センサーは、秋月電子で売られている赤外線
　ICを使います。大量に貰えたので使います。



　PI制御を入れて、走行させてみましたが
　首を振る動作は解消されませんでした。



３輪から４輪へ

　前方の首を振る動作が解消されないまま、ギアボックス
　トラブルから、３輪から４輪を使うようにシャーシ変更
　になりました。



　４輪に変更してから、試しに走らせてみると
　左右のモータ回転数に、若干の差があるのに
　直線性がよくなりました。

　前輪があるために、左右に首を振る動作をして
　いるのにも関わらず、前輪にある質量を動かす
　ことまではできなくなっています。

　３輪から４輪への変更が、直進における首振り
　動作を、メカで解決する方法になりました。

　前輪が左右に振れないと、ライントレースが
　不可能となるので、サーボモータを取付けて
　おきます。



　前輪が１輪から２輪になることで、首振り運動
　がなくなり、直進性能が安定することを、長く
　MCRに関わって、やっと理解できました。
目次 前 次

inserted by FC2 system