Go to Qiita Advent Calendar 2024 Top
LoginSignup
2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@pbjpkas

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

Posted at

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

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

servo_controller.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ビデオキャプチャデバイスの組込み
  9. Lチカで始めるテスト自動化(9)6DoFロボットアームの組込み
  10. Lチカで始めるテスト自動化(10)6DoFロボットアームの制御スクリプトの保守性向上

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

  • 可変抵抗器VR1～VR6は50kΩで、摺動子の端子をArduino LeonardoのAnalog In A0～A5に接続します。
    • A0～A5が各々Servo ID 0～5に対応します。
    • 筆者は東京コスモス電機のGF063P1B503をマルツで購入しました。
  • スイッチSW1、SW2をDigital Pinの#10、#11に接続します。

servo_controller_circuit.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()を実行する。
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. おわりに
ロボットアームの位置決めをコントローラのつまみを回してできるようになり、何度もコマンドを発行して合わせるよりもやりやすくなりました。

2
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
2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?