楽しいことをしたい: 【Processing】平面二軸アームを動かす

こんにちは。前回の記事では、平面二軸アームの設計ということで、フリーソフトである3DCADのDesignSpark Mechanicalを使ってサーボモータ同士を繋ぐアーム部分の設計をしました。今回の記事では、その3DCADで作製した設計の通りにアクリル板を切り出して、アクリル曲げヒータで曲げ加工をし、サーボと合わせてロボットアームを作成し、ProcessingとArduinoを使って動かします。

　今回購入したのは厚さ3 mmの透明アクリル板910 mm × 600 mmです。近所のホームセンターで4800円でした。この板から今回使用するアームで使用する分（250 mm × 50 mm）を切り出します。切り出すためには、プラスチックカッターを使用します。プラスチックカッターによる加工は、安価であるというメリットがあります。しかし、切る部分に溝を彫り、ある程度のところまで彫れたら力を加えて割る必要があるため細かい加工が難しく、時間がかかるという欠点があります。

▲アクリルカッター

初めての加工ということもあり、細かい部分が割れてしまうというアクシデントがありましたが、組み立てには支障なかったのでこのまま使用します。（卓上糸鋸盤がほしい…。）

▲赤丸部分が欠けてしまっている

　切り出すことができたら、アクリル曲げヒータを使って設計どおりに曲げます。アクリル曲げヒータはアクリルに170℃付近まで熱を加える事で熱可塑性により加工を可能としています。アクリル曲げヒータは市販のものを購入すると結構なお値段(\7500~)になりますが、自分で材料を集めれば安く作ることもできます。僕はファブクラウドさんが販売しているアクリルヒータキットを購入しました。（送料込み3000円程度）

　曲げ加工まで済んだらサーボモータをくっつけて（横着して両面テープでとめた）ロボットアームの形にします。

▲横着ぼろっと

最後に、Processingで逆運動学により算出された各関節の角度をArduinoにシリアル通信で送信するプログラムを書いて動かします。

_kiitaniさん(@monitorgazer)が投稿した動画 - 12月 12, 2014 at 9:27午後 PST

　加工が適当すぎて、ボールペンの接触が不安定ですね。高専時代にもっと機械加工を実践しておくべきだったな〜と思います。

　うーん。アーティスティック…。

以下プログラムです。

Processing(データ送信側)

import controlP5.*;
import processing.serial.*;

ControlP5 cp5;
PGraphics pg;
Slider2D ui;
Serial myPort;

int[] cmd = {255,0,3,0,90,90};
boolean init = false;

float x1,y1,x2,y2,xe,ye;
float oldX2,oldY2,oldXe,oldYe;
float angle1;
float angle2;
float angle1_;
float angle2_;
float the1;
float the2;
final float l1 = 100;
final float l2 = 100;
final int r = 20;
int dir = 1;

void setup(){
  size(640,640);
  stroke(255,255,255);
  fill(255,255,255);
  //set to origin.
  x1 = 300;
  y1 = 350;
  pg = createGraphics(width,height,JAVA2D);
  cp5 = new ControlP5(this);
  
  ui = cp5.addSlider2D("Position")
    .setSize(200,200)
    .setArrayValue(new float[] {79, 1})
    .setMaxX(150)
    .setMaxY(132)
    .setMinX(-90)
    .setMinY(-50)
    .setPosition(30,30)
    ;
  String portName = Serial.list()[5];
  println(Serial.list());
  myPort = new Serial(this,portName,9600);
  init = true;
}

void draw()
{ 
  //myPort.write("123,456\0");
  background(0);
  angle1_ = acos((pow(l1,2)-pow(l2,2)+(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2)))/((2*l1)*sqrt(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2))));
  angle1 = atan2(ui.arrayValue()[1],ui.arrayValue()[0])-angle1_;
  angle2_ = acos((pow(l2,2)-pow(l1,2)+(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2)))/((2*l2)*sqrt(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2))));
  angle2 = angle1_ + angle2_;
  //print(angle1);print(":");println(angle2);
  float degangle1 = angle1 * 180/PI;
  float degangle2 = angle2 * 180/PI;
  angle1 = degangle1 * PI/180;
  angle2 = degangle2 * PI/180;
  x2 = x1+l1*cos(angle1);
  y2 = y1+l1*sin(angle1);
  xe = x2+l2*cos(angle1+angle2);
  ye = y2+l2*sin(angle1+angle2);
  if(degangle1 <= -90){
    xe = oldXe;
    ye = oldYe;
    x2 = oldX2;
    y2 = oldY2;
    degangle1 = -90;
  }
  if(degangle1 >= 90){
    xe = oldXe;
    ye = oldYe;
    x2 = oldX2;
    y2 = oldY2;
    degangle1 = 90;
  }
  if(angle2<=0){
    xe = oldXe;
    ye = oldYe;
    x2 = oldX2;
    y2 = oldY2;
    degangle2 = 0;
  }
  if(angle2>=180){
    xe = oldXe;
    ye = oldYe;
    x2 = oldX2;
    y2 = oldY2;
    degangle2 = 180;
  }
  ellipse(x1,y1,r,r);
  ellipse(x2,y2,r,r);
  fill(255,0,0);
  ellipse(xe,ye,r,r);
  fill(255);
  line(x1,y1,x2,y2);
  line(x2,y2,xe,ye);
  pg.beginDraw();
    pg.noStroke();
    pg.fill(0,100,100);
    //pg.ellipse(xe,ye,r,r);
  pg.endDraw();
  image(pg.get(0,0,width,height),0,0);
  
  //print(degangle1);print(":");println(degangle2);
  cmd[4] = (int)(90-degangle1);
  cmd[5] = (int)(180-degangle2);
  oldXe = xe;
  oldYe = ye;
  oldX2 = x2;
  oldY2 = y2;
  servoCmd();
}

void servoCmd() {
  if(!init) return;
  for(int i = 0; i < 6; i++) {
    myPort.write((byte)cmd[i]);
  }
}

void mouseDragged(){
  int x = (int)(mouseX-x1);
  int y = (int)(mouseY-y1);
  
  if( x <= -90) x = -90;
  if( x >= 150) x = 150;
  if( y <= -50) y = -50;
  if( y >= 132) y = 130;
  ui.arrayValue()[0] = x;
  ui.arrayValue()[1] = y; 
  print(ui.arrayValue()[0]);print(":");println(ui.arrayValue()[1]);
}

Arduino(データ受信側)

 #include <Servo.h> 
#include <string.h>
#include <stdlib.h>

Servo myservo1,myservo2,myservo3;
float deg1,deg2;
unsigned char incomingByte = 0;
unsigned char data[20];
int axis1 = 9;
int axis2 = 10;
int axis3 = 11;

int count = 0;
int paramLength = 0;
int paramCount = 0;
 
int pos = 0;    // variable to store the servo position 
 
void setup() 
{
  Serial.begin(9600); 
  myservo1.attach(axis1,800,2200);
  myservo2.attach(axis2,800,2200);
} 
 
 
void loop() 
{ 
  if(Serial.available() > 0){
      incomingByte = Serial.read();
      switch(count){
          case 0:
              paramLength = 0;
              paramCount = 0;
              if(incomingByte == 0xff){ //Header
                  data[count++] = incomingByte;
              }
              break;
          case 1:
              if(incomingByte == 0){ //ID
                  data[count++] = incomingByte;
              }
              break;
          case 2:    //LENGTH
              data[count++] = incomingByte;
              paramLength = incomingByte;
              break;
          default:
              if(paramLength > 0){
                      data[count++] = incomingByte;
                      paramCount++;
                      if(paramCount >= paramLength){
                          ServoCmdExec();
                          count = 0;
                      }
              }else{
                  count = 0;
              }
              break;
      }
  } 
}

void ServoCmdExec(){
    if((data[4] >= 0) && (data[4] <= 180) ) myservo1.write(data[4]);
    if((data[5] >= 0) && (data[5] <= 180) ) myservo2.write(data[5]);
}

複数のサーボをProcessingとArduinoのシリアル通信で制御するためには少し工夫が必要になります。

参考にしたサイト様

RCサーボモータをArduinoとProcessingでGUI制御（その１）　【Arduino編】

楽しいことをしたい

2014年12月13日土曜日

【Processing】平面二軸アームを動かす

0 件のコメント:

コメントを投稿

ジャンルアーカイブ