目次 前 次

---

ライントレーサ処理

　手元に台車メカがあるので、ADuC7026を利用して
　ライントレーサを実現します。



　ラインの状態をセンシングするのに、コース面に
　密着するタイプのラインセンサーを使います。



　台車メカとセンサーの動作を調べるための
　プログラムを作成します。

　モータドライバ、センサーとの接続は
　以下とします。

GP2  sensor input
GP11 right motor pulse output
GP12 left  motor pulse output
GP13 arm   motor pulse output
GP47 LED monitor

　端末からのコマンドで動作を確認する
　として、１文字コマンドを決めます。

? ヘルプ
A アーム上下動制御
L 左DUTY比設定（10進数で指定）
R 右DUTY比設定（10進数で指定）
D DUTY比表示
S センサー状態を８ビットで表示

　モータを回転させるには、DUTY比を
　与えてスピードを制御します。
　PWM(Pulse Width Modulation)を使い
　ます。

　PWMには、クロックが必要なので
　タイマー３割込みを使います。

　以上をまとめて、次のソースコード
　となります。

#include <ADuC7026.h>

#define OFF 0
#define ON  OFF+1

/* data definitions */
typedef unsigned char  UBYTE ;
typedef   signed char  SBYTE ;
typedef unsigned short UWORD ;
typedef   signed short SWORD ;
typedef unsigned long  ULONG ;
typedef   signed long  SLONG ;

void  IRQ_Handler(void) __irq;
void  init_usr(void);

#define MASKFF 0xFF
#define MASK0F 0x0F
#define MASK80 0x80
#define MASK40 0x40
#define MASK20 0x20
#define MASK10 0x10
#define MASK08 0x08
#define MASK04 0x04
#define MASK02 0x02
#define MASK01 0x01
#define MASKF0 0xF0

#define PCNT_MAX 100
#define SFT_BIT  16

ULONG timcnt ;

void  rs_putchar(UBYTE x);
void  crlf(void);
void  rs_puts(UBYTE *x);
UBYTE get_hex(UBYTE x);
void  show_help(void);

void  delay_ms(UWORD x);
void  delay_100us(UWORD x);

UBYTE get_value(UBYTE *x);
void  show_value(UBYTE x);

/* global variables */
volatile UBYTE uflag ;
volatile UBYTE sbuf[8] ;
volatile UBYTE sindex ;
volatile UBYTE cmd ;

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

volatile UBYTE aflag ;

volatile UBYTE sensor ;
volatile UBYTE pcnt ;
volatile UBYTE lduty ;
volatile UBYTE rduty ;
volatile UBYTE aduty ;
volatile UBYTE ldutyx ;
volatile UBYTE rdutyx ;
volatile UBYTE adutyx ;

int main(void)
{
  int loop ;
  UBYTE tmp ;
  /* initialize user */
  init_usr();	
  /* show message */
  rs_puts("Hello"); crlf();
  /* endless loop */
  while(ON)
  {
    /* command interrpreter */
    if ( uflag == ON ) {
      /* clear flag */
      uflag = OFF ;
      /* new line */
      crlf();
      /* judge */
      cmd = *(sbuf+0) ;
      if ( cmd == '?' ) { show_help(); }
      /* ARM handling */
      if ( cmd == 'A' ) {
        /* get parameter */
        tmp = get_hex( *(sbuf+1) );
        /* set flag */
        aflag = OFF ;
        if ( tmp ) { aflag = ON ; }
      }
      /* set left duty */
      if ( cmd == 'L' ) { lduty = get_value( sbuf+1 ) ; }
      /* set right duty */
      if ( cmd == 'R' ) { rduty = get_value( sbuf+1 ) ; }
      /* get data */
      if ( cmd == 'D' ) {
        rs_puts("Left  duty = ") ; show_value( lduty ); crlf();
        rs_puts("Right duty = ") ; show_value( rduty ); crlf();
        rs_puts("ARM ") ;
        if ( aflag == ON ) {
          rs_puts("enable") ;
        } else {
          rs_puts("disable") ;
        }
        crlf();
      }
      /* show sensor data */
      if ( cmd == 'S' ) {
        /* get data */
        sensor = GP2DAT & MASKFF ;
        /* separate */
        for ( loop = 7 ; loop > -1 ; loop-- ) {
          tmp = ((sensor >> loop) & 1) + '0' ;
          rs_putchar( tmp ) ;
        }
        /* new line */
        crlf();
      }
    }
  }
  /* dummy return */
  return (0);
}

void IRQ_Handler(void) __irq
{
  volatile UBYTE ch ;
  volatile UBYTE portx ;
  /* judge UART receive interruption */
  if ( (IRQSTA & UART_BIT) == UART_BIT ) {
    /* judge */
    if ( COMSTA0 & 1 ) {
      /* clear flag */
      ch = COMRX ;
      *(sbuf+sindex) = ch ;
      sindex++ ;      
      if ( ch == '\r' ) {
        sindex = 0 ;
        uflag  = ON ;
      }
    }
  }
  /* judge timer3 interruption (1ms) */
  if ( (IRQSTA & WATCHDOG_TIMER_BIT) == WATCHDOG_TIMER_BIT ) {
    /* clear timer3 interrupt flag */
    T3CLRI = 0xff ;
    /* increment */
    timcnt++ ;
    /* impress */
    portx = 0 ;
    if ( pcnt < ldutyx ) { portx |= 2 ; }
    if ( pcnt < rdutyx ) { portx |= 4 ; }
    if ( aflag == ON ) {
      if ( pcnt < adutyx ) { portx |= 8 ; }
    }
    GP0DAT &= 0xff200000 ;
    GP0DAT |= (portx << SFT_BIT);
    /* PWM counter increment */
    pcnt++ ;
    /* update duty */
    if ( pcnt == PCNT_MAX ) {
      pcnt = 0 ;
      ldutyx = lduty ;
      rdutyx = rduty ;
      adutyx = aduty ;
    }	    
    /* blink */
    if ( (timcnt & 0x3ff) == 1000 ) {
      GP4DAT ^= (1 << 23);
    }
  }
}

void init_usr(void)
{
  /* select clock 10.44MHz 
       initialized in start up routine
  */
  PLLKEY1 = 0xaa ;
  PLLCON  = 0x01 ;
  PLLKEY2 = 0x55 ;
  /* power control
       initialized in start up routine 
  */
  /* clear flag */
  uflag = OFF ;
  aflag = OFF ;
  /* clear counter */
  timcnt = 0 ;
  /* initialize UART */
  {
    /* set baud rate 19200 bps CD = 2 */
    COMCON0 = 0x80 ; /* select COMDIV1 and COMDIV0 */
    COMDIV0 = 0x11 ;
    COMDIV1 = 0x00 ;
    /* set conditions */
    COMCON0 = 0x03 ; /* select COMRX and COMTX , 8bit data , 1 stop bit , no parity */
    /* enable interrupt */
    COMIEN0 = 0x01 ; /* ERBFI */
  }
  /* P0 */
  {
    /* */
    GP0DAT = 0x0F000000 ;
  }
  /* P1 */
  {
    /* use UART */
    GP1CON = 0x00000011 ; 
    /* */
    GP1DAT = 0xfef00000 ;
  }
  /* P2 */
  {
    /* all bits inputs */
    GP2DAT = 0x00000000 ;
  }
  /* P3 */
  {
    /* all bits outputs */
    GP3DAT = 0xff000000 ;
  }
  /* P4 */
  {
    GP4DAT = 0xff000000 ;
  }
  /* initialize timer 3 (1ms) */
  {
    T3LD  = 33   ; /* (32.768kHz / 33) = about 1kHz */
    T3CON = 0xc0 ; /* enable , cyclic , 1/1 */ 
  }
  /* clear */
  pcnt = 0 ;
  lduty = 0 ;
  rduty = 0 ;
  aduty = 50 ;
  ldutyx = 0 ;
  rdutyx = 0 ;
  adutyx = 50 ;
  /* enable timer 3 interrupt and UART interrupt */
  IRQEN = WATCHDOG_TIMER_BIT | UART_BIT ;
}

/* UART putchar */
void  rs_putchar(UBYTE x)
{
  /* ? transmmit buffer empty */
  while( (COMSTA0 & 0x40) == 0 ) ;
  /* set value */
  COMTX = x ;
}

/* carriage return and line feed */
void  crlf(void)
{
  rs_putchar('\r');
  rs_putchar('\n');
}

/* UART puts */
void  rs_puts(UBYTE *x)
{
  while ( *x != '\0' ) {
    rs_putchar( *x ) ;
    x++ ;
  }
}

/* convert ASCII to number */
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 ;
}

/* show help */
void  show_help(void)
{
  rs_puts("? help")                 ; crlf();
  rs_puts("A enable or disable arm"); crlf();
  rs_puts("L left duty")            ; crlf();
  rs_puts("R right duty")           ; crlf();
  rs_puts("D show duty")            ; crlf();
  rs_puts("S show sensor value")    ; crlf();
}

void  delay_100us(UWORD x)
{
  ULONG last ;
  /* calculate */
  last = timcnt + x ;
  /* wait */
  while ( timcnt < last ) ;
}

void  delay_ms(UWORD x)
{
  ULONG last ;
  /* calculate */
  last = timcnt + 10 * x ;
  /* wait */
  while ( timcnt < last ) ;
}

UBYTE get_value(UBYTE *x)
{
  volatile UBYTE result ;
  /* default */
  result = 0 ;
  /* x10 */
  result = get_hex( *x ) ;
  result *= 10 ;
  /* x1 */
  result += get_hex( *(x+1) );

  return result ;
}

void  show_value(UBYTE x)
{
  UBYTE msg[3] ;
  /* separate */
  x %= 100 ;
  *(msg+0) = x / 10 ;
  *(msg+1) = x % 10 ;
  *(msg+2) = '\0' ;
  /* conversion */
  *(msg+0) = asc_hex[*(msg+0)] ;
  *(msg+1) = asc_hex[*(msg+1)] ;
  /* show */
  rs_puts( msg ) ; 
}

　ポートは、次のように使います。



　メカには、10ピンコネクタがあり、次のように
　電源と信号線が割当てられています。



　ADuC7026の電源電圧は3.3Vですが、基板上に
　３端子レギュレータがあるので、信号線とは
　独立したケーブルで電源だけを接続します。

　左右のDCモータには、２ピンケーブルで
　パルスを供給します。



　ARMから与えるパルスでは、DCモータをドライブできない
　ので、パワートランジスタ、パワーMOSFETを使って駆動
　します。



　DCモータをドライブするパワートランジスタ、パワーMOSFET
　は、３個にします。ライントレーサは、前に進む、右に進む
　左に進むだけでゴールまで到着すればよいので、DCモータの
　駆動にパワーデバイスは２個で充分。
　アームがあるので、合計３つのパワーデバイスを使います。

　３つのパワーデイバスを使うので、ARMに接続するドライブ
　回路は、次のようになります。



　利用している素子は、手持ちを利用したので、最適な部品
　の選択になっていません。パワートランジスタは、１Ａを
　流せるなら、何でも構わないでしょう。

　DCモータと並列に入れてあるキャパシタは、モータが発生する
　ノイズを低減します。ダイオードは、モータで発生する逆電圧
　を還流させて、トランジスタを保護させる役目を持ちます。

　トランジスタのベースエミッタ間の抵抗は、ARMからパルスを
　出していないときに、確実にトランジスタをオフ状態にして
　モータが回らないようにする役目を持っています。

　この回路を実装した基板は、以下です。



　上の基板では、フォトカプラを利用して、ARMとモータ制御
　基板を電気的に絶縁しています。回路は、簡単で次のように
　なっています。



　フォトカプラで絶縁すると、モータの電源電圧を気に
　せずに、ARMからパルスを出力できるようになります。

　フォトカプラで扱える信号周波数は10kHz程度なので
　パルスの最大周波数は、10kHzを上限にしておきます。

　ファームウエアで、モータとセンサーの動作を確認できた
　ので、ライントレーサのファームウエアを考えます。

　ライントレーサで有名な、MCRのコースを走行する
　ことを想定します。




　ライントレーサのファームウエアは、３ブロックに
　分けて考えると、簡単に構成できます。
　　センサー入力
　　モータ制御
　　移動方式決定

　３ブロックは、次のように命名しています。

センサー入力  sensor_handling
モータ制御    control_motor
移動方式決定  generate_run

　３ブロックに分割し、１ブロックを複数の関数で
　記述します。
　こうすると、実走行の前に、PersonalComputer内
　でシミュレーションできるからです。



　各ブロックを、どういう関数で構成するかを
　考えていきます。

　sensor_handling
　　８ビットのセンサーでコース上のライン状態を
　　取得しなければならないので、関数get_sensor
　　を用意します。
      typedef unsigned char UBYTE ;

      UBYTE get_sensor(void);

　　今回は、GP2からデータを入力するので、次のように
　　関数を定義します。
      #define MASKFF 0xff 

      UBYTE get_sensor(void)
      {
        UBYTE result ;

        result = GP2DAT & MASKFF ;

        return result ;
      }

　　また、取得したセンサーデータを加工しやすい
　　フォーマットにするための変換をconversionで
　　担当します。
      UBYTE conversion(UBYTE x);

　　関数get_sensorで取得した８ビットデータは
　　白で０、黒で１となっています。
　　白が１、黒が０の方が考えやすかったので
　　論理反転します。
      UBYTE conversion(UBYTE x)
      {
        return( x ^ MASKFF);
      }

　control_motor
　　モータへPWM波形を出力する処理は、タイマー割込みを
　　利用しています。動作を確認するファームウエアでは
　　コマンドに続けてDUTY比を与えています。

　　モータを回転させるには、回転方向とDUTY比を指定
　　して、タイマー割込みハンドラが自動処理するよう
　　関数を２つ用意します。

　　ライントレーサの移動は、前、右、左が大半なので
　　モータの回転方向を指定する関数を定義せずに、
　　DUTY比だけを設定する関数を用意します。
      void put_motor(UBYTE which,UBYTE dx);

　　今回のライントレーサは、左右にモータをもっているので
　　左右のどちらかとDUTY比をパラメータとして与えます。
      #define LEFT_MOTOR  2
      #define RIGHT_MOTOR 1

      UBYTE lduty ;
      UBYTE rduty ;

      void put_motor(UBYTE which,UBYTE dx)
      {
        if ( which == LEFT_MOTOR  ) { lduty = dx ; }
        if ( which == RIGHT_MOTOR ) { rduty = dx ; }
      }

　　関数put_motorは、グローバル変数に値を格納するだけとし
　　タイマー割込みで、自動で値を更新します。

　generate_run
　　マシンをどう動かすのかを、このブロックで確定します。

　　センサーデータは８ビットなので、ビットパターンは
　　256通りになります。この256パターンから実際に遭遇
　　するセンサーデータを抽出して、動かします。

　　コースから逸脱しないで走行するには、センターラインを
　　トレースするのが最も簡単です。

　　入力されるセンサーデータは、２進で表現すると
　　次の１０種類程度になります。

0001 1000
0001 1100
0011 1000
0011 0000
0110 0000
1100 0000
1111 0000
0000 1100
0000 0110
0000 0011
0000 1111
0000 0000
1111 1111

　　Ｃを利用するので、２進から16進に変換します。

0001 1000 => 0x18 (CENTER0)
0001 1100 => 0x1c (CENTER1)
0011 1000 => 0x38 (CENTER2)
0011 0000 => 0x30 (BIT_LEFT)
0110 0000 => 0x60 (LEFT)
1100 0000 => 0xc0 (EXTRA_LEFT)
1111 0000 => 0xf0 (LEFT_WHITE)
0000 1100 => 0x0c (BIT_RIGHT)
0000 0110 => 0x06 (RIGHT)
0000 0011 => 0x03 (EXTRA_RIGHT)
0000 1111 => 0x0f (RIGHT_WHITE)
0000 0000 => 0x00 (ALL_BLACK)
1111 1111 => 0xff (ALL_WHITE)

　これらの値を利用し、判定部分を作成します。

　各条件から、次に実現する内容をリスト。

sensor = CENTER0     => keep
sensor = CENTER1     => keep
sensor = CENTER2     => keep
sensor = BIT_LEFT    => right DUTY bit increment
sensor = LEFT        => right DUTY increment
sensor = EXTRA_LEFT  => right DUTY extra increment
sensor = BIT_RIGHT   => left  DUTY bit increment
sensor = RIGHT       => left  DUTY increment
sensor = EXTRA_RIGHT => left  DUTY extra increment
sensor = LEFT_WHITE  => mode change
sensor = RIGHT_WHITE => mode change
sensor = ALL_WHITE   => mode change
sensor = ALL_BLACK   => mode change

　現状維持、左右のモータのDUTY比変更、モード変更に
　限定しています。左右のモータのDUTY比を変更したなら
　少し時間を置いて、同じ回転数になるようにDUTY比を
　変更しないと、コースアウトします。

　低速走行時には、500ms後に左右のモータのDUTY比を
　同じにし、高速走行時では100ms後に左右のモータを
　同じDUTY比に戻す制御をかけます。

　モータのDUTY比変更関数を定義しているので、この
　後に、低速、高速走行を判定し、持続時間経過後に
　左右のタイヤの回転数が同じになるようDUTY比を
　再設定します。

  /* left */
  put_motor_duy( left_motor , left_motor_duty ) ;
  /* right */
  put_motor_duy( rigth_motor , right_motor_duty ) ;
  /* delay */
  delay_ms(100) ;
  if ( left_motor_duty < 30 || left_right_duty < 30) {
    delay_ms(400) ; 
  }
  /* return same duty ratio */
  put_motor_duty( left_motor  , 50 );
  put_motor_duty( rigth_motor , 50 );

　上のように定義して、ラッパー関数を利用し
　左右のモータに設定するDUTY比を指定します。

void set_motor_duty(UBYTE dl,UBYTE dr)
{
  /* left */
  put_motor_duy(LEFT_MOTOR,dl) ;
  /* right */
  put_motor_duy(RIGHT_MOTOR,dr) ;
  /* delay */
  delay_ms(100) ;
  if ( dl < 30 || dr < 30) { delay_ms(400) ; }
  /* return same duty ratio */
  put_motor_duty( left_motor  , 50 );
  put_motor_duty( rigth_motor , 50 );
}

　走行モードは、次の４つにしたので
　どう制御するのかを考えます。

normal_run
crank_run
lane_change
blind_run

　blind_runは直進しながら、徐々にスピードを
　あげるだけなので、DUTY比を増やしていきます。

void blind_run(void)
{
  set_motor_duty(10,10); delay_ms(500);
  set_motor_duty(25,25); delay_ms(500);
  set_motor_duty(35,35); delay_ms(500);
  set_motor_duty(45,45); delay_ms(500);
  set_motor_duty(50,50); delay_ms(500);
}

　normal_runは、センサー情報を使い左右の
　モータのDUTY比を設定します。

void normal_run(UBYTE x)
{
  if ( x == ALL_WHITE ) {
    /* slow */
    set_motor_duty(30,30);
    /* change mode */
    mode = CRANK ;
    /* set state */
    state = 0 ;
  }
  if ( x == LEFT_WHITE  ) { 
    /* slow */
    set_motor_duty(30,30);
    /* change mode */
    mode = LANE ;
    /* set state */
    state = 0 ;
    /* store direction */
    xdir = DIR_LEFT ;
  }
  if ( x == RIGHT_WHITE ) {
    /* slow */
    set_motor_duty(30,30);
    /* change mode */
    mode = LANE ;
    /* set state */
    state = 0 ;
    /* store direction */
    xdir = DIR_LEFT ;
  }
  if ( x == CENTER0     ) { set_motor_duty(50,50); }
  if ( x == CENTER1     ) { set_motor_duty(50,50); }
  if ( x == CENTER2     ) { set_motor_duty(50,50); }
  if ( x == EXTRA_RIGHT ) { set_motor_duty(25,50); }
  if ( x == RIGHT       ) { set_motor_duty(35,50); }
  if ( x == BIT_RIGHT   ) { set_motor_duty(45,50); }
  if ( x == BIT_LEFT    ) { set_motor_duty(50,45); }
  if ( x == LEFT        ) { set_motor_duty(50,35); }
  if ( x == EXTRA_LEFT  ) { set_motor_duty(50,25); }
  /* delay */
  delay_ms( 100 ) ;
}

　normal_runからは、crank_runかlane_changeに走行
　モードを変更しなければならないので、状態変更と
　方向設定を入れます。

　crank_runは、コースの状態を見てシーケンサを動かします。



　上の画像から次のように、シーケンス処理を定義しました。

void crank_run(void)
{
  switch ( state ) {
    /* move to ALL_BLACK area */
    case  0 : lcmove() ;
              sensor = get_sensor();
              if ( sensor == ALL_BLACK ) { state = 1 ; }
              break ;
    /* turn */
    case  1 : crank_turn();
              sensor = get_sensor();
              if ( is_close_center(sensor) == ON ) { state = 2 ; }
              break ;
    /* return NORMAL */
    case  2 : mode = NORMAL ;
              state = 0 ;
              break ;
    /* others */
    default : 
              break ;
  }
}

　ALL_BLACKを検出するまで、直進していき旋回します。
　直進は、normal_runとほぼ同じ処理になりますが
　DUTY比が異なるので、独立した関数で対応します。

void lcmove()
{
  /* get sensor data */
  sensor = get_sensor() ;
  if ( sensor == LEFT_WHITE  ) { xdir = DIR_LEFT ; }
  if ( sensor == RIGHT_WHITE ) { xdir = DIR_RIGHT; }
  if ( sensor == CENTER0     ) { set_motor_duty(30,30); }
  if ( sensor == CENTER1     ) { set_motor_duty(30,30); }
  if ( sensor == CENTER2     ) { set_motor_duty(30,30); }
  if ( sensor == EXTRA_RIGHT ) { set_motor_duty(10,30); }
  if ( sensor == RIGHT       ) { set_motor_duty(15,30); }
  if ( sensor == BIT_RIGHT   ) { set_motor_duty(20,30); }
  if ( sensor == BIT_LEFT    ) { set_motor_duty(30,20); }
  if ( sensor == LEFT        ) { set_motor_duty(30,15); }
  if ( sensor == EXTRA_LEFT  ) { set_motor_duty(30,10); }
  /* delay */
  delay_ms(100) ;
}

　旋回は、関数で対応します。

void crank_turn()
{
  /* turn */
  if ( xdir == DIR_RIGHT ) { set_motor_duty(30,15) ; }
  if ( xdir == DIR_LEFT )  { set_motor_duty(15,30) ; }
  /* delay */
  delay(100);
}

　lane_changeは、コースの状態を見てシーケンサを動かします。



　上の画像から次のように、シーケンス処理を定義しました。

void lane_change(void)
{
  /* sequencer */
  switch ( state ) {
    /* move to ALL_BLACK area */
    case  0 : lcmove() ;
              sensor = get_sensor();
              if ( sensor == ALL_BLACK ) { state = 1 ; }
              break ;
    /* turn */
    case  1 : lane_turn();
              state = 2 ;
              break ;
    /* move on ALL_BLACK area */
    case  2 : lane_move();
              sensor = get_sensor();
              if ( is_close_center(sensor) == ON ) { state = 3 ; }
              break ;
    /* turn */
    case  3 : lane_turn_second();
              sensor = get_sensor();
              if ( is_close_center(sensor) == ON ) { state = 4 ; }
              break ;
    /* return NORMAL */
    case  4 : mode = NORMAL ;
              state = 0 ;
              break ;
    /* others */
    default : 
              break ;
  }
}

　車線変更にともなう旋回は２回あるので
　別個に定義します。

void lane_turn()
{
  /* turn */
  if ( xdir == DIR_RIGHT ) { set_motor_duty(30,20) ; }
  if ( xdir == DIR_LEFT )  { set_motor_duty(20,30) ; }
  /* delay */
  delay_ms(100);
  /* return straight */
  set_duty(30,30);
}

void lane_turn_second()
{
  /* opposite turn */
  if ( xdir == DIR_RIGHT ) { set_motor_duty(20,30) ; }
  if ( xdir == DIR_LEFT )  { set_motor_duty(30,20) ; }
  /* delay */
  delay_ms(100);
}

　ALL_BLACKのコース面を走行するのは
　細かく時間を区切って移動させます。

void lane_move()
{
  /* straight */
  set_duty(30,30) ;
  /* delay */
  delay_ms(100);
}

(under construction)

目次 前 次