ロボットアームとArduino (GR-ROSE)で機構学のお勉強

tmanda.hatenablog.com で紹介したロボットアームを制御するための機構学のお勉強です。 ↑これの話です。

さっそくはじめていきましょう。

まずはロボットアームの関節角度を定義します。今回はロボットアームが垂直に立った状態を0度とします。
（サーボモータの可動範囲は0度±135度）

ロボットアームの関節の位置(サーボモータの回転軸の位置)を計算します。

今回はロボットアームの先端が水平に同じ高さになるように制御します。  

「ロボットアームの先端が水平に同じ高さ」を数式で表すと次のようになります。

①②の式を代入すると、 ここで黄色マークした式をこの後で使います。根本のモータを好きなように制御したときの他の2つのモータ角度を計算します。

あとはプログラムにモータ角度をセットするコードを書くということになります。

※注意点
1. モータの可動範囲は実物のアームが干渉しない範囲になります。アーム同士がぶつからないようにしましょう。
2. 今回は高さを一定に保つ制御をしていますが、リンクが届かない範囲があります。以下の絵を参照。

今回使用したArduino互換ボード「GR-ROSE」には近藤科学のサーボモータを接続できるコネクタとライブラリが用意されているので、すぐに試すことができました。

www.renesas.com

ソースコードは以下です。

#include <ICS.h>

IcsController ICS(Serial1);
IcsServo servo0;
IcsServo servo1;
IcsServo servo2;
IcsServo servo3;

int theta0_pulse = 0;  //3500-11500
int theta1_pulse = 0;
int theta2_pulse = 0;
int theta3_pulse = 0;

bool motion_enable = false;
bool dir0 = false;
bool dir1 = false;

#define TH0_ZERO 7500
#define TH0_LIMIT_MIN 3500
#define TH0_LIMIT_MAX 11500

#define TH1_ZERO 7500
#define TH1_LIMIT_MIN 4500
#define TH1_LIMIT_MAX 10500

#define TH2_ZERO 7500
#define TH2_LIMIT_MIN 4500
#define TH2_LIMIT_MAX 10500

#define TH3_ZERO 7500
#define TH3_LIMIT_MIN 4500
#define TH3_LIMIT_MAX 10500


void setup() {

  Serial.begin(115200);
  pinMode(PIN_LED1, OUTPUT);

  ICS.begin();
  servo0.attach(ICS, 0x01);
  servo1.attach(ICS, 0x02);
  servo2.attach(ICS, 0x03);
  servo3.attach(ICS, 0x04);
  
  theta0_pulse = TH0_ZERO;
  theta1_pulse = servo1.getPosition();
  theta2_pulse = servo2.getPosition();
  theta3_pulse = servo3.getPosition();

  setPosition4axis(theta0_pulse, theta1_pulse, theta2_pulse, theta3_pulse);
  delay(1000);
  digitalWrite(PIN_LED1, HIGH);
}

void loop() {
  if(Serial.available()){
    switch(Serial.read()){
      case 's':
        motion_enable = !motion_enable;
        break;
      case '0':
        motion_enable = false;
        theta0_pulse = TH0_ZERO;
        theta1_pulse = TH1_ZERO;
        theta2_pulse = TH2_ZERO;
        theta3_pulse = TH3_ZERO;
        break;
      default:
        break;
    }
  }
  
  if(motion_enable){
    int tmp;
    int dth = 50;

    if(theta0_pulse > (TH0_LIMIT_MAX - 400)){
      dth = 50 - (TH0_LIMIT_MAX - 400 - theta0_pulse)/8;
      if(dth < 5) dth = 5;
    } else if(theta0_pulse < (TH0_LIMIT_MIN + 400)) {
      dth = 50 - (TH0_LIMIT_MIN + 400 - theta0_pulse)/8;
      if(dth < 5) dth = 5;
    } else {
      dth = 50;
    }
    if(dir0){
      theta0_pulse += dth;
      if(theta0_pulse > TH0_LIMIT_MAX){
        theta0_pulse = TH0_LIMIT_MAX;
        dir0 = !dir0;
      }
    } else {
      theta0_pulse -= dth;
      if(theta0_pulse < TH0_LIMIT_MIN){
        theta0_pulse = TH0_LIMIT_MIN;
        dir0 = !dir0;
      }
    }//*/
    if(theta1_pulse > 8620){
      dth = 100 - (theta1_pulse-8620)/4;
      if(dth < 5) dth = 5;
    } else if(theta1_pulse < 6000) {
      dth = 100 - (6000 - theta1_pulse)/4;
      if(dth < 5) dth = 5;
    } else {
      dth = 100;
    }
    if(dir1){
      theta1_pulse += dth;
      if(theta1_pulse > 9000){
        theta1_pulse = 9000;
        dir1 = !dir1;
      }
    } else {
      theta1_pulse -= dth;
      if(theta1_pulse < 5620){
        theta1_pulse = 5620;
        dir1 = !dir1;
      }
    }//*/
    tmp = calc_theta2_pulse(theta1_pulse);
    if(tmp != 0) theta2_pulse = tmp;
    theta3_pulse = TH3_ZERO - ((theta1_pulse - TH1_ZERO) + (theta2_pulse - TH2_ZERO));
  }

  setPosition4axis(theta0_pulse, theta1_pulse, theta2_pulse, theta3_pulse);
  delay(25);
}

void setPosition4axis(int pos0, int pos1, int pos2, int pos3){
  if(pos0 < TH0_LIMIT_MIN){
    pos0 = TH0_LIMIT_MIN;
  } else if(pos0 > TH0_LIMIT_MAX){
    pos0 = TH0_LIMIT_MAX;
  }
  if(pos1 < TH1_LIMIT_MIN){
    pos1 = TH1_LIMIT_MIN;
  } else if(pos1 > TH1_LIMIT_MAX){
    pos1 = TH1_LIMIT_MAX;
  }
  if(pos2 < TH2_LIMIT_MIN){
    pos2 = TH2_LIMIT_MIN;
  } else if(pos2 > TH2_LIMIT_MAX){
    pos2 = TH2_LIMIT_MAX;
  }
  if(pos3 < TH3_LIMIT_MIN){
    pos3 = TH3_LIMIT_MIN;
  } else if(pos3 > TH3_LIMIT_MAX){
    pos3 = TH3_LIMIT_MAX;
  }
  Serial.print(servo0.setPosition(pos0)); Serial.write('\t');
  Serial.print(servo1.setPosition(pos1)); Serial.write('\t');
  Serial.print(servo2.setPosition(pos2)); Serial.write('\t');
  Serial.print(servo3.setPosition(pos3)); Serial.println();//*/
}


int calc_theta2_pulse(int theta1){
  const float l1 = 68.0f;
  const float l2 = 60.0f;
  const float l2_ref = 89.8f;
  const float pulse_to_rad = 135.0f*M_PI/(4000.0f*180.0f);
  const float rad_to_pulse = (4000.0f*180.0f)/(135.0f*M_PI);
  
  const float theta1_rad = (float)(theta1 - TH1_ZERO) * pulse_to_rad;
  const float p2_y = l1 * cos(theta1_rad);
  
  float theta2_rad;
  float cos_theta2 = (l2_ref - p2_y)/l2;
  
  if(cos_theta2 > 1.0f){
    return 0; //Error
  }
  theta2_rad = acos(cos_theta2) - theta1_rad;
  
  return ((theta2_rad * rad_to_pulse) + TH2_ZERO);
}