コメント無し

# はじめに 三次元座標に先端を移動するロボットには、いろいろな種類がある。X,Y,Z軸にそって移動するロボット（直交座標型）は、腕の途中にモーターが配置されるので、重くなりモーターの必要トルクが大きくなり、動きも遅くなる。 それに対し、リンクロボットは、3本のリンクで構成され、根元にモーターを設置するので、軽く速度も速いので、組立ロボットなどに使われている。 このリンクロボットを、サーボモーターと竹ひごで作り、LEDを点けて高速スパイラルスキャンしてみた。サーボモーターの角度を求める式は、rasbien に無料でインストールできる数式処理ソフトであるMATHEMATICAを用いて導出した。 ＜動画＞

# 構成  全体図を写真で示す。 ＜全体図＞  # 部品リスト |項|部品|仕様|購入先| |---|---|---|---| |1|Raspberry pi|Raspberry pi 2 + AC adaptor| スイッチサイエンス | |2|サーボドライバ|Adafruit 16チャンネル PWM/サーボ HAT for Raspberry Pi|スイッチサイエンス | |3| サーボモーター| DG-92R|秋月電子| |4| リンク1|アルミパイプ　直径4mm| ホームセンター| |5| リンク2| 竹ひご　直径　2mm|ホームセンター| |6| 接続部材|ゴムパイプ 2mm , 4 mm| ホームセンター| |7| 構造部材| タミヤ 楽しい工作シリーズ No.164 ユニバーサル金具4本セット (70164)|amazon| # 組立 ３つのサーボは、写真のように、120度毎に5cmの長さの金具に取り付ける。 ＜写真サーボ取付＞  次に、アルミパイプを、長さ10cm　3本、　長さ20 cmの竹ひごを3本準備する。 写真のように、サーボホーンにアルミパイプをつぶして穴あけし、ねじ止めする。 ＜サーボホーン取付＞  アルミパイプと竹ヒゴは、ゴムパイプで接続する。竹ひごの先端は、2mmのゴムパイプでまとめる。LEDをテープで止める。配線は、一本づつ竹ひごとアルミパイプにそわせてテープで止める。 ＜全体写真＞  # 準備 Adafruit 16チャンネル PWM/サーボ HAT for Raspberry Piでは、簡単に使えるadafruit-servokit が準備されている。以下にインストールの手順を説明する。 ==まず最新版に ＄sudo apt update ＄sodo apt upgrade== ==circuitpythonのインストール ＄sodo pip3 install adafruit-circuitpython-servokit== ==以上で、次のように簡単にサーボが動きます。 from adafruit-servokit import ServoKit kit= ServoKit(channels=16) kit.servo[0].angle=0== # サーボ角度の決め方 下図に、3本のリンクのうち1本のリンクを抜き出して、y=0の面を示している。　ここで、先端は、3次元座標を移動する(x,y,z)。リンク２は、サーボモーターの軸に取り付けられて、この面上を円運動する（半径b ）。　リンク１の半径a の球と、リンク2の半径bの円の交点(wx,wy,wz)を求める。  ここで、MATHEMETICAの Solve[{式1,式2},{求める変数x,y,z}]関数を使う。求めた式に、数値を代入し、(式/. {a -> 0.2 , b-> 0.1})、X座標ー5cm(soko) とZ座標(takasa)からリンク１の角度angle1を求める。次に、c言語の記述に変換(CFrom[])し、さらに、Python用に変換する(Power-> pow , ArcTan->atan2, Sqrt -> sqrt)。 最後に、テキストfileに、export してphthon code に張り付ける。 ==MATHEMATICAでは、各行毎にSHIFT＋改行で処理されます。==  ++MATHEMATICAインストール方法 （2GB必要） $sudo apt-get update &&sudo apt-get install wolfram-engine++ # Python コード :::plantuml:Python_modules @startuml (*) --> "軌跡の計算 main" --> [XYZ座標のセット] "setxyh(x1,y1,h1)" --> [角度を計算] "angle(x,y,h)" -->[各サーボの角度セット] "setangle.set()" if "END?" then -->[YES](*) else -->[まだ]"軌跡の計算 main" @enduml ::: 先に求めた角度は、y=0のサーボの角度であるので、x,y,zを円柱座標(r,t,z)に変換して、角度t を、t, t+120,t-120の３角度に変換することで、３つのサーボの角度を指定している。 ```python:main.py #main.py import math from math import * import setangle from angle import angle import time import led def setxyh(x1,y1,h1): x=x1/10 y=y1/10 h=h1/10 r= sqrt( x**2 + y**2) t= atan2(y,x)*180/pi setangle.set(angle(r/100.,t,h/100.),angle(r/100.,t-120,h/100.),angle(r/100.,t+120,h/100.)) # スパイラル軌跡の計算 s=0 led.on(0xFFFF) while s < 30 : x1=80 * math.cos(s) y1=80 * math.sin(s) h1= 2*s + 125 setxyh(x1,y1,h1) s=s+0.01 setangle.set(45,45,45) time.sleep(10) # LED OFF led.on(0x0000) ``` ```python:angle.py #angle.py # a= 0.1 meter # b= 0.2 meter # x1s= 0.05 meter # This module calculate arm angle from r theta z from math import * def angle(r,t,z): do = t * pi / 180 wwz = z wwx = r * cos(do) wwy = r * sin(do) kakudo= 180*atan2(-0.05 + (pow(pow(0.05 - wwx,2) + pow(wwz,2),-1)*((0.05 - wwx)*(0.0325 - pow(wwx,2) - pow(wwy,2)) + (0.05 + wwx)*pow(wwz,2) + pow(-(pow(wwz,2)*(0.0017 - 0.02* (pow(0.05 - wwx,2) - pow(wwy,2) + pow(wwz,2)) - 0.08*(0.01 + pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2)) + pow(pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2),2))),0.5)))/2.,(pow(wwz,-1)*pow(pow(0.05 - wwx,2) + pow(wwz,2),-1)*(-0.04*pow(wwz,2) + pow(wwz,2)*(0.01 + pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2)) + 0.05*pow(-(pow(wwz,2)* (0.0017 - 0.02*(pow(0.05 - wwx,2) - pow(wwy,2) + pow(wwz,2)) - 0.08* (0.01 + pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2)) + pow(pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2),2))),0.5) - wwx*pow(-(pow(wwz,2)*(0.0017 - 0.02*(pow(0.05 - wwx,2) - pow(wwy,2) + pow(wwz,2)) - 0.08*(0.01 + pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2)) + pow(pow(0.05 - wwx,2) + pow(wwy,2) + pow(wwz,2),2))),0.5)))/2.)*pow(pi,-1) return(kakudo) ``` ```python:setangle.py # setangle.py # This module move arms to specified angles. # When arms are vertical # their angle of servo are 180 degree. import time from adafruit_servokit import ServoKit kit = ServoKit (channels =16) def set(a,a2,a4): kit.servo[0].angle=180-a kit.servo[1].angle=180-a2 kit.servo[2].angle=180-a4 # ``` # 動画 照明を消してLEDの明かりのみで撮影してみた。 @[youtube](https://youtu.be/huHyAR2roZo) # まとめ リンクロボットを、サーボモーターと竹ひごで作り、LEDを点けて高速スパイラルスキャンしてみた。 ++サーボは、振動することがあるので、ステッピングモーターの方が安定する。「V ステッピングモータ+ 28BYJ-48 ULN2003ドライバーボード セット」が使える。 ただし、配線が、多くなる（モーター毎ドライバーラズパイ間に4本、合計12本）のと、角度のゼロ位置設定が必要になる。++