Lチカで始めるテスト自動化(11)ロボットアームのコントローラ製作

##1. はじめに
Lチカで始めるテスト自動化シリーズ第十一弾です。

テスト対象の操作をしやすいよう、ロボットアームの位置決めをコントローラのつまみを回してできるようにします。

![servo_controller.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/221758/b541870e-e8be-3258-cfcd-238e6d070ca2.jpeg)

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

1. [Lチカで始めるテスト自動化](https://qiita.com/pbjpkas/items/fd4bd2689deda2d793aa)
2. [Lチカで始めるテスト自動化(2)テストスクリプトの保守性向上](https://qiita.com/pbjpkas/items/337bce392281ee5f0526)
3. [Lチカで始めるテスト自動化(3)オシロスコープの組込み](https://qiita.com/pbjpkas/items/b80b7e199a5aed4688a4)
4. [Lチカで始めるテスト自動化(4)テストスクリプトの保守性向上(2)](https://qiita.com/pbjpkas/items/a02c4128baf5a3f2a46c)
5. [Lチカで始めるテスト自動化(5)WebカメラおよびOCRの組込み](https://qiita.com/pbjpkas/items/9af9e6441e1e20c5909e)
6. [Lチカで始めるテスト自動化(6)AI(機械学習)を用いたPass/Fail判定](https://qiita.com/pbjpkas/items/405d55f809da1221b867)
7. [Lチカで始めるテスト自動化(7)タイムスタンプの保存](https://qiita.com/pbjpkas/items/8e3c4330f4a81d30bd53)
8. [Lチカで始めるテスト自動化(8)HDMIビデオキャプチャデバイスの組込み](https://qiita.com/pbjpkas/items/58f84c347e44fb9d5635)
9. [Lチカで始めるテスト自動化(9)6DoFロボットアームの組込み](https://qiita.com/pbjpkas/items/7a4cb4c59886b1fd2cf9)
10. [Lチカで始めるテスト自動化(10)6DoFロボットアームの制御スクリプトの保守性向上](https://qiita.com/pbjpkas/items/c011d7ad6f98bc1458d5)

##2. 回路
以下に回路図を示します。

- 可変抵抗器VR1～VR6は50kΩで、摺動子の端子をArduino LeonardoのAnalog In A0～A5に接続します。
  - A0～A5が各々Servo ID 0～5に対応します。
  - 筆者は東京コスモス電機の[GF063P1B503](https://www.google.com/search?q=GF063P1B503)をマルツで[購入](https://www.marutsu.co.jp/pc/i/59791/)しました。
- スイッチSW1、SW2をDigital Pinの#10、#11に接続します。

![servo_controller_circuit.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/221758/78cf14eb-9495-ef20-fc39-d78081e43170.png)

##3. Arduinoファームウェアの改修
ユースケースを以下に示します。

1. スイッチSW1を押しながら可変抵抗器VR1～VR6を操作して位置決めを行う。
2. スイッチSW2を押してサーボの回転角度情報をUARTに出力し、(キャプチャリプレイの)キャプチャを行う。

プログラムの改修箇所を以下に示します。

- Digital Pin #10、#11のピンモードをINPUT_PULLUPに設定する。
- loop()内でスイッチの押下判定を行う。
- スイッチSW1を押されたらANALOG IN A0～A5の電圧に応じてサーボを回転させる。
- スイッチSW2を押されたらservo_read()を実行する。

```c++:Arduino6DoFArmV2.ino
/***********************************************************************
 * Arduino6DoFArm
 * 2020-07-24 by ka's
 * 2020-08-30 update
 *
 * This software includes the work that is distributed
 * in the Apache License 2.0
 ***********************************************************************/
#include <Servo.h>

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

//Digital Pin Assign
#define BTN_REFLECT_ADC_VAL  10 // このボタンを押すとADCの値がサーボの角度に反映される
#define BTN_PRINT_SERVO_READ 11 // このボタンを押すとservo_read()が実行される

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 angle);
        rotate_reflect_adc_val();
    private:
        int     servo_id;
        int     servo_pin;
        long    last_angle;
        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;
    last_angle     = initial;
    rotate_initial = initial;
    rotate_lower   = lower;
    rotate_upper   = upper;
}

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

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

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

servo::rotate_reflect_adc_val()
{
    int  value = analogRead(servo_id);
    long angle = map(value, 0, 1023, rotate_lower, rotate_upper);
    if( last_angle != angle )
    {
        Serial1.print(servo_id);
        Serial1.print(" ");
        Serial1.println(angle);
        last_angle = angle;
        rotate(angle);
    }
}

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

void servo_rotate_reflect_adc_val()
{
    servo00.rotate_reflect_adc_val();
    servo01.rotate_reflect_adc_val();
    servo02.rotate_reflect_adc_val();
    servo03.rotate_reflect_adc_val();
    servo04.rotate_reflect_adc_val();
    servo05.rotate_reflect_adc_val();
}

/***********************************************************************
  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)
{
    static int i = 0;
    static char buf[CMD_BUF_LENGTH];
    static bool echoback = true;
    int return_val = CMD_OK;

    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') )
        {
            if(echoback) 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;
            }
        }
    }
}

/***********************************************************************
  Function : setup and loop
 ***********************************************************************/
void setup()
{
    Serial1.begin(115200);
    pinMode(BTN_REFLECT_ADC_VAL, INPUT_PULLUP);
    pinMode(BTN_PRINT_SERVO_READ,INPUT_PULLUP);
    servo_init();
}

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

    if(digitalRead(BTN_REFLECT_ADC_VAL)==LOW)
    {
        servo_rotate_reflect_adc_val();
    }
    if(digitalRead(BTN_PRINT_SERVO_READ)==LOW)
    {
        servo_read();
        while(digitalRead(BTN_PRINT_SERVO_READ)==LOW){};
    }

}
```

##4. おわりに
ロボットアームの位置決めをコントローラのつまみを回してできるようになり、何度もコマンドを発行して合わせるよりもやりやすくなりました。