はじめに

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

＜動画＞

構成

全体図を写真で示す。
＜全体図＞

部品リスト

組立

３つのサーボは、写真のように、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 コード

先に求めた角度は、y=0のサーボの角度であるので、x,y,zを円柱座標(r,t,z)に変換して、角度t を、t, t+120,t-120の３角度に変換することで、３つのサーボの角度を指定している。

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)

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)

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の明かりのみで撮影してみた。

まとめ

リンクロボットを、サーボモーターと竹ひごで作り、LEDを点けて高速スパイラルスキャンしてみた。

サーボは、振動することがあるので、ステッピングモーターの方が安定する。「V ステッピングモータ+ 28BYJ-48 ULN2003ドライバーボード セット」が使える。 ただし、配線が、多くなる（モーター毎ドライバーラズパイ間に4本、合計12本）のと、角度のゼロ位置設定が必要になる。