def __init__(self):
for _ in range(10):
try:
self._arduino = serial.Serial('/dev/ttyACM0', 9600)
break
except FileNotFoundError:
self._arduino = None
for _ in range(10):
try:
self._rs485 = rsc_u485.RSC_U485('/dev/ttyUSB0', 460800)
break
except FileNotFoundError:
self._rs485 = None
self.motor = [0., 0.]
self.servo = [9, -6, 9, 15, -90]
self.gate = [0, 0, 0]
self.sensor = [0, 0, 0, 0]
self.dist = 0.
self.c = 0
if self._rs485:
for i in range(8):
self._rs485.torque(i + 1, 1)
import time
import rsc_u485
import sys
if __name__ == '__main__':
servo = rsc_u485.RSC_U485('/dev/ttyUSB0', 460800)
args = sys.argv
for id in range(1, 9):
servo.torque(id, 1 if int(args[1]) else 0)
#time.sleep(1)
print(
str(id) + " servo's torque " + "on!!" if int(args[1]) else "off!!")
# coding: utf-8
"""
FutabaのRS405,RS406CB制御用ライブラリ
制御例
"""
import time
import rsc_u485
import sys
argvs = sys.argv
argc = len(argvs)
if __name__ == '__main__':
# メインルーチン
servo = rsc_u485.RSC_U485('/dev/ttyUSB0', 115200)
if (argc < 2): # 引数が足りない場合は、その旨を表示
print 'Usage: # python %s filename' % argvs[0]
quit() # プログラムの終了
i = int(argvs[1])
Range = int(argvs[2])
print "サーボ番号%d,角度を%d度動かそうとしている" % (i, Range)
servo.torque(i, 1)
print '最高速度で%d度の位置へ回転' % Range
print 'ID%dのサーボのトルクをオン' % i
def handle_arm_move(req):
# print "This service catched data num=%d, angle=%d motion=%s" %(req.num, req.angle, req.motion)
servo = rsc_u485.RSC_U485('/dev/ttyUSB0', 115200)
if req.motion == "catch":
# simultaneously two servo action
# assumed only #5,3 servo action
if int(req.num / 10) != 0:
print "dual servo mode start"
num1 = int(req.num / 10) #servo num1
num2 = int(req.num % 10) #servo num2
servo.torque(num1, 1) #torqe on
servo.torque(num2, 1)
print "move angle %d" % req.angle
time.sleep(2) # しばし待つ
servo.move(num1, -req.angle, 300)
servo.move(num2, +req.angle, 300)
time.sleep(2) # しばし待つ
print 'current angle1:%d' % servo.getAngle(num1)
print 'current angle2:%d' % servo.getAngle(num2)
time.sleep(2) # しばし待つ
return ServoResponse(1)
#single servo action
else:
print 'ID%d Torqu on' % req.num
servo.torque(int(req.num), 1)
print "move angle %d" % req.angle
print 'current angle:%d' % servo.getAngle(req.num)
time.sleep(2)
servo.move(int(req.num), req.angle, 300)
time.sleep(2) # しばし待つ
print 'current angle:%d' % servo.getAngle(req.num)
return ServoResponse(1)
elif req.motion == "free":
for i in range(1, 10):
print "reset servo No. %d" % i
time.sleep(1) # しばし待つ
servo.torque(i, 0)
print "servo free mode complete"
return ServoResponse(1)
elif req.motion == "open":
print 'ID%d Torqu on' % req.num
servo.torque(int(req.num), 1)
print "move angle %d" % req.angle
print 'current angle:%d' % servo.getAngle(req.num)
time.sleep(2)
servo.move(int(req.num), req.angle, 300)
time.sleep(2) # しばし待つ
print 'current angle:%d' % servo.getAngle(req.num)
return ServoResponse(1)