目次

マニュピュレータロボット制御スケッチ

 サーボモータを多数利用した身近な例は
 マニュピュレータロボットでしょう。



 手元にあるマニュピュレータロボットは
 サーボモータを5個利用した5軸です。

 サーボモータを制御するには、関数analogWriteを
 利用しますが、PWM波形を出力できるピンは固定
 されています。



 5、6、9、10、11の5ピンを
 サーボモータ制御に割り当てます。

 Arduinoの出力では、サーボモータを駆動できる
 パワーがないので、フォトカプラ利用ドライブ
 回路を接続します。



 回路は簡単で、抵抗とフォトカプラがあるだけ。



 フォトカプラを入れるのは、Arduinoを大電力回路
 からのバックラッシュから保護するため。

 Arduinoとは、14ピンケーブルとブレッドボード
 ワイヤーを使い接続します。





 マニュピュレータで利用しているサーボモータは
 12V電源なので、ACアダプターを駆動回路に
 接続します。



 サーボモータからは、3ピンワイヤーが出ています。



 中央に電源、左右にGNDとパルス入力が配置されています。

 左右を間違えてコネクタに接続しても、モータが
 電源の逆挿しで破損しないための、工夫です。

 Arduinoで5個のサーボモータを制御するので
 ピンアサインを決めます。

 5 Finger
 6 Wrist
 9 Arm
10 Elbow
11 Body

 ピンアサイン仕様から、Arduinoのコードで使える
 記述を定義します。

#define Finger 5
#define Wrist  6
#define Arm    9
#define Elbow 10
#define Body  11

 5個のサーボモータは、角度で0〜180まで扱えるので
 対応する変数を用意しておきます。

int angleFinger ;
int angleWrist  ;
int angleArm    ;
int angleElbow  ;
int angleBody   ;

 コマンドインタプリタで、サーボモータの各軸の
 角度を指定された場合の処理を考えます。

 コマンドインタプリタで利用する1文字コマンド
 を次のように決めました。

 コマンドインタプリタで参照する文字列が
 配列sbufに含まれているとし、処理を定義
 します。

    // get command 
    cmd = sbuf[0] ;
    // judge
    if ( cmd == '?' ) { show_help() ; }
    if ( is_scmd(cmd) == ON ) { 
      val = get_value() ;
      if ( val > MAXANGLE ) { val = MAXANGLE ; }
      switch ( cmd ) { 
        case 'f' : angleFinger = val ; break ;
        case 'w' : angleWrist  = val ; break ;
        case 'a' : angleArm    = val ; break ;
        case 'e' : angleElbow  = val ; break ;
        case 'b' : angleBody   = val ; break ;
      }
    }

 コマンドインタプリタで利用している関数を
 定義していきます。

 show_help
  コマンドの説明を表示すればよいので
  Serialオブジェクトのprintlnメソッド
  を使い、羅列すれば充分。

      void show_help()
      {
        Serial.println("? help");
        Serial.println("f set finger degree");
        Serial.println("w set wrist  degree");
        Serial.println("a set arm    degree");
        Serial.println("e set elbow  degree");
        Serial.println("b set body   degree");
      }

 is_scmd
  コマンド1文字が、サーボモータの角度を指定
  するかどうかで、計算処理をするか否かを判定
  します。

  各関節のパラメータを指定するコマンドである
  ときに、ONを返すようにします。

      int is_scmd(char x) {
        int result ;
        // default
        result = OFF ;
        // judge
        if ( x == 'f' ) { result = ON ; }
        if ( x == 'w' ) { result = ON ; }
        if ( x == 'a' ) { result = ON ; }
        if ( x == 'e' ) { result = ON ; }
        if ( x == 'b' ) { result = ON ; }

        return result ;
      }

 get_value
  バッファsbufの先頭にコマンド1文字、続けて
  角度が10進数で入ってくるとします。

  例) w123{enter}


  sbufの添字1から最大3数字を取出して
  数値に変換すればよいでしょう。

  コマンドはパラメータを含めて'\r'で終了するので
  これをターミネータとし、計算します。

      int get_value() {
        int result ;
        char i ;
        // default
        result = 0 ;
        // calculate
        for ( i = 0 ; i < 3 ; i++ ) {
          if ( sbuf[i+1] == '\r' ) break ;
          result = result * 10 + (sbuf[i+1] - '0') ;
        }
        return result ;
      }

 コマンドインタプリタが出来れば、端末から
 送信されてくる文字列を指定バッファに格納
 する処理を考えます。

 受信割込みで、文字列を指定バッファに格納
 することにします。Serialでは、割込みを
 serialEventという関数で定義します。

 受信割込みハンドラは、次のように定義。

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

 コマンド、パラメータが格納されたかを
 フラグuflagで通知します。

 loopをまとめます。

void loop()
{
  // serial handling
  if ( uflag == ON ) {
    uflag = OFF ;
    // get command 
    cmd = sbuf[0] ;
    // judge
    if ( cmd == '?' ) { show_help() ; }
    if ( is_scmd(cmd) == ON ) { 
      val = get_value() ;
      if ( val > MAXANGLE ) { val = MAXANGLE ; }
      switch ( cmd ) { 
        case 'f' : angleFinger = val ; break ;
        case 'w' : angleWrist  = val ; break ;
        case 'a' : angleArm    = val ; break ;
        case 'e' : angleElbow  = val ; break ;
        case 'b' : angleBody   = val ; break ;
      }
    }
  }
  // impress
  finger.write(angleFinger);
  wrist.write(angleWrist);
  arm.write(angleArm);
  elbow.write(angleElbow);
  body.write(angleBody);
  // delay
  delay(xinterval);
}

 setupは、Servoオブジェクトのピンアサインと
 初期値の90度を設定しておきます。

void setup()
{
  // serial
  Serial.begin(9600);
  // attaches the servo
  finger.attach(Finger);
  wrist.attach(Wrist);
  arm.attach(Arm);
  elbow.attach(Elbow);
  body.attach(Body);
  // place 90 degree
  angleFinger = 90 ;
  angleWrist  = 90 ;
  angleArm    = 90 ;
  angleElbow  = 90 ;
  angleBody   = 90 ;
  finger.write(angleFinger);
  wrist.write(angleWrist);
  arm.write(angleArm);
  elbow.write(angleElbow);
  body.write(angleBody);
  // delay counter
  xinterval = INTERV ;
  // command interpreter buffer
  uflag = OFF ;
  sindex = 0 ;
}

 スケッチに、まとめます。

#include <Servo.h>

#define OFF 0
#define ON  OFF+1

// pin assignment
#define Finger 5
#define Wrist  6
#define Arm    9
#define Elbow 10
#define Body  11

#define MAXANGLE 180

#define INTERV 200

// create servo object to control servo motors
Servo finger ;
Servo wrist  ;
Servo arm    ;
Servo elbow  ;
Servo body   ;

char cmd ;
int  xinterval ;
char uflag ;
char sbuf[8] ;
char sindex ;
int  val ;

int angleFinger ;
int angleWrist  ;
int angleArm    ;
int angleElbow  ;
int angleBody   ;

void show_help()
{
  Serial.println("? help");
  Serial.println("f set finger degree");
  Serial.println("w set wrist  degree");
  Serial.println("a set arm    degree");
  Serial.println("e set elbow  degree");
  Serial.println("b set body   degree");
}

int get_value() {
  int result ;
  char i ;
  // default
  result = 0 ;
  // calculate
  for ( i = 0 ; i < 3 ; i++ ) {
    if ( sbuf[i+1] == '\r' ) break ;
    result = result * 10 + (sbuf[i+1] - '0') ;
  }
  return result ;
}

int is_scmd(char x) {
  int result ;
  // default
  result = OFF ;
  // judge
  if ( x == 'f' ) { result = ON ; }
  if ( x == 'w' ) { result = ON ; }
  if ( x == 'a' ) { result = ON ; }
  if ( x == 'e' ) { result = ON ; }
  if ( x == 'b' ) { result = ON ; }

  return result ;
}

void setup()
{
  // serial
  Serial.begin(9600);
  // attaches the servo
  finger.attach(Finger);
  wrist.attach(Wrist);
  arm.attach(Arm);
  elbow.attach(Elbow);
  body.attach(Body);
  // place 90 degree
  angleFinger = 90 ;
  angleWrist  = 90 ;
  angleArm    = 90 ;
  angleElbow  = 90 ;
  angleBody   = 90 ;
  finger.write(angleFinger);
  wrist.write(angleWrist);
  arm.write(angleArm);
  elbow.write(angleElbow);
  body.write(angleBody);
  // delay counter
  xinterval = INTERV ;
  // command interpreter buffer
  uflag = OFF ;
  sindex = 0 ;
}

void loop()
{
  // serial handling
  if ( uflag == ON ) {
    uflag = OFF ;
    // get command 
    cmd = sbuf[0] ;
    // judge
    if ( cmd == '?' ) { show_help() ; }
    if ( is_scmd(cmd) == ON ) { 
      val = get_value() ;
      if ( val > MAXANGLE ) { val = MAXANGLE ; }
      switch ( cmd ) { 
        case 'f' : angleFinger = val ; break ;
        case 'w' : angleWrist  = val ; break ;
        case 'a' : angleArm    = val ; break ;
        case 'e' : angleElbow  = val ; break ;
        case 'b' : angleBody   = val ; break ;
      }
    }
  }
  // impress
  finger.write(angleFinger);
  wrist.write(angleWrist);
  arm.write(angleArm);
  elbow.write(angleElbow);
  body.write(angleBody);
  // delay
  delay(xinterval);
}

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

 このスケッチを使うときは、ArduinoIDEの端末
 エミュレータでは'\r'を入力できないので、
 TeraTermを使って処理します。

目次

inserted by FC2 system