LoginSignup
4
1

More than 3 years have passed since last update.

@pbjpkas

Lチカで始めるテスト自動化(9)6DoFロボットアームの組込み

Posted at

1. はじめに

Lチカで始めるテスト自動化シリーズ第九弾です。

GUIの自動テストやIoTデバイスを組み込んだシステムのE2E自動テストができるようロボットアームで機器を操作できるようにします。

アマゾンで数千円で売られている6自由度のロボットアームをシリアル通信でコマンド制御できるようにしてテストベンチに組み込み、テストスクリプトにコマンドを記述してアームを動かします。

qiita.jpg

これまでの記事はこちらをご覧ください。

  1. Lチカで始めるテスト自動化
  2. Lチカで始めるテスト自動化(2)テストスクリプトの保守性向上
  3. Lチカで始めるテスト自動化(3)オシロスコープの組込み
  4. Lチカで始めるテスト自動化(4)テストスクリプトの保守性向上(2)
  5. Lチカで始めるテスト自動化(5)WebカメラおよびOCRの組込み
  6. Lチカで始めるテスト自動化(6)AI(機械学習)を用いたPass/Fail判定
  7. Lチカで始めるテスト自動化(7)タイムスタンプの保存
  8. Lチカで始めるテスト自動化(8)HDMIビデオキャプチャデバイスの組込み

2. パーツ

パーツ 型番
ロボットアーム DiyStudio 6自由度ロボットアームDIYキット1
六角スペーサー M3x10mm x2(グリッパーをサーボホーンに取り付けるために使用)
Arduinoボード Arduino Leonardo
USB-UART変換 FT232RQ USBシリアル変換モジュールキット(秋月電子通商)
Arduinoシールド基板 プロトタイプシールド基板[UBD-ARD53X68-HL2]2(aitendo)
足の長いピンソケット 6ピン x1、8ピン x2、10ピン x13
単列ピンヘッダ 4ピン x1、6ピン x1、7ピン x24
DCジャック ブレッドボード用DCジャックDIP化キット(秋月電子通商)
ブレッドボードのジャンパーピン ・メス-メス x2(DCジャックとサーボ給電用ピンヘッダの接続)
・オス-メス x1(DCジャックのGNDとArduinoのGND接続)
・オス-オス x5(USBシリアル変換モジュールとArduinoの接続)
電源 手持ちの5V 3AのACアダプタ

Arduino Leonardoを利用しているのは、Arduino UNO R3は通信開始時にリセットがかかってサーボが暴走することがあったためです。

2.1 追加で欲しくなったパーツ

グリッパーのサーボは延長ケーブルが欲しいトコロ。

  • サーボの延長ケーブル
  • M3のナイロンナット x4
  • M3x5mmのスクリューキャップ x1（グリッパーをサーボホーンへ取り付ける用として）
  • DC 5Vの容量の大きなアダプタ

3. 回路

以下にArduinoのサーボシールドを示します。

  • この回路はシステムの実現性の確認を目的とした実験室レベルのものです。保護回路などは特に入っていませんので適宜安全設計を行ってください。
  • Arduino Leonardoへの給電はPCと接続したUSBシリアル変換モジュールから行います。
  • サーボへの給電はACアダプタで行い、Arduino Leonardoは制御信号の供給のみ行います。

servo-shield-detail.png

4. Arduinoファームウェア

4.1 コマンド

コマンド 引数 機能
echo <flag> コマンド操作のエコーバック、flag=0でOFF、flag=1でON
servo <id> <rotate> サーボの回転制御、id:0～5、rotate:0～180
servoread なし サーボの回転角度の表示
  • エコーバックはteratermなどで手動で操作するには必要ですがテストスクリプトで自動実行する場合はない方が都合が良いです。初期値をソースコードの237行目で設定しています。
  • サーボのIDは土台に近い側から順に0～5を振っています。

4.2 ソース

  • ArduinoのServoライブラリを利用しています。
  • サーボはArduinoのDigital Pin #2～#7に配線しています。
  • サーボごとに回転角度の初期値や下限、上限を設定できるようservoという派生クラスを作っています。
    • 特にグリッパーは開閉に必要なサーボの回転角度が約60°で、この範囲を超えてサーボを動かすとサーボを破損する恐れがあります。
    • 設定した範囲を超えてサーボが回転しないようservo::rotate()を実装しています。
  • コマンド送受信はFT232RQ USBシリアル変換モジュールを経由してSerial1で行います。
Arduino6DoFArm.ino
/***********************************************************************
 * Arduino6DoFArm
 * 2020-07-24 by ka's
 ***********************************************************************/
/***********************************************************************
 * Copyright 2020 ka's@pbjpkas
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ***********************************************************************/
#include <Servo.h>

#define ERR_OK       0
#define ERR_INVALID -1 // 不正
#define ERR_NULL    -2 // 引数がNULL
#define ERR_MALLOC  -3 // mallocの戻り値がNULL

void setup();
void loop();

/***********************************************************************
  Servo
 ***********************************************************************/
class servo : public Servo
{
    public:
        servo(int id, int pin, uint8_t initial, uint8_t lower, uint8_t upper);
        ~servo();
        initialize();
        rotate(int value);
    private:
        int     servo_id;
        int     servo_pin;
        uint8_t rotate_initial;//初期値
        uint8_t rotate_lower;  //可動範囲(下側)
        uint8_t rotate_upper;  //可動範囲(上側)
};

servo::servo(int id, int pin, uint8_t initial, uint8_t lower, uint8_t upper)
{
    servo_id       = id;
    servo_pin      = pin;
    rotate_initial = initial;
    rotate_lower   = lower;
    rotate_upper   = upper;
}

servo::~servo()
{
    detach();
}

servo::initialize()
{
    attach(servo_pin);
    rotate(rotate_initial);
}

servo::rotate(int value)
{
    if(value<rotate_lower or rotate_upper<value)
    {
        Serial1.print("### out of range:");
        Serial1.print(servo_id);
        Serial1.print(" ");
        Serial1.println(value);
        return ERR_INVALID;
    }
    else
    {
        write(value);
        return ERR_OK;
    }
}

servo servo00(0, 2,  85,  40, 140);
servo servo01(1, 3,  90,  75, 110);
servo servo02(2, 4,  90,  50, 110);
servo servo03(3, 5,  90,  50, 110);
servo servo04(4, 6,  90,   0, 180);
servo servo05(5, 7, 170, 120, 180);

void servo_init()
{
    servo05.initialize();
    delay(500);
    servo00.initialize();
    delay(500);
    servo01.initialize();
    delay(500);
    servo02.initialize();
    delay(500);
    servo03.initialize();
    delay(500);
    servo04.initialize();
    delay(500);
}

void servo_read()
{
    Serial1.print(servo00.read());
    Serial1.print(" ");
    Serial1.print(servo01.read());
    Serial1.print(" ");
    Serial1.print(servo02.read());
    Serial1.print(" ");
    Serial1.print(servo03.read());
    Serial1.print(" ");
    Serial1.print(servo04.read());
    Serial1.print(" ");
    Serial1.print(servo05.read());
    Serial1.print("\r\n");    
}

/***********************************************************************
  Function Prototype : Command Mode
 ***********************************************************************/
#define CMD_OK          ERR_OK
#define CMD_BUF_LENGTH      64 // 63+1
#define CMD_MAX_LENGTH      64 // 63+1
#define ARG_MAX_LENGTH      64 // 63+1

void cmd_print_help(void);
void cmd_print_ver(void);
int  cmd_execute(bool *echoback, char *buf);
void cmd_rx_data(void);

/***********************************************************************
  Function : Command Mode
 ***********************************************************************/
void cmd_print_help(void)
{
    Serial1.println(F("Available Command:"));
    Serial1.println(F("help, ?             : print Help Messages"));
    Serial1.println(F("ver                 : print Version Information"));
    Serial1.println(F("echo <flag>         : flag(0 to off, 1 to on)"));
    Serial1.println(F("servo <id> <rotate> : id(0-5), rotate(0-180)"));
    Serial1.println(F("servoread           : print rotation value"));
}
void cmd_print_ver(void)
{
    Serial1.print("This is ");
    Serial1.print(__FILE__);
    Serial1.print(" ");
    Serial1.print("Build at ");
    Serial1.print(__DATE__);
    Serial1.print(" ");
    Serial1.print(__TIME__);
    Serial1.print("\r\n");
}

int cmd_execute(bool *echoback, char *buf)
{
    int i, x, y;
    unsigned int ux;
    int return_val = CMD_OK;
    int echo_on = false;
    char cmd[CMD_MAX_LENGTH];
    char arg1[ARG_MAX_LENGTH];
    char arg2[ARG_MAX_LENGTH];

    strcpy(cmd, "");
    strcpy(arg1, "");
    strcpy(arg2, "");
    sscanf(buf, "%s %s %s", &cmd, &arg1, &arg2);

    if     (strcmp(cmd, "help")==0){ cmd_print_help(); }
    else if(strcmp(cmd, "?"   )==0){ cmd_print_help(); }
    else if(strcmp(cmd, "ver" )==0){ cmd_print_ver();  }

    else if(strcmp(cmd, "echo")==0)
    {
        if(atoi(arg1))
        {
            *echoback = true;
        }
        else
        {
            *echoback = false;
        }
    }
    else if(strcmp(cmd, "servo")==0)
    {
        switch(atoi(arg1))
        {
            case 0:
              return_val = servo00.rotate(atoi(arg2));
              break;
            case 1:
              return_val = servo01.rotate(atoi(arg2));
              break;
            case 2:
              return_val = servo02.rotate(atoi(arg2));
              break;
            case 3:
              return_val = servo03.rotate(atoi(arg2));
              break;
            case 4:
              return_val = servo04.rotate(atoi(arg2));
              break;
            case 5:
              return_val = servo05.rotate(atoi(arg2));
              break;
            default:
              Serial1.println("### Invalid Parameter. ###");
              return_val = ERR_INVALID;
              break;
        }
    }
    else if(strcmp(cmd, "servoread")==0)
    {
        servo_read();
    }
    else
    {
        Serial1.println("### Unknown Command. ###");
        return_val = ERR_INVALID;
    }

    return return_val;
}

void cmd_rx_data(void)
{
    int i;
    int return_val = CMD_OK;
    char buf[CMD_BUF_LENGTH];
    bool echoback = true;

    i=0;
    while(1)
    {
        if(Serial1.available())
        {
            buf[i] = Serial1.read();
            if(echoback) Serial1.print(buf[i]); //echo-back

            if ( (buf[i] == 0x08) or (buf[i] == 0x7f) ) //BackSpace, Delete
            {
                buf[i] = '\0';
                if(i) i--;
            }
            else if( (buf[i] == '\r') or (buf[i] == '\n') )
            {
                Serial1.print( F("\r\n") );
                buf[i] = '\0';
                return_val = cmd_execute(&echoback, &buf[0]);
                for(i=0; i<CMD_BUF_LENGTH; i++) buf[i] = '\0';
                i=0;

                if(return_val == ERR_OK)
                {
                    Serial1.print(F("OK\r\n"));
                }
            }
            else
            {
                i++;
                if(i>=CMD_BUF_LENGTH)
                {
                    Serial1.print(F("### CMD BUFFER FULL, CLEAR. ###\r\n"));
                    for(i=0; i<CMD_BUF_LENGTH; i++) buf[i] = '\0';
                    i=0;
                }
            }
        }
    }// while
}

/***********************************************************************
  Function : setup and loop
 ***********************************************************************/
void setup()
{
    Serial1.begin(115200);
    servo_init();
}

// the loop routine runs over and over again forever
void loop()
{
    cmd_rx_data();
}

5. テストスクリプト

こちらの動画（消しゴムをつかんで移動し、元の場所に戻します）のテストスクリプトを以下に示します。

  • 一動作ごとにsleepを入れています。
  • アームを下向きに動かす操作はサーボをちょっとずつ動かすのがポイントです。
script.csv
#
# Arduino Leonardo, dsrdtr=0
open_uart,COM10,0
#
send,servo 5 140
sleep,0.25
send,servo 1 95
sleep,0.25
send,servo 3 60
sleep,0.25
send,servo 2 95
sleep,0.25
send,servo 2 100
sleep,0.25
send,servo 2 105
sleep,0.25
send,servo 2 110
sleep,0.25
send,servo 1 90
sleep,0.25
send,servo 1 87
sleep,0.25
send,servo 5 170
sleep,0.25
send,servo 1 90
sleep,0.25
send,servo 0 85
sleep,0.25
send,servo 0 80
sleep,0.25
send,servo 0 75
sleep,0.25
send,servo 0 70
sleep,0.25
send,servo 1 87
sleep,0.25
send,servo 5 140
sleep,0.25
send,servo 1 90
sleep,1
send,servo 1 87
sleep,0.25
send,servo 5 170
sleep,0.25
send,servo 1 90
sleep,0.25
send,servo 0 75
sleep,0.25
send,servo 0 80
sleep,0.25
send,servo 0 85
sleep,0.25
send,servo 0 90
sleep,0.25
send,servo 1 87
sleep,0.25
send,servo 5 140
sleep,0.25
send,servo 1 90
sleep,0.25
send,servo 2 90
sleep,0.25
send,servo 3 90
sleep,0.25
send,servo 5 170

6. おわりに

  • テストスクリプトに記述したコマンドに従ってロボットアームを動かすことができました。
  • シリアル通信でコマンド制御できるようにすればロボットアームに限らずわりと何でもテストベンチに組み込めるように思います。
  • ロボットアームを実際に動かしてみて、下向きのアームの動作はきめ細かく制御する必要があることが分かりました。
  • 今後の改善ネタを以下に挙げます。
    • アームの滑らかな動作の実現
    • テストスクリプトの保守性向上
      • テストスクリプトにサーボの回転角度をそのまま記述するとメンテナンスが大変
    • 複数のサーボの同時制御
    • 保護回路の搭載

  1. 正式な商品名は「DiyStudio 6自由度ロボットアームDIYキットアルミメカニカルアームジョーArduinor MG995サーボドライブバルク（結合する必要があります）は日本語の組み立て説明書を送ることができます」です。 ↩

  2. こちらは筆者が使用しているUBD-ARD53x68-HL-B(販売終了)の後継モデルです。 ↩

  3. ★2.54★単列長足ピンソケット [PS254S-H8.5](aitendo)、[スイッチサイエンス] Arduinoシールド用ピンソケットのセット(R3対応) など ↩

  4. 筆者は1x40のピンヘッダを必要なピン数にカットして使用しています。 ↩

4
1
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
4
1