編集履歴一覧に戻る
tomfiveのアイコン画像

tomfive が 2021年02月28日12時45分30秒 に編集

コメント無し

本文の変更

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

-

@[スパイラル駆動動画](https://youtu.be/huHyAR2roZo)

+

@[youtube](https://youtu.be/huHyAR2roZo)

# 構成 ![キャプションを入力できます](https://camo.elchika.com/ca9bdfac816bf84802587ffd3e20dce752350710/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f66343835306238322d353236632d343634392d393266362d366565346436626664633066/) 全体図を写真で示す。 <全体図> ![全体図](https://camo.elchika.com/36384cd7226eaaabff81eb23514f86dcc2639cc6/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f62643936363535642d623936382d346439632d383230612d303262663861333766636466/) # 部品リスト |項|部品|仕様|購入先| |---|---|---|---| |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| # 組立 3つのサーボは、写真のように、120度毎に5cmの長さの金具に取り付ける。 <写真サーボ取付> ![サーボ取付図](https://camo.elchika.com/22e233dd3aada936075b7d6f54cc192f437f65cd/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f63653232376465322d366634302d343564662d616361612d386264623163313238626464/) 次に、アルミパイプを、長さ10cm 3本、 長さ20 cmの竹ひごを3本準備する。 写真のように、サーボホーンにアルミパイプをつぶして穴あけし、ねじ止めする。 <サーボホーン取付> ![サーボホーン取付部](https://camo.elchika.com/f5aec3ac17ae40dc02df75c81c6564f4b5a34868/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f65373961343565372d626363622d346266342d613466662d346531313937333834386630/) アルミパイプと竹ヒゴは、ゴムパイプで接続する。竹ひごの先端は、2mmのゴムパイプでまとめる。LEDをテープで止める。配線は、一本づつ竹ひごとアルミパイプにそわせてテープで止める。 <全体写真> ![エルボー部分](https://camo.elchika.com/a00fc4058149085c02a5162d0e59e006b105eaa2/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f37396638663132362d363738302d343131372d393833312d306531303262633932333661/) # 準備 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)。リンク2は、サーボモーターの軸に取り付けられて、この面上を円運動する(半径b )。 リンク1の半径a の球と、リンク2の半径bの円の交点(wx,wy,wz)を求める。 ![y=0 面](https://camo.elchika.com/8b39c5d62840ded03950e4af3836376e6d581241/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f37616562373234382d646533352d343334312d626436392d313961373739633165636637/) ここで、MATHEMETICAの Solve[{式1,式2},{求める変数x,y,z}]関数を使う。求めた式に、数値を代入し、(式/. {a -> 0.2 , b-> 0.1})、X座標ー5cm(soko) とZ座標(takasa)からリンク1の角度angle1を求める。次に、c言語の記述に変換(CFrom[])し、さらに、Python用に変換する(Power-> pow , ArcTan->atan2, Sqrt -> sqrt)。 最後に、テキストfileに、export してphthon code に張り付ける。 ==MATHEMATICAでは、各行毎にSHIFT+改行で処理されます。== ![MATHEMATICA CODE](https://camo.elchika.com/2aca35386a27d205894d21812e7acd95abc572d2/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f34613939653033342d393461392d343761362d626662642d3961393638303236306237382f37313261383036642d633461662d346339662d383864302d323933393866623431653862/) ++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の3角度に変換することで、3つのサーボの角度を指定している。 ```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の明かりのみで撮影してみた。

-

@[スパイラル駆動動画](https://youtu.be/huHyAR2roZo)

+

@[youtube](https://youtu.be/huHyAR2roZo)

# まとめ リンクロボットを、サーボモーターと竹ひごで作り、LEDを点けて高速スパイラルスキャンしてみた。 ++サーボは、振動することがあるので、ステッピングモーターの方が安定する。「V ステッピングモータ+ 28BYJ-48 ULN2003ドライバーボード セット」が使える。 ただし、配線が、多くなる(モーター毎ドライバーラズパイ間に4本、合計12本)のと、角度のゼロ位置設定が必要になる。++