目次 前 次

---

動作テスト(Arduino)

　Arduinoを使い、動作試験をしました。

　使ったArduinoは、以下。



　制御信号を与えるために、オシロジイラズを利用。




　オシロジイラズは、デジタルの４ビット入力と
　４ビット出力を持つので、PWM波形出力の観測と
　モード指定スイッチの状態指定ができます。

　出力はトリガーを与える場合に使えます。

　Arduinoのピンアサインは、以下と指定。

　ポートＢ

　　下位３ビットをハーフブリッジICであるSN754410NEの
　　制御に利用。また、モードとスタートトリガを入力
　　できるようにします。

    PB5 (output)LED
    PB4 (input) MODE
    PB3 (input) TRG
    PB2 (output)left_motor
    PB1 (output)right_motor
    PB0 (output)XEN

　　LEDは、Arduino基板上にあるので、オシロジイラズとの
　　接続はなし。
　　MODE、TRGにオシロジイラズの出力を接続。
　　PB2からPB0は、オシロジイラズの入力に接続。

　　PB2、PB1は、関数analogWriteでPWM波形を
　　生成できるため、楽できるようにします。

　ポートＣ

　　Arduinoは、１バイトを１ポートで扱えないため
　　４ビットのマルチプレクサ74HC157を利用して
　　上位、下位のニブルで各々４ビットずつ入力後
　　合成。

　　ピン割り当ては、以下としました。

    PC5 (output)
    PC4 (output)select
    PC3 (input) sensor_3(sensor_7)
    PC2 (input) sensor_2(sensor_6)
    PC1 (input) sensor_1(sensor_5)
    PC0 (input) sensor_0(sensor_4)

　　PC4で、74HC157にどちらの値を入力するか指示
　　しておきます。

　ここまでで、ハードウエア仕様を決定したので
　ファームウエアを考えていきます。

　PWM波形生成

　　Arduinoの持つ関数analogWriteを使い、左右の
　　モータ回転を、わかりやすい関数名で管理する
　　ことにしました。

byte calc_duty(byte x)
{
  word tmp ;
  byte result ;
  /* */
  tmp = (255 * x) / 100 ;
  /* tmp = 255 * (100 - x) / 100 ; */
  /* calculate */
  result = (byte)tmp ;

  return result ;
}

void send_left(byte x)
{
  analogWrite(LEFT,x);
}

void send_right(byte x)
{
  analogWrite(RIGHT,x);
}

　　send_left、send_rightは、analogWriteのラッパー
　　関数としておきます。

　　関数analogWriteでは、DUTY比を％では扱えないので
　　％で扱えるよう、変換関数を定義しておきます。

byte calc_duty(byte x)
{
  word tmp ;
  byte result ;
  /* */
  tmp = (255 * x) / 100 ;
  /* tmp = 255 * (100 - x) / 100 ; */
  /* calculate */
  result = (byte)tmp ;

  return result ;
}

　　利用するときは、DUTY比をcalc_dutyに与えて
　　返値を、send_left、send_rightに渡します。

    send_right(calc_duty(75));
    send_left(calc_duty(75)) ;


　センサーデータ取得

　　センサーは、８ビットで路面情報を出力してきます。




　　利用しているセンサーは、４ビット目をスタートゲート
　　の開閉を論理値で出力してくるので、左、中央、右での
　　判定処理では、情報量が大きくなるため、３ビットだけ
　　を使うことに。

　　6、3、1のビット位置から、論理値を取得して合体します。

　　仕様を確定したなら、関数を定義。

byte get_sensor()
{
  byte result ;
  byte tmp ;
  byte dh ;
  byte dl ;
  /* upper */
  PORTC |= (1 << SEL_BIT);
  dh = PINC & MASK0F ;
  /* lower */
  PORTC &= ~(1 << SEL_BIT);
  dl = PINC & MASK0F ;
  /* concatenate */
  tmp = dh | dl ;
  /* inverse */
  tmp ^= 0xff ;
  /* 6,3,1 bit */
  if ( tmp & 0x40 ) { result |= 4 ; }
  if ( tmp & 0x08 ) { result |= 2 ; }
  if ( tmp & 0x02 ) { result |= 1 ; }

  return result ;
}

　動作テストと移動の２モードある方がデバッグには
　便利なので電源投入後、モード指定スイッチの状態
　を読み込み、内部で保持します。

  mflag = OFF ;
  if ( PINB & 0x10 ) { mflag = ON ; }

　モードの論理値で、処理を分けて考えます。

  /* test mode */
  if ( mflag == ON ) {
      /* command interpreter */
      if ( uflag == ON ) {
        /* clear flag */
        uflag = OFF ;
        /* new line */
        crlf() ;
        /* get command */
        cmd = *(sbuf+0) ;
        /* help */
        if ( cmd == '?' ) { show_help(); }
        if ( cmd == 'E' ) { rflag = ON ; }
        if ( cmd == 'D' ) { rflag = OFF; }
        if ( cmd == 'R' ) { send_right(calc_duty(75)); }
        if ( cmd == 'r' ) { send_right(  0); }
        if ( cmd == 'L' ) { send_left(calc_duty(75)) ; }
        if ( cmd == 'l' ) { send_left(  0) ; }
        if ( cmd == 'S' ) { show_sensor(); }
        if ( cmd == 'M' ) { show_mode(); }
      }
  } else {
    /* shift register handling */
    if ( sflag == ON ) {
      /* clear flag */
      sflag = OFF ;
      /* shift */
      swsft <<= 1 ;
      /* mask */
      swsft &= 3 ;
      /* get switch state */
      if ( (PINB & 8) == OFF ) { swsft |= ON ; }
      /* judge */
      if ( swsft == 1 ) {
        mcnt = mcnt + 1  ;
        mcnt = mcnt & ON ;
        rflag = mcnt & ON ;
      }
    }
    /* move handling */
    if ( tflag == ON ) {
      /* clear flag */
      tflag = OFF ;
      /* get sensor */
      sensor = get_sensor();
      /* default */
      rrx = 50 ;
      llx = 50 ;
      /* judge */
      if ( sensor == 1 ) {
        rrx = 60 ;
        llx = 40 ; 
      }
      if ( sensor == 4 ) {
        rrx = 40 ;
        llx = 60 ; 
      }
      /* impress */
      send_left( calc_duty(llx) );
      send_right( calc_duty(rrx) );
    }
  }

　テストモードのときは、コマンドインタプリタを
　用意し、指定コマンドを１文字で受け取ります。

　コマンドとそれに対応した処理は、以下。

? ヘルプ（コマンドリストの表示）
E SN754410NEのE端子に'1'を出力
D SN754410NEのE端子に'0'を出力
R 右モータのDUTY比を７５％に設定
r 右モータのDUTY比を０％に設定
L 左モータのDUTY比を７５％に設定
l 左モータのDUTY比を０％に設定
S 現在のセンサー値を表示
M 現在のモードを表示

　スタートトリガーの取得と移動シーケンスの
　トリガーは、タイマー割込みを利用。

　タイマー割込みは、１００ｍｓごとに発生するとし
　次の関数でフラグを設定。

void update_trigger()
{
  tflag = ON ;
  /* increment */
  xcnt++ ;
  /* judge */
  if ( xcnt == 10 ) {
    xcnt = 0 ;
    ycnt++ ;
    sflag = ON ;
  }
  /* impress */
  PORTB &= ~(1 << LED_BIT) ;
  if ( ycnt & ON ) { PORTB |= (1 << LED_BIT) ; }
}

　Arduinoが動作していることを示すために
　基板上のLEDを点滅させます。

　まとめると、以下。

/*
  mcrx.ino

  Pin assignment

  PORTB
    PB5 (output)LED
    PB4 (input) MODE
    PB3 (input) TRG
    PB2 (output)left_motor
    PB1 (output)right_motor
    PB0 (output)XEN

  PORTC
    PC5 (output)
    PC4 (output)select
    PC3 (input) sensor_3(sensor_7)
    PC2 (input) sensor_2(sensor_6)
    PC1 (input) sensor_1(sensor_5)
    PC0 (input) sensor_0(sensor_4)

  PORTD
    PD7 (output)
    PD6 (output)
    PD5 (output)
    PD4 (output)
    PD3 (output)
    PD2 (output)
    PD1 (output)TxD
    PD0 (input) RxD

*/
#include <MsTimer2.h>

#define OFF 0
#define ON  OFF+1

#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASKFF 0xff
#define MASK7F 0x7f

#define SEL_BIT 4
#define LED_BIT 5
#define MRD_BIT 4
#define TRG_BIT 3

#define LEFT  10
#define RIGHT 9

/* variables */
boolean tflag ;
boolean sflag ;
boolean mflag ;
boolean rflag ;
boolean uflag ;
boolean oflag ;

byte xcnt ;
byte ycnt ;
byte swsft ;
byte sensor ;
byte lcnt ;
byte mcnt ;
byte pcnt ;
byte scnt ;
byte rrx ;
byte llx ;
byte cmd ;
byte ch ;
byte sbuf[4] ;
byte sindex ;

/* function prototype */
void update_trigger();
void show_help();
void show_sensor(void);
void show_mode(void);
byte get_hex(byte x);
char get_asc(byte x);
void rs_putchar(char x);
void rs_puts(char *ptr);
void crlf();
byte get_sensor();
byte calc_duty(byte x);
void send_left(byte x);
void send_right(byte x);

void setup()
{
  /* initialize serial */
  Serial.begin(9600);
  sindex = 0 ;
  /* clear flags */
  tflag = OFF ;
  sflag = OFF ;
  mflag = OFF ;
  rflag = OFF ;
  uflag = OFF ;
  oflag = ON ;
  rflag = OFF ;
  /* initialize port values */
  PORTB = 0x18 ;
  PORTC = 0x0f ; /* pull up */
  PORTD = 0x00 ;
  /* initialize port direction */
  DDRB = 0xe7 ;
  DDRC = 0xf0 ; /* lower nibble inputs */
  DDRD = 0xfe ;
  /* variables */
  xcnt = 0 ;
  ycnt = 0 ;
  mcnt = 0 ;
  /* judge */
  if ( PINB & (1 << MRD_BIT) ) { mflag = ON ; }
  /* 100ms period */
  MsTimer2::set(100,update_trigger);
  /* enable */ 
  MsTimer2::start();
}

void loop()
{
  /* opening message */
  if ( oflag == ON ) {
    /* new line */
    crlf();
    /* clear flag */
    oflag = OFF ;
    /* message */
    rs_puts("Hello , Darling !");
    /* new line */
    crlf();
    /* TEST MODE */
    if ( mflag == ON ) {
      rs_puts("***** Test Mode *****");
      crlf();
      show_help();
    }
  }
  /* SN854410NE enable control */
  if ( rflag == ON ) { PORTB |= 0x01 ; }
  else               { PORTB &= 0xfe ; }
  /* test mode */
  if ( mflag == ON ) {
      /* command interpreter */
      if ( uflag == ON ) {
        /* clear flag */
        uflag = OFF ;
        /* new line */
        crlf() ;
        /* get command */
        cmd = *(sbuf+0) ;
        /* help */
        if ( cmd == '?' ) { show_help(); }
        if ( cmd == 'E' ) { rflag = ON ; }
        if ( cmd == 'D' ) { rflag = OFF; }
        if ( cmd == 'R' ) { send_right(calc_duty(75)); }
        if ( cmd == 'r' ) { send_right(  0); }
        if ( cmd == 'L' ) { send_left(calc_duty(75)) ; }
        if ( cmd == 'l' ) { send_left(  0) ; }
        if ( cmd == 'S' ) { show_sensor(); }
        if ( cmd == 'M' ) { show_mode(); }
      }
  } else {
    /* shift register handling */
    if ( sflag == ON ) {
      /* clear flag */
      sflag = OFF ;
      /* shift */
      swsft <<= 1 ;
      /* mask */
      swsft &= 3 ;
      /* get switch state */
      if ( (PINB & 8) == OFF ) { swsft |= ON ; }
      /* judge */
      if ( swsft == 1 ) {
        mcnt = mcnt + 1  ;
        mcnt = mcnt & ON ;
        rflag = mcnt & ON ;
      }
    }
    /* move handling */
    if ( tflag == ON ) {
      /* clear flag */
      tflag = OFF ;
      /* get sensor */
      sensor = get_sensor();
      /* default */
      rrx = 50 ;
      llx = 50 ;
      /* judge */
      if ( sensor == 1 ) {
        rrx = 60 ;
        llx = 40 ; 
      }
      if ( sensor == 4 ) {
        rrx = 40 ;
        llx = 60 ; 
      }
      /* impress */
      send_left( calc_duty(llx) );
      send_right( calc_duty(rrx) );
    }
  }
}

void update_trigger()
{
  tflag = ON ;
  /* increment */
  xcnt++ ;
  /* judge */
  if ( xcnt == 10 ) {
    xcnt = 0 ;
    ycnt++ ;
    sflag = ON ;
  }
  /* impress */
  PORTB &= ~(1 << LED_BIT) ;
  if ( ycnt & ON ) { PORTB |= (1 << LED_BIT) ; }
}

void show_help()
{
  rs_puts("? help")              ; crlf();
  rs_puts("E enable")            ; crlf();
  rs_puts("D disable")           ; crlf();
  rs_puts("R rotate right")      ; crlf();
  rs_puts("r stop right")        ; crlf();
  rs_puts("L rotate left")       ; crlf();
  rs_puts("l stop left")         ; crlf();
  rs_puts("S show sensor value") ; crlf();
  rs_puts("M show mode")         ; crlf();
}

void  show_sensor(void)
{
  byte tmp ;
  char smsg[4] ;
  /* get sensor */
  tmp = get_sensor() ;
  /* separate */
  *(smsg+3) = '\0' ;
  *(smsg+2) = (tmp & ON) + '0' ; tmp = tmp >> 1 ;
  *(smsg+1) = (tmp & ON) + '0' ; tmp = tmp >> 1 ;
  *(smsg+0) = (tmp & ON) + '0' ;
  /* show */
  rs_puts(smsg);
  /* new line */
  crlf();
}

void  show_mode(void)
{
  if ( mflag == ON ) { rs_puts("TEST"); }
  else               { rs_puts("MOVE"); }
  /* new line */
  crlf();
}

byte  get_hex(byte x)
{
  byte 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 rs_putchar(char x)
{
  Serial.write(x);
}

void rs_puts(char *ptr)
{
  while ( *ptr ) {
    rs_putchar( *ptr );
    ptr++;
  }
}

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

char get_asc(byte x)
{
  char result ;
  /* default */
  result = '0' ;
  /* convert */
  if ( x < 10 ) { result = x + '0' ; }
  if ( 9 <= x && x <= 16 ) { result = x - 10 + 'A' ; }

  return result ;
}

byte get_sensor()
{
  byte result ;
  byte tmp ;
  byte dh ;
  byte dl ;
  /* upper */
  PORTC |= (1 << SEL_BIT);
  dh = PINC & MASK0F ;
  /* lower */
  PORTC &= ~(1 << SEL_BIT);
  dl = PINC & MASK0F ;
  /* concatenate */
  tmp = dh | dl ;
  /* inverse */
  tmp ^= 0xff ;
  /* 6,3,1 bit */
  if ( tmp & 0x40 ) { result |= 4 ; }
  if ( tmp & 0x08 ) { result |= 2 ; }
  if ( tmp & 0x02 ) { result |= 1 ; }

  return result ;
}

byte calc_duty(byte x)
{
  word tmp ;
  byte result ;
  /* */
  tmp = (255 * x) / 100 ;
  /* tmp = 255 * (100 - x) / 100 ; */
  /* calculate */
  result = (byte)tmp ;

  return result ;
}

void send_left(byte x)
{
  analogWrite(LEFT,x);
}

void send_right(byte x)
{
  analogWrite(RIGHT,x);
}

/* receive interrupt */
void serialEvent()
{
  char ch;
  if ( Serial.available() > 0 ) {
    /* get 1 character */
    ch = Serial.read();
    /* store */
    *(sbuf+sindex) = ch ;
    /* increment */
    sindex++ ;
    /* judge */
    if ( ch == '\r' ) {
      sindex = 0 ; 
      uflag  = ON ;
    }
  }
}

　テストモードで操作すると、以下。



　コマンドを与えて、次の環境で双方のモータ回転を
　確認できました。

---

目次 前 次