LoginSignup
0
0

More than 1 year has passed since last update.

@hsgucci

EEZYbotARMを教示/再生で動かす

Last updated at Posted at 2023-05-18

はじめに

ここでは，EEZYbotARM mk1を予め準備した各関節の角度値で次々と動かしてみます．
イメージとしては，産業用ロボットのティーチング・プレイバック(教示/再生)に近いです．

目次へ戻るには ここ をクリック

※本ページは実験のテキストです．

概要

前回はシリアル通信でEEZYbotARM mk1を動かしました．
今回は，擬似的な教示/再生をやってみます．

ただ，教示と再生の両方の機能を実装するのは大変なので，

前回のシリアル通信のプログラムで目標の角度のパルス幅を調べておいて，
今回のプログラムでそれを時々刻々と再生していく，という形になります．

スケッチを書き込む

まずは、サンプルのプログラムを動かしてみましょう。

メニューバーから[ファイル]-[新規ファイル]をクリックし、新しいプログラムを書く準備をしてください。

以下のプログラムを打ち込み、コンパイル/実行してみましょう。
ファイル名は servo_play.ino としました。

servo_play.ino
// ライブラリの読み込み
#include <Wire.h>                     // I2C通信
#include <Adafruit_PWMServoDriver.h>  // PCA9685でサーボを動かす

// サーボのパルス幅(μs)
#define SERVO_MIN   800   // 最小パルス幅
#define SERVO_CENT 1500   // ニュートラル(センター)
#define SERVO_MAX  2200   // 最大パルス幅

// ロボットアームのピン番号
#define SERVO0_YAW    0
#define SERVO1_LINK1  1
#define SERVO2_LINK2  2
#define SERVO3_HAND   3
#define SERVO10_GATE  10

// サーボ制御クラスの作成
Adafruit_PWMServoDriver Servo = Adafruit_PWMServoDriver(0x40);  // PCA9685のI2Cアドレスは0x40番地

// サーボの現在位置が格納されている変数
int sv0 = 1500; // 初期値は1500us
int sv1 = 1500;
int sv2 = 1500;
int sv3 = 1500;
int sv10 = 1500;

// サーボの目標位置が格納される変数
int target_sv0 = 1500; // 初期値は1500us
int target_sv1 = 1500;
int target_sv2 = 1500;
int target_sv3 = 1500;
int target_sv10 = 1500;

// 初期化関数
void setup() {
  Serial.begin( 9600 );     // シリアル通信を初期化する。通信速度は9600bps

  // PWMドライバを初期化する
  Servo.begin();

  // PWM周波数を50Hzに設定する（デフォルト値）
  Servo.setPWMFreq(50);

  // 初期姿勢をセット
  Servo.writeMicroseconds(SERVO0_YAW,   sv0);
  Servo.writeMicroseconds(SERVO1_LINK1, sv1);
  Servo.writeMicroseconds(SERVO2_LINK2, sv2);
  Servo.writeMicroseconds(SERVO3_HAND,  sv3);
  Servo.writeMicroseconds(SERVO10_GATE,  sv10);

  // 1秒待機する
  delay(1000);

  Serial.println( "Hello Arduino!" );   // 最初に1回だけメッセージを表示する

  delay(1000);

  // ステップ１
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(2000);  // 2000ミリ秒かけて目標値に行く

  // ステップ２ 
  target_sv0 = 2000;
  writeServos(2000);

  // ステップ３
  target_sv1 = 1000;  // SERVO1_LINK1の目標値
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(1000);  // 1000ミリ秒かけて目標値へ行く

  // ステップ４
  target_sv1 = 2000;
  target_sv0 = 2000;
  writeServos(2000);

  // 以降のステップを自分で増やす
}

// ループ関数
void loop() {
}


// サーボの角度を徐々に変える関数
void writeServos(int duration_ms) {

  // かける時間が0のときは，一気に目標値へ行く
  if ( duration_ms != 0) {
    // 20ミリ秒で割って，繰り返し回数を決める
    int time_step = duration_ms / 20;

    // それぞれのサーボの20ミリ秒おきの加算量を計算
    float sv0_step = ( target_sv0 - sv0 ) / time_step;
    float sv1_step = ( target_sv1 - sv1 ) / time_step;
    float sv2_step = ( target_sv2 - sv2 ) / time_step;
    float sv3_step = ( target_sv3 - sv3 ) / time_step;
    float sv10_step = ( target_sv10 - sv10 ) / time_step;

    // ループ
    for ( int i = 0; i < time_step; i++ ) {
      
      // １回分加算
      int val0 = sv0 + i * sv0_step;
      int val1 = sv1 + i * sv1_step;
      int val2 = sv2 + i * sv2_step;
      int val3 = sv3 + i * sv3_step;
      int val10 = sv10 + i * sv10_step;

      // 加算された量を出力
      Servo.writeMicroseconds(SERVO0_YAW, val0 );
      Servo.writeMicroseconds(SERVO1_LINK1, val1 );
      Servo.writeMicroseconds(SERVO2_LINK2, val2);
      Servo.writeMicroseconds(SERVO3_HAND,  val3);
      Servo.writeMicroseconds(SERVO10_GATE, val10);

      delay(17);  // 本来は20ミリ秒のdelayなのだが，20にすると時間が長くなるので
    }

    // 最後は目標値に一致
    sv0 = target_sv0;
    sv1 = target_sv1;
    sv2 = target_sv2;
    sv3 = target_sv3;
    sv10 = target_sv10;

  } else { // duration_ms == 0のとき
    // 一気に目標値へ
    sv0 = target_sv0;
    sv1 = target_sv1;
    sv2 = target_sv2;
    sv3 = target_sv3;
    sv10 = target_sv10;
  }

  // サーボに出力
  Servo.writeMicroseconds(SERVO0_YAW,   sv0);
  Servo.writeMicroseconds(SERVO1_LINK1, sv1);
  Servo.writeMicroseconds(SERVO2_LINK2, sv2);
  Servo.writeMicroseconds(SERVO3_HAND,  sv3);
  Servo.writeMicroseconds(SERVO10_GATE, sv10);

  // シリアルモニタに最終の出力値を表示
  Serial.print( sv0 );
  Serial.print("\t");
  Serial.print( sv1 );
  Serial.print("\t");
  Serial.print( sv2 );
  Serial.print("\t");
  Serial.print( sv3 );
  Serial.print("\t");
  Serial.println( sv10 );
}

動作テスト

Arduinoに書き込みが終わると，ロボットアームに力が入って動きます．

まずは，土台のサーボが2秒間かけて1000マイクロ秒の位置まで移動 します．

次に，2秒間かけて2000マイクロ秒の位置まで移動 します．

次は, 1秒間かけて,土台が1000マイクロ秒，リンク１も1000マイクロ秒の位置まで移動 します.

次は, 2秒間かけて,土台が2000マイクロ秒，リンク１も2000マイクロ秒の位置まで移動 します.

これだけで終了です．

プログラム解説

それではプログラムを解説していきます．

準備

#includeや#defineについては，
前回のシリアル通信と同じなので割愛します．

グローバル変数に，目標位置を格納する変数が追加されました。

目標位置のグローバル変数
// サーボの目標位置が格納される変数
int target_sv0 = 1500; // 初期値は1500us
int target_sv1 = 1500;
int target_sv2 = 1500;
int target_sv3 = 1500;
int target_sv10 = 1500;

EEZYbotARMの姿勢を変えるには、今までのプログラムとは異なり、この変数に目標値を書き込んでください．

ループ関数

先にループ関数を解説しておきますが，
今回はループ関数は空です．

ループ関数にサーボの動作プログラムを書いてしまうと，
何度も何度も繰り返して実行してしまうので，
動作の再生部分はsetup関数に書き込みます．

loop関数
// ループ関数
void loop() {
   // 特に何もしない
}

初期化関数

次は初期化関数です．

Servoクラスの初期化や，初期角度の出力はこれまでと同様です．

setup関数
// 初期化関数
void setup() {
  Serial.begin( 9600 );     // シリアル通信を初期化する。通信速度は9600bps

  // PWMドライバを初期化する
  Servo.begin();

  // PWM周波数を50Hzに設定する（デフォルト値）
  Servo.setPWMFreq(50);

  // 初期姿勢をセット
  Servo.writeMicroseconds(SERVO0_YAW,   sv0);
  Servo.writeMicroseconds(SERVO1_LINK1, sv1);
  Servo.writeMicroseconds(SERVO2_LINK2, sv2);
  Servo.writeMicroseconds(SERVO3_HAND,  sv3);
  Servo.writeMicroseconds(SERVO10_GATE,  sv10);

  // 1秒待機する
  delay(1000);

  Serial.println( "Hello Arduino!" );   // 最初に1回だけメッセージを表示する

  delay(1000);

  // ステップ１
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(2000);  // 2000ミリ秒かけて目標値に行く

  // ステップ２ 
  target_sv0 = 2000;
  writeServos(2000);

  // ステップ３
  target_sv1 = 1000;  // SERVO1_LINK1の目標値
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(1000);  // 1000ミリ秒かけて目標値へ行く

  // ステップ４
  target_sv1 = 2000;
  target_sv0 = 2000;
  writeServos(2000);

  // 以降のステップを自分で増やす
}

今回注目する点は，writeServos関数です．

writeServosの使用例
  writeServos(かける時間のミリ秒);

  // それぞれの目標値を先に書いてから、WriteServos関数を呼ぶだけ
  target_sv1 = 1000;  // SERVO1_LINK1の目標値
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(1000);  // 1000ミリ秒かけて目標値へ行く

writeServos関数

この関数は，運営側で用意したものです．
ブラックボックスとして呼び出して使えば十分ですが，中身に興味があったら読んでみてください．

この関数，実はベースになる部分はChatGPTが作ってくれた んです．
そのままでは使えなかったので，色々改変して現在の形になっています．

writeServos関数
// サーボの角度を徐々に変える関数
void writeServos(int duration_ms) {

  // かける時間が0のときは，一気に目標値へ行く
  if ( duration_ms != 0) {
    // 20ミリ秒で割って，繰り返し回数を決める
    int time_step = duration_ms / 20;

    // それぞれのサーボの20ミリ秒おきの加算量を計算
    float sv0_step = ( target_sv0 - sv0 ) / time_step;
    float sv1_step = ( target_sv1 - sv1 ) / time_step;
    float sv2_step = ( target_sv2 - sv2 ) / time_step;
    float sv3_step = ( target_sv3 - sv3 ) / time_step;
    float sv10_step = ( target_sv10 - sv10 ) / time_step;

    // ループ
    for ( int i = 0; i < time_step; i++ ) {
      
      // １回分加算
      int val0 = sv0 + i * sv0_step;
      int val1 = sv1 + i * sv1_step;
      int val2 = sv2 + i * sv2_step;
      int val3 = sv3 + i * sv3_step;
      int val10 = sv10 + i * sv10_step;

      // 加算された量を出力
      Servo.writeMicroseconds(SERVO0_YAW, val0 );
      Servo.writeMicroseconds(SERVO1_LINK1, val1 );
      Servo.writeMicroseconds(SERVO2_LINK2, val2);
      Servo.writeMicroseconds(SERVO3_HAND,  val3);
      Servo.writeMicroseconds(SERVO10_GATE, val10);

      delay(17);  // 本来は20ミリ秒のdelayなのだが，20にすると時間が長くなるので
    }

    // 最後は目標値に一致
    sv0 = target_sv0;
    sv1 = target_sv1;
    sv2 = target_sv2;
    sv3 = target_sv3;
    sv10 = target_sv10;

  } else { // duration_ms == 0のとき
    // 一気に目標値へ
    sv0 = target_sv0;
    sv1 = target_sv1;
    sv2 = target_sv2;
    sv3 = target_sv3;
    sv10 = target_sv10;
  }

  // サーボに出力
  Servo.writeMicroseconds(SERVO0_YAW,   sv0);
  Servo.writeMicroseconds(SERVO1_LINK1, sv1);
  Servo.writeMicroseconds(SERVO2_LINK2, sv2);
  Servo.writeMicroseconds(SERVO3_HAND,  sv3);
  Servo.writeMicroseconds(SERVO10_GATE, sv10);

  // シリアルモニタに最終の出力値を表示
  Serial.print( sv0 );
  Serial.print("\t");
  Serial.print( sv1 );
  Serial.print("\t");
  Serial.print( sv2 );
  Serial.print("\t");
  Serial.print( sv3 );
  Serial.print("\t");
  Serial.println( sv10 );
}

プログラムの改良

現在のプログラムは，土台とリンク１のサーボが少し動くだけです．

これに，関数を書き加えて他の関節も動くように書き換えてみてください．

次々と動きを書き加えて，一連の動作パターンを作成する
  target_sv2 = 1000;  // SERVO2_LINK2の目標値
  target_sv3 = 1000;  // SERVO3_HANDの目標値
  writeServos(2000);  // 2000ミリ秒かけて目標値へ行く


  target_sv1 = 1000;  // SERVO1_LINK1の目標値
  target_sv0 = 1000;  // SERVO0_YAWの目標値
  writeServos(1000);  // 1000ミリ秒かけて目標値へ行く

  ...

目標の角度のパルス幅をいくつにするか調べるには，
前回のシリアル通信のプログラムを再度書き込んで，
シリアルモニタで値を見てメモっておくしか方法がありません．．．

ちょっと不便ですね(>_<

まとめ

今回はwriteServos関数を利用して，「関節角度を徐々に動かす」という動作をしてみました．

今回のプログラムは，教示と再生の両方を実装していません．
再生機能しかないため，教示データを得るのが大変面倒です．

教示モードのプログラムと，再生モードのプログラムを切り替えて動かせるようにするプログラムは，簡単には書くことができません．

複数の機能を同時に実現するのは，大変難しいプログラミングです．

こちらも，興味ある人はチャレンジしてみてください．

おわりに

これでEEZYbotARMのプログラミングは終了です．

目次 へ戻って次の作業を行ってください。

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0