目次 前 次

---

サーボモータテスト

　部屋の片付けをしていると、小さなサーボモータが
　出てきました。



　銘版がないので、どんな仕様か不明で、動くのかどうかも
　わからないので、Arduinoスケッチで動作確認してみます。

　用意したスケッチは、以下。

#include <MsTimer2.h>
#include <Servo.h>

#define OFF 0
#define ON  OFF+1

#define LED_BIT 5

Servo xSrv ;

/* variables */
boolean uflag ;
boolean eflag ;
boolean tflag ;

byte xcnt ;
char sbuf[16] ;
byte sindex ;

byte cmd ;

byte xangle ;

void update_trigger();
void show_help();
void rs_putchar(char x);
void rs_puts(char *ptr);
void crlf();
void show_val();
void rs_print(byte x);
byte get_hex(char x);
char get_asc(byte x);
void set_angle();
void get_angle();

void setup()
{
  Serial.begin(9600);
  rs_puts("Hello !");
  crlf();
  show_help();
  crlf();
  /* port value */
  PORTB = 0x00 ;
  PORTC = 0x00 ;
  PORTD = 0x00 ;
  /* port direction */
  DDRB = 0xff ;
  DDRC = 0xf0 ;
  DDRD = 0xfe ;
  /* flags */
  uflag = OFF ;
  eflag = OFF ;
  tflag = OFF ;
  /* initialize counter*/
  xcnt = 0 ;
  /* initialize servo motor */
  xSrv.attach(11,1000,2000);
  xSrv.write(90);
  /* 1000ms period */
  MsTimer2::set(1000,update_trigger);
  MsTimer2::start();
}

void loop()
{
  /* command interpreter */
  if ( uflag == ON ) {
    /* clear flag */
    uflag = OFF ;
    /* get command */
    cmd = *(sbuf+0) ;
    /* help */
    if ( cmd == '?' ) { show_help(); }
    /* set angle */
    if ( cmd == 'A' ) { set_angle(); }
    /* show angle */
    if ( cmd == 'a' ) { get_angle(); }
    /* enable */
    if ( cmd == 'G' ) { 
      eflag = ON ;
      xangle = 0 ; 
    }
    /* disable */
    if ( cmd == 'g' ) {
      eflag = OFF ; 
      xangle = 0 ; 
    }
    /* new line */
    crlf() ;
  }
  /* timer handling */
  if ( tflag == ON ) {
    tflag = OFF ;
    if ( eflag == ON ) {
      xSrv.write(xangle);
      /* update */
      xangle += 15 ;
      /* judge */
      if ( xangle > 180 ) { xangle = 0 ; }
    }
  }
}

void update_trigger()
{
  /* set flag */
  tflag = ON ;
  /* inrement */
  xcnt++ ;
  /* flashing */
  PORTB &= ~(1 << LED_BIT) ;
  if ( xcnt & ON ) { PORTB |= (1 << LED_BIT) ; }
}

void show_help()
{
  rs_puts("? help")      ; crlf();
  rs_puts("A set angle") ; crlf();
  rs_puts("a get angle") ; crlf();
  rs_puts("G enable")    ; crlf();
  rs_puts("g disable")   ; crlf();
}

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');
}

byte get_hex(char x)
{
  byte result ;
  /* default */
  result = 15 ;
  /* 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 ;
}

char get_asc(byte x)
{
  char result ;
  /* default */
  result = '0' ;
  /* judge */
  if ( x < 10 ) { result = x + '0' ; }
  else {
    result = x - 10 + 'A' ;    
  }

  return result ;
}

void set_angle()
{
  byte aa;
  byte result ;
  byte ii ;
  byte jj ;
  /* default */
  result = 0 ;
  /* caoulate */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    /* get ascii code */
    aa = *(sbuf+1+ii) ;
    /* judge */
    if ( aa == '\r' ) break ;
    /* calculate */
    result = result * 10 + get_hex( aa ) ;
  }
  /* reange check */
  if ( result > 180 ) { result = 180 ; }
  /* set */
  xSrv.write( result ) ;
}

void get_angle()
{
  byte aa[3];
  byte result ;
  byte ii ;
  byte jj ;
  /* get current angle */
  result = xSrv.read();
  /* separate */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    jj = 2 - ii ;
    *(aa+jj) = result % 10 ;
    result /= 10 ;
  }
  /* convert */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    *(aa+ii) = get_asc( *(aa+ii) );
  }
  /* zero surpress */
  if ( *(aa+0) == '0' ) {
    *(aa+0) = ' ' ;
    if ( *(aa+1) == '0' ) {
      *(aa+1) = ' ' ;
    }
  }
  /* show */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    rs_putchar( *(aa+ii) );
  }
  /* new line */
  crlf();
}

/* 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 ;
    }
  }
}

　コマンドインタプリタを用意して、インタラクティブに
　サーボモータの動きを扱えるようにしています。

　コマンドの内訳は、以下。

? コマンド一覧表示
A 角度設定（０から１８０の整数）
a 現在の角度表示
G 角度を自動で増やす処理開始
g 角度を自動で増やす処理終了

　コマンド'G'、'g'は、フラグ処理にし、他は
　専用関数を定義して対応。

　コマンド一覧表示

　　プリミティブの文字列表示関数rs_putsがあるとして
　　それを呼び出して、対応する処理を実現。

void show_help()
{
  rs_puts("? help")      ; crlf();
  rs_puts("A set angle") ; crlf();
  rs_puts("a get angle") ; crlf();
  rs_puts("G enable")    ; crlf();
  rs_puts("g disable")   ; crlf();
}

　　１文字表示の関数を定義し、その関数を使い
　　他の関数をラッパーとして定義。

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');
}

　角度設定

　　コマンド'A'に続けて、最大３個までの数字が
　　受信バッファに入っているので、それらを取得
　　して、ArduinoAPIに渡します。

void set_angle()
{
  byte aa;
  byte result ;
  byte ii ;
  byte jj ;
  /* default */
  result = 0 ;
  /* caoulate */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    /* get ascii code */
    aa = *(sbuf+1+ii) ;
    /* judge */
    if ( aa == '\r' ) break ;
    /* calculate */
    result = result * 10 + get_hex( aa ) ;
  }
  /* reange check */
  if ( result > 180 ) { result = 180 ; }
  /* set */
  xSrv.write( result ) ;
}

　　数字を数値に変換する関数を用意して、対応。

byte get_hex(char x)
{
  byte result ;
  /* default */
  result = 15 ;
  /* 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 ;
}

　角度表示

　　ArduinoAPIで、サーボモータの角度値を取得できます。
　　角度値を取得したなら、10進数に変換後表示するだけ。

void get_angle()
{
  byte aa[3];
  byte result ;
  byte ii ;
  byte jj ;
  /* get current angle */
  result = xSrv.read();
  /* separate */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    jj = 2 - ii ;
    *(aa+jj) = result % 10 ;
    result /= 10 ;
  }
  /* convert */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    *(aa+ii) = get_asc( *(aa+ii) );
  }
  /* zero surpress */
  if ( *(aa+0) == '0' ) {
    *(aa+0) = ' ' ;
    if ( *(aa+1) == '0' ) {
      *(aa+1) = ' ' ;
    }
  }
  /* show */
  for ( ii = 0 ; ii < 3 ; ii++ ) {
    rs_putchar( *(aa+ii) );
  }
  /* new line */
  crlf();
}

　　10進数でも、最大３けたなので、２けた、１けたの
　　場合は、上位の０をスペースに置換して、見やすく
　　しています。

　　数値を数字に変換するための関数を用意。

char get_asc(byte x)
{
  char result ;
  /* default */
  result = '0' ;
  /* judge */
  if ( x < 10 ) { result = x + '0' ; }
  else {
    result = x - 10 + 'A' ;
  }

  return result ;
}

　コマンドインタプリタで必要となる関数を定義したので
　その部分をまとめると、以下。

  if ( uflag == ON ) {
    /* clear flag */
    uflag = OFF ;
    /* get command */
    cmd = *(sbuf+0) ;
    /* help */
    if ( cmd == '?' ) { show_help(); }
    /* set angle */
    if ( cmd == 'A' ) { set_angle(); }
    /* show angle */
    if ( cmd == 'a' ) { get_angle(); }
    /* enable */
    if ( cmd == 'G' ) { 
      eflag = ON ;
      xangle = 0 ; 
    }
    /* disable */
    if ( cmd == 'g' ) {
      eflag = OFF ; 
      xangle = 0 ; 
    }
    /* new line */
    crlf() ;
  }

　イベント通知フラグを用意して、受信バッファに
　処理すべき情報が入ったことを認識させます。

　自動で角度を増やすために、タイマー割込みを使います。

　タイマー割込みで、指定時間経過をイベント通知
　フラグを使ってloop()に知らせます。

　イベント通知フラグを使えば、角度の自動インクリメントは
　次のように記述できます。

  if ( tflag == ON ) {
    tflag = OFF ;
    if ( eflag == ON ) {
      xSrv.write(xangle);
      /* update */
      xangle += 15 ;
      /* judge */
      if ( xangle > 180 ) { xangle = 0 ; }
    }
  }

　時間経過の通知の他に、角度のインクリメントを
　やっていよいか否かをフラグで制御します。

　角度のインクリメントをするか、しないかを
　フラグで制御するので、コマンド'G'、'g'で
　セット、リセットを担当。

　端末ソフトを利用して、サーボモータの動きを
　制御すると、次のようになります。



　このスケッチで、銘板のないサーボモータは
　壊れているとわかりました。

　銘板のついた、次のサーボモータは動いたので
　比べることで、OKとNGを判断できました。

---

目次 前 次