def initMotors(self):
logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.DEBUG)
self.chain = DxlChain(dxlPath, rate=1000000, timeout=0.5)
self.lastMotorsPos = {}
chainIds = self.chain.get_motor_list() # Get chain ids
# Check if all needed motors are in the chain
for motor in allMotorsIds:
if not motor in chainIds:
raise ValueError("{}: motor not found in chain".format(motor))
self.lastMotorsPos[motor] = 0.
self.initMotorsSpeedAndTorque()
self.initMotorsBounds()
def current_datetime(request):
now = datetime.datetime.now()
chain=DxlChain("/dev/ttyACM0", rate=57600, timeout=0.1)
motors = chain.get_motor_list()
try:
sync_write_pos_speed([1,2][500,500],[50,300])
except error e:
logger.error(e)
derp = "<html><body>motors now contains %s</body></html>" %motors # %strung
return HttpResponse(derp)
def open(self):
self.pi = pigpio.pi()
self.dyn_chain = DxlChain(self.port, rate=1000000)
self.dyn_chain.open()
self.motors = self.dyn_chain.get_motor_list()
assert len(
self.motors
) == 6, 'Some arm motors are missing. Expected 6 instead got %d' % len(
self.motors)
self.tyro_manager = TyroManager(self.dyn_chain)
self.tyro_manager.start()
self.opened = True
def start(self):
GPIO.setmode(GPIO.BOARD)
GPIO.setup(DYN_CTRL, GPIO.OUT)
GPIO.output(DYN_CTRL, GPIO.HIGH)
time.sleep(1.0)
GPIO.output(DYN_CTRL, GPIO.LOW)
dyn_chain = DxlChain(USB_PORT, rate=1000000)
print 'Started'
motors = dyn_chain.get_motor_list() # Discover all motors on the chain and return their IDs
act_pos = dyn_chain.get_position()
self.arm = ArmCtrl(dyn_chain)
self.model_current_pos = self.arm.get_arm_pose()
self.arm.init_pos()
self.send_arm_state()
self.xyz_model[0] = self.arm.get_arm_pose()[0]
self.xyz_model[1] = self.arm.get_arm_pose()[1]
self.xyz_model[2] = self.arm.get_arm_pose()[2]
super(ArmModule, self).start()
def __init__(self):
rospy.init_node('full_hw_controller')
self.excitation = False
self.lower_limit = False
self.upper_limit = False
self.calibrated = False
self.chain = DxlChain(rospy.get_param('~port'), rate=rospy.get_param('~rate'))
self.motors = self.chain.get_motor_list()
self.pub_reset_enc = rospy.Publisher('/scara_setup/linear_encoder/reset', Empty, queue_size=100)
rospy.Subscriber("/full_hw_controller/linear/command", Float64, self.callback_linear)
rospy.Subscriber("/full_hw_controller/linear/override", Float64, self.callback_linear_override)
self.shoulder_pub = rospy.Publisher("/full_hw_controller/shoulder/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/shoulder/command", Float64, self.callback_shoulder)
self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/elbow/command", Float64, self.callback_elbow)
self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/wrist/command", Float64, self.callback_wrist)
self.fingerjoint_pub = rospy.Publisher("/full_hw_controller/fingerjoint/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64, self.callback_fingerjoint)
rospy.Subscriber("/full_hw_controller/set_excitation", Bool, self.callback_excitation)
rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool, self.callback_lower_limit)
rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool, self.callback_upper_limit)
rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool, self.callback_calibrated)
rospy.Subscriber("/full_hw_controller/cog_linear", Empty, self.callback_cog_linear)
self.rate = rospy.Rate(20) # 10hz
self.set_excitation(False)
# coding=utf-8
from __future__ import print_function
import RPi.GPIO as GPIO
import threading
import sys
sys.path.insert(0, '/home/pi/dynamixel_hr')
from dxl.dxlchain import DxlChain
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
print(chain.get_motor_list())
class Pair(object):
"""
A quadrature encoder paired with one Dynamixel
"""
counter = 0
currentA = 1
currentB = 1
motor = 0
lockRotary = threading.Lock()
encoderA = 0
encoderB = 0
position = 0
initialPosition = 0
direction = 0
from dxl.dxlchain import DxlChain
from dxl.dxlcore import Dxl
port = "/dev/ttyACM0"
timeout = 0.01
rates = [2000000 / (data + 1) for data in range(0, 255)]
rates.append(2250000)
rates.append(2500000)
rates.append(3000000)
chain = DxlChain(port, rate=1000000, timeout=timeout)
for rate in rates:
print("rate", rate)
chain.reopen(port, rate, timeout=timeout)
try:
chain.factory_reset(Dxl.BROADCAST)
except Exception as e:
print(e)
chain.close()
#!/usr/bin/env python
# coding: utf-8
import rospy
from ik import *
import tkMessageBox
from Tkinter import *
from dynamixel_sdk import *
from dxl.dxlchain import DxlChain
from proyecto_delta.msg import posicion
global motores
motores = DxlChain("/dev/ttyUSB0", rate=1000000)
motors = motores.get_motor_list()
print motors
def callback_function(data):
ri = inverse(data.x, data.y, data.z)
motor1 = int(round(3800 - 10.8888 * ri[0]))
print motor1
motor2 = int(round(3050 - 11.8888 * ri[1]))
motor3 = int(round(2350 - 11.4444 * ri[2]))
motores.goto(1, motor1, speed=20, blocking=False)
motores.goto(2, motor2, speed=20, blocking=False)
motores.goto(3, motor3, speed=20, blocking=False)
rospy.loginfo("He recibido: Px = %f, Py = %f y Pz = %f", data.x, data.y,
data.z)
return [motor1, motor2, motor3]
rospy.init_node('tele_local', anonymous=True)
class Quadrapod:
def __init__(self):
self.initMotors()
self.initSensors()
def initMotors(self):
logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.DEBUG)
self.chain = DxlChain(dxlPath, rate=1000000, timeout=0.5)
self.lastMotorsPos = {}
chainIds = self.chain.get_motor_list() # Get chain ids
# Check if all needed motors are in the chain
for motor in allMotorsIds:
if not motor in chainIds:
raise ValueError("{}: motor not found in chain".format(motor))
self.lastMotorsPos[motor] = 0.
self.initMotorsSpeedAndTorque()
self.initMotorsBounds()
def initMotorsSpeedAndTorque(self):
for id in allMotorsIds:
self.chain.set_reg(id, "moving_speed", motorSpeed)
self.chain.set_reg(id, "max_torque", motorTorque)
self.chain.set_reg(id, "torque_limit", 1023)
def initMotorsBounds(self):
self.motorsBounds = {}
for i, it in enumerate(getInitialMotorsPositions()):
id = allMotorsIds[i]
if motorsDirs[i] == -1:
self.motorsBounds[id] = [it - moveSize, it]
elif motorsDirs[i] == 1:
self.motorsBounds[id] = [it, it + moveSize]
def initSensors(self):
self.myAHRS = myAHRS_plus(myAHRS_plusPath)
self.frontDistSensor = DistanceSensor(frontDistPins[0], frontDistPins[1])
self.backDistSensor = DistanceSensor(backDistPins[0], backDistPins[1])
def printMotorsTemperatures(self):
print("Motors temperatures:")
for motor in allMotorsIds:
try:
temp = self.chain.get_reg(motor, "present_temp")
print("{}: {} degree celsus".format(motor, temp))
except:
print("{}: Cannot get temperature".format(motor))
# Move motor to given position (between 0 and 1)
def moveMotor(self, id, pos, speed=None):
pos = self.motorsBounds[id][0] + (pos * moveSize)
if pos < 0:
pos = 0
elif pos > nbMotorStep:
pos = nbMotorStep - 1
try:
self.chain.goto(id, pos, speed, False)
except:
print(id)
print("Move motor exception")
pass
def waitMotorsStopped(self):
try:
self.chain.wait_stopped(allMotorsIds)
except:
print("wait motor exception")
sleep(0.5)
# Get motor position between 0 and 1
def getMotorPosition(self, id):
try:
pos = self.chain.get_reg(id, "present_position")
self.lastMotorsPos[id] = (pos - self.motorsBounds[id][0]) / moveSize
return self.lastMotorsPos[id]
except:
print(id)
print("Get motor exception")
return self.lastMotorsPos[id]
def getSensorsValues(self):
frontDist = self.frontDistSensor.getDistance()
backDist = self.backDistSensor.getDistance()
data = self.myAHRS.getValues()
roll = data[1]
pitch = data[2]
return frontDist, backDist, roll, pitch
class HWCoupling:
def __init__(self):
rospy.init_node('full_hw_controller')
self.excitation = False
self.lower_limit = False
self.upper_limit = False
self.calibrated = False
self.chain = DxlChain(rospy.get_param('~port'), rate=rospy.get_param('~rate'))
self.motors = self.chain.get_motor_list()
self.pub_reset_enc = rospy.Publisher('/scara_setup/linear_encoder/reset', Empty, queue_size=100)
rospy.Subscriber("/full_hw_controller/linear/command", Float64, self.callback_linear)
rospy.Subscriber("/full_hw_controller/linear/override", Float64, self.callback_linear_override)
self.shoulder_pub = rospy.Publisher("/full_hw_controller/shoulder/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/shoulder/command", Float64, self.callback_shoulder)
self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/elbow/command", Float64, self.callback_elbow)
self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/wrist/command", Float64, self.callback_wrist)
self.fingerjoint_pub = rospy.Publisher("/full_hw_controller/fingerjoint/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64, self.callback_fingerjoint)
rospy.Subscriber("/full_hw_controller/set_excitation", Bool, self.callback_excitation)
rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool, self.callback_lower_limit)
rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool, self.callback_upper_limit)
rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool, self.callback_calibrated)
rospy.Subscriber("/full_hw_controller/cog_linear", Empty, self.callback_cog_linear)
self.rate = rospy.Rate(20) # 10hz
self.set_excitation(False)
def callback_cog_linear(self, data):
self.callback_linear_override(Float64(-0.4))
while self.lower_limit == False:
pass
self.pub_reset_enc.publish(Bool(True))
rospy.sleep(0.05)
self.callback_linear_override(Float64(0.4))
while self.calibrated == False:
pass
self.callback_linear_override(Float64(0.0))
def set_excitation(self, par):
if par:
val = 1
#print "Excitation ON"
self.excitation = True
else :
val = 0
self.excitation = False
#print "Excitation OFF"
self.chain.set_reg(1, "torque_enable", val)
self.chain.set_reg(2, "torque_enable", val)
self.chain.set_reg(3, "torque_enable", val)
self.chain.set_reg(4, "torque_enable", val)
self.chain.set_reg(5, "torque_enable", val)
def callback_linear(self, data):
command = abs(round(data.data * 25600))
#if command > 5:
# command = command + 80
if command > 1023:
command = 1023
if data.data < 0:
command = command + 1024
if self.lower_limit and (data.data < 0.0):
return
if self.upper_limit and (data.data > 0.0):
return
if self.excitation:
self.chain.set_reg(1, "moving_speed", command)
def callback_linear_override(self, data):
if data.data == 0.0:
self.chain.set_reg(1, "torque_enable", 0)
return
self.chain.set_reg(1, "torque_enable", 1)
command = abs(round(data.data * 25600))
#if command > 5:
# command = command + 80
if command > 1023:
command = 1023
if data.data < 0:
command = command + 1024
if self.lower_limit and (data.data < 0.0):
return
if self.upper_limit and (data.data > 0.0):
return
self.chain.set_reg(1, "moving_speed", command)
def callback_excitation(self, data):
self.set_excitation(data.data)
def callback_calibrated(self, data):
self.calibrated = data.data
def callback_lower_limit(self, data):
self.lower_limit = data.data
if self.lower_limit and self.chain.get_reg(1, "present_speed") > 1024:
self.chain.set_reg(1, "moving_speed", 0)
def callback_upper_limit(self, data):
self.upper_limit = data.data
if self.upper_limit and self.chain.get_reg(1, "present_speed") < 1024:
self.chain.set_reg(1, "moving_speed", 0)
def callback_shoulder(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(2, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(2, "goal_pos", command)
def callback_elbow(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(3, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(3, "goal_pos", command)
def callback_wrist(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(4, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(4, "goal_pos", command)
def callback_fingerjoint(self, data):
command = 512 + round(data.data / 0.005117188)
#chain.goto(5, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(5, "goal_pos", command)
def loop(self):
while not rospy.is_shutdown():
rawval = (self.chain.get_reg(2, "present_position") - 2048) * 0.001533203
self.shoulder_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(3, "present_position") - 2048) * 0.001533203
self.elbow_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(4, "present_position") - 2048) * 0.001533203
self.wrist_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(5, "present_position") - 512) * 0.005117188
self.fingerjoint_pub.publish(Float64(rawval))
self.rate.sleep()
self.set_excitation(False)
self.chain.disable()
from dxl.dxlchain import DxlChain
import time
dyn_chain = DxlChain("/dev/ttyUSB0", rate=1000000)
print dyn_chain.get_motor_list()
print dyn_chain.get_position()
dyn_chain.close()
"""
python 2 test code for dynamixel ax12a turretcam
"""
import random
from dxl.dxlchain import DxlChain
chain=DxlChain("/dev/ttyACM0",rate=1000000)
def ___init___():
# Open the serial device
#chain=DxlChain("/dev/ttyACM0",rate=1000000)
# Load all the motors and obtain the list of IDs
motors=chain.get_motor_list() # Discover all motors on the chain and return their IDs
print motors
#center motors for mounting orientation
center()
#chain.sync_write_pos([1,2], [512,512])
# this corresponds to the position limits of a current <as of this writing> turret prototype. No automatic handling yet.
xmin = 0 # theoretical min 0
xmax = 1023 # theoretical max 1023
ymin = 150
ymax = 550
x_position_variable = 512
y_position_variable = 512
from dxl.dxlchain import DxlChain, DxlMotor
import time
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
chain.open()
chain.motors[250] = DxlMotor.instantiateMotor(250, 12)
# chain.get_motor_list(broadcast=False)
motor_id = 250
chain.ping(motor_id)
chain.set_reg(250, 'max_torque', 1023)
chain.set_reg(250, 'torque_limit', 1023)
while True:
load = chain.get_reg(250, 'present_load') & 1023
print 'load = %d' % load
if load > 200:
count += 1
else:
count = 0
if count > 4:
break
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import Float64
from dxl.dxlchain import DxlChain
chain = DxlChain("/dev/ttyUSB0", rate=250000)
motors = chain.get_motor_list()
chain.set_reg(5, "torque_enable", 1)
def callback(data):
command = 512 + round(data.data / 0.005117188)
chain.goto(5, command, speed=0, blocking=False)
def talker():
rospy.init_node('fingerjoint_hw_controller')
pub = rospy.Publisher("/fingerjoint_hw_controller/state", Float64, queue_size=10)
rospy.Subscriber("/fingerjoint_hw_controller/command", Float64, callback)
rate = rospy.Rate(50) # 10hz
while not rospy.is_shutdown():
rawval = (chain.get_reg(5, "present_position") - 512) * 0.005117188
pub.publish(Float64(rawval))
rate.sleep()
if __name__ == '__main__':
def turretconnect():
chain = DxlChain("/dev/ttyACM0", rate=57600)
chain.sync_write_pos_speed([1,2][512,512],[512,512])
from dxl.dxlchain import DxlChain
from dxl.dxlcore import Dxl
port="COM21"
timeout=0.01
rates=[2000000/(data+1) for data in range(0,255)]
rates.append(2250000)
rates.append(2500000)
rates.append(3000000)
chain=DxlChain(port,rate=1000000,timeout=timeout)
for rate in rates:
print "rate",rate
chain.reopen(port,rate,timeout=timeout)
try:
chain.factory_reset(Dxl.BROADCAST)
except Exception,e:
print e
chain.close()
def __init__(self):
rospy.init_node('full_hw_controller')
self.excitation = False
self.lower_limit = False
self.upper_limit = False
self.calibrated = False
self.chain = DxlChain(rospy.get_param('~port'),
rate=rospy.get_param('~rate'))
self.motors = self.chain.get_motor_list()
self.pub_reset_enc = rospy.Publisher(
'/scara_setup/linear_encoder/reset', Empty, queue_size=100)
rospy.Subscriber("/full_hw_controller/linear/command", Float64,
self.callback_linear)
rospy.Subscriber("/full_hw_controller/linear/override", Float64,
self.callback_linear_override)
self.shoulder_pub = rospy.Publisher(
"/full_hw_controller/shoulder/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/shoulder/command", Float64,
self.callback_shoulder)
self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state",
Float64,
queue_size=10)
rospy.Subscriber("/full_hw_controller/elbow/command", Float64,
self.callback_elbow)
self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state",
Float64,
queue_size=10)
rospy.Subscriber("/full_hw_controller/wrist/command", Float64,
self.callback_wrist)
self.fingerjoint_pub = rospy.Publisher(
"/full_hw_controller/fingerjoint/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64,
self.callback_fingerjoint)
rospy.Subscriber("/full_hw_controller/set_excitation", Bool,
self.callback_excitation)
rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool,
self.callback_lower_limit)
rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool,
self.callback_upper_limit)
rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool,
self.callback_calibrated)
rospy.Subscriber("/full_hw_controller/cog_linear", Empty,
self.callback_cog_linear)
self.rate = rospy.Rate(20) # 10hz
self.set_excitation(False)
class HWCoupling:
def __init__(self):
rospy.init_node('full_hw_controller')
self.excitation = False
self.lower_limit = False
self.upper_limit = False
self.calibrated = False
self.chain = DxlChain(rospy.get_param('~port'),
rate=rospy.get_param('~rate'))
self.motors = self.chain.get_motor_list()
self.pub_reset_enc = rospy.Publisher(
'/scara_setup/linear_encoder/reset', Empty, queue_size=100)
rospy.Subscriber("/full_hw_controller/linear/command", Float64,
self.callback_linear)
rospy.Subscriber("/full_hw_controller/linear/override", Float64,
self.callback_linear_override)
self.shoulder_pub = rospy.Publisher(
"/full_hw_controller/shoulder/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/shoulder/command", Float64,
self.callback_shoulder)
self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state",
Float64,
queue_size=10)
rospy.Subscriber("/full_hw_controller/elbow/command", Float64,
self.callback_elbow)
self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state",
Float64,
queue_size=10)
rospy.Subscriber("/full_hw_controller/wrist/command", Float64,
self.callback_wrist)
self.fingerjoint_pub = rospy.Publisher(
"/full_hw_controller/fingerjoint/state", Float64, queue_size=10)
rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64,
self.callback_fingerjoint)
rospy.Subscriber("/full_hw_controller/set_excitation", Bool,
self.callback_excitation)
rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool,
self.callback_lower_limit)
rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool,
self.callback_upper_limit)
rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool,
self.callback_calibrated)
rospy.Subscriber("/full_hw_controller/cog_linear", Empty,
self.callback_cog_linear)
self.rate = rospy.Rate(20) # 10hz
self.set_excitation(False)
def callback_cog_linear(self, data):
self.callback_linear_override(Float64(-0.4))
while self.lower_limit == False:
pass
self.pub_reset_enc.publish(Bool(True))
rospy.sleep(0.05)
self.callback_linear_override(Float64(0.4))
while self.calibrated == False:
pass
self.callback_linear_override(Float64(0.0))
def set_excitation(self, par):
if par:
val = 1
#print "Excitation ON"
self.excitation = True
else:
val = 0
self.excitation = False
#print "Excitation OFF"
self.chain.set_reg(1, "torque_enable", val)
self.chain.set_reg(2, "torque_enable", val)
self.chain.set_reg(3, "torque_enable", val)
self.chain.set_reg(4, "torque_enable", val)
self.chain.set_reg(5, "torque_enable", val)
def callback_linear(self, data):
command = abs(round(data.data * 25600))
#if command > 5:
# command = command + 80
if command > 1023:
command = 1023
if data.data < 0:
command = command + 1024
if self.lower_limit and (data.data < 0.0):
return
if self.upper_limit and (data.data > 0.0):
return
if self.excitation:
self.chain.set_reg(1, "moving_speed", command)
def callback_linear_override(self, data):
if data.data == 0.0:
self.chain.set_reg(1, "torque_enable", 0)
return
self.chain.set_reg(1, "torque_enable", 1)
command = abs(round(data.data * 25600))
#if command > 5:
# command = command + 80
if command > 1023:
command = 1023
if data.data < 0:
command = command + 1024
if self.lower_limit and (data.data < 0.0):
return
if self.upper_limit and (data.data > 0.0):
return
self.chain.set_reg(1, "moving_speed", command)
def callback_excitation(self, data):
self.set_excitation(data.data)
def callback_calibrated(self, data):
self.calibrated = data.data
def callback_lower_limit(self, data):
self.lower_limit = data.data
if self.lower_limit and self.chain.get_reg(1, "present_speed") > 1024:
self.chain.set_reg(1, "moving_speed", 0)
def callback_upper_limit(self, data):
self.upper_limit = data.data
if self.upper_limit and self.chain.get_reg(1, "present_speed") < 1024:
self.chain.set_reg(1, "moving_speed", 0)
def callback_shoulder(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(2, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(2, "goal_pos", command)
def callback_elbow(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(3, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(3, "goal_pos", command)
def callback_wrist(self, data):
command = 2048 + round(data.data / 0.001533203)
#chain.goto(4, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(4, "goal_pos", command)
def callback_fingerjoint(self, data):
command = 512 + round(data.data / 0.005117188)
#chain.goto(5, command, speed=0, blocking=False)
if self.excitation:
self.chain.set_reg(5, "goal_pos", command)
def loop(self):
while not rospy.is_shutdown():
rawval = (self.chain.get_reg(2, "present_position") -
2048) * 0.001533203
self.shoulder_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(3, "present_position") -
2048) * 0.001533203
self.elbow_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(4, "present_position") -
2048) * 0.001533203
self.wrist_pub.publish(Float64(rawval))
rawval = (self.chain.get_reg(5, "present_position") -
512) * 0.005117188
self.fingerjoint_pub.publish(Float64(rawval))
self.rate.sleep()
self.set_excitation(False)
self.chain.disable()
import Adafruit_PCA9685
#Imports for ESC
import os #importing os library so as to communicate with the system
import time #importing time library to make Rpi wait because its too impatient
os.system ("sudo pigpiod") #Launching GPIO library
time.sleep(1) # As i said it is too impatient and so if this delay is removed you will get an error
import pigpio #importing GPIO library
import picamera #Import camera module
pwm = Adafruit_PCA9685.PCA9685()
#Import DxlChain
from dxl.dxlchain import DxlChain
# Open the serial device
chain=DxlChain("/dev/ttyUSB0",rate=1000000)
# Load all the motors and obtain the list of IDs
motors=chain.get_motor_list() # Discover all motors on the chain and return their IDs
Motor1=1
Motor2=2
#ESC Variables
ESC1=4 #Connect the Propeller ESC in this GPIO pin
ESC2=18 #Connect the DC Motor ESC in this GPIO pin
pi = pigpio.pi();
pi.set_servo_pulsewidth(ESC1, 0)
pi.set_servo_pulsewidth(ESC2, 0)
max_value = 2000 #change this if your ESC's max value is different
min_value = 700 #change this if your ESC's min value is different
def main():
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
motors = chain.get_motor_list()
print motors
reset_id = raw_input("Do you want to change your current motor id?[Y/N]:")
if reset_id in ["y", "Y", "yes", "Yes"]:
oldid = int(raw_input("Which motor ID do you want to change:"))
newid = int(raw_input("What is the new ID you want to set:"))
chain.set_reg(oldid, "id", newid)
# Before we start to print out the motor we should first set up the P, I, D control
for motor in motors:
if motor == 1:
setup_PID(chain, motor, 10, 10, 8) # 10,10,8
elif motor == 2:
setup_PID(chain, motor, 25, 100, 23) # [30,100,10] [25,100,0]
else:
break
# First step is to initialize the motor to a specific motion
for motor in motors:
if motor == 1:
chain.goto(motor, INITIAL_POSITION_1, SPEED)
elif motor == 2:
chain.goto(motor, INITIAL_POSITION_2, SPEED)
else:
chain.goto(motor, INITIAL_POSITION, SPEED)
while True:
mode = int(raw_input("1.control servo 2. draw square: "))
if mode != 1 and mode != 2:
print ("The mode selection you enter is invalid please re-enter the mode")
else:
break
# if the user select control servo then run the first process
if mode == 1:
while True:
try:
print ("the current positions for the motors are:")
for motor in motors:
pos = chain.get_position(motor)
pos = pos[motor]
print ("%d: %f" % (motor, position_to_angle(pos)))
current_projection(chain)
for motor in motors:
new_pos = raw_input("What is the new position you want to goto for motor %d(0-180):" % (motor))
new_pos = angle_to_position(float(new_pos))
chain.goto(motor, new_pos, SPEED)
time.sleep(1)
except KeyboardInterrupt:
print ("\n")
print ("The program end")
break
else:
while True:
try:
yaw = float(raw_input("please define the angle for the base motor: "))
pitch = float(raw_input("please define the desired angle for the above motor: "))
start_time = time.time()
# initialize the position
chain.goto(1, angle_to_position(yaw), SPEED)
chain.goto(2, angle_to_position(pitch), SPEED)
time.sleep(2)
# The default direction will be clock wise
cp = forward_kinematics(yaw, pitch)
current_projection(chain)
fp1 = cp + np.array([[100], [0], [0], [0]])
straight_line_move(chain, cp, fp1)
print ("Arrive fp1")
current_projection(chain)
print fp1
time.sleep(1)
fp2 = fp1 + np.array([[0], [-100], [0], [0]])
straight_line_move(chain, fp1, fp2)
current_projection(chain)
print ("Arrive fp2")
current_projection(chain)
print fp2
time.sleep(1)
fp3 = fp2 + np.array([[-100], [0], [0], [0]])
straight_line_move(chain, fp2, fp3)
print ("Arrive fp3")
current_projection(chain)
print fp3
time.sleep(1)
fp4 = fp3 + np.array([[0], [100], [0], [0]])
straight_line_move(chain, fp3, fp4)
print ("Arrive fp4")
current_projection(chain)
print fp4
time.sleep(0)
total_time = time.time() - start_time
print ("the process takes %f seconds" % (total_time))
except KeyboardInterrupt:
print ("\n")
print ("The program end")
break
]
radmotors = self._int2rad(self.curr_joints, offset=True)
radchain = self.mik.motor_to_chain(radmotors)
rot_matrix = self.mik.direct_kinematics(radchain)
self.curr_pos = self.mik.dh_to_pos(rot_matrix)
return [self.curr_pos[0], self.curr_pos[1], self.curr_pos[2], 0, 0, 0]
if __name__ == "__main__":
# Power cycle dynamixels
GPIO.setmode(GPIO.BOARD)
GPIO.setup(DYN_CTRL, GPIO.OUT)
GPIO.output(DYN_CTRL, GPIO.HIGH)
time.sleep(0.1)
GPIO.output(DYN_CTRL, GPIO.LOW)
dyn_chain = DxlChain(USB_PORT, rate=1000000)
# Load all the motors and obtain the list of IDs
motors = dyn_chain.get_motor_list(
) # Discover all motors on the chain and return their IDs
print motors
act_pos = dyn_chain.get_position()
arm = ArmCtrl(dyn_chain)
arm.curr_joints = dyn_chain.get_position([1, 2, 4, 6])
arm.init_pos()
# Disable the motors
if arm.VIEW_POS_RT:
while True:
joints = arm._int2rad(arm.curr_joints, offset=True)
print joints
from dxl.dxlchain import DxlChain, DxlMotor
import time
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
chain.open()
# chain.motors[250] = DxlMotor.instantiateMotor(250, 12)
chain.get_motor_list()
motor_id = 8
chain.set_reg(motor_id, 'cw_angle_limit', 0)
chain.set_reg(motor_id, 'ccw_angle_limit', 0)
chain.ping(motor_id)
chain.set_reg(motor_id, 'max_torque', 1023)
chain.set_reg(motor_id, 'torque_limit', 1023)
while True:
load = chain.get_reg(motor_id, 'present_load')
direction = load >> 10
load = load % 1023
print 'load = %d, dir = %d' % (load, direction)
if load >= 9:
chain.set_reg(motor_id, 'moving_speed', 1023)
time.sleep(1)
# Functions for moves
def xMove( mX, mX2, goX, speed ):
chain.goto( mX, goX, speed )
chain.goto( mX2, goX, speed )
return;
def yMove( mY, goY, speed ):
chain.goto( mY, goY, speed )
return;
def zMove( mZ, goZ, speed ):
chain.goto(mZ, goZ, speed )
return;
# Initialize Serial
chain = DxlChain("/dev/ttyUSB0", rate = 1000000)
# Initialize Motors
motors = chain.get_motor_list()
print motors
# Display Initial Position
currPosX = chain.get_position(mX)
currPosY = chain.get_position(mY)
currPosZ = chain.get_position(mZ)
currPos = [currPosX,currPosY,currPosZ]
print currPos
# Auto Home if Not at Home
if currPos != home:
xMove()
from dxl.dxlchain import DxlChain
chain = DxlChain("/dev/ttyACM0", rate = 57600, timeout = 0.02)
print(chain.get_motor_list())
class Arm:
def __init__(self, port=USB_PORT):
self.port = port
self.goal = Point(0, 0, 0, 0)
self.opened = False
self.pi = None
def open(self):
self.pi = pigpio.pi()
self.dyn_chain = DxlChain(self.port, rate=1000000)
self.dyn_chain.open()
self.motors = self.dyn_chain.get_motor_list()
assert len(
self.motors
) == 6, 'Some arm motors are missing. Expected 6 instead got %d' % len(
self.motors)
self.tyro_manager = TyroManager(self.dyn_chain)
self.tyro_manager.start()
self.opened = True
def close(self):
self.opened = False
self.dyn_chain.close()
def goto2D(self, y, z, r, speed=50):
'''
Uses inverse kinematic to go to a position in planar space (only y and z)
'''
r = math.radians(r)
self.goal = Point(0, y, z, r)
joints = ik(*self.goal)
joints = map(math.degrees, joints)
goals = angles_to_motors(*joints)
return self.write_goal_without_base(goals[1],
goals[2],
goals[3],
speed=speed)
def goto(self, x, y, z, r, speed=50):
'''
Uses inverse kinematic to go to a position in space
'''
r = math.radians(r)
self.goal = Point(x, y, z, r)
joints = ik(*self.goal)
joints = map(math.degrees, joints)
goals = angles_to_motors(*joints)
return self.write_goal(*goals, speed=speed)
def write_goal_without_base(self, goal23, goal4, goal5, speed=50):
'''
Write goal positions of all servos with given speed
'''
goal23 = clamp(goal23, MOTORS[2]['min'], MOTORS[2]['max'])
goal4 = clamp(goal4, MOTORS[4]['min'], MOTORS[4]['max'])
goal5 = clamp(goal5, MOTORS[5]['min'], MOTORS[5]['max'])
if isinstance(speed, list):
s23, s4, s5 = speed
self.dyn_chain.sync_write_pos_speed(
[2, 3, 4, 5], [goal23, 1023 - goal23, goal4, goal5],
[s23, s23, s4, s5])
else:
self.dyn_chain.sync_write_pos_speed(
[2, 3, 4, 5], [goal23, 1023 - goal23, goal4, goal5],
[speed] * 5)
def write_single_goal(self, motor_id, value, speed=50):
if motor_id == 2:
self.dyn_chain.sync_write_pos_speed([2, 3], [value, 1023 - value],
[speed] * 2)
else:
self.dyn_chain.sync_write_pos_speed([motor_id], [value], [speed])
def write_goal(self, goal1, goal23, goal4, goal5, speed=50):
'''
Write goal positions of all servos with given speed
'''
goal1 = clamp(goal1, MOTORS[1]['min'], MOTORS[1]['max'])
goal23 = clamp(goal23, MOTORS[2]['min'], MOTORS[2]['max'])
goal4 = clamp(goal4, MOTORS[4]['min'], MOTORS[4]['max'])
goal5 = clamp(goal5, MOTORS[5]['min'], MOTORS[5]['max'])
if isinstance(speed, list):
s1, s23, s4, s5 = speed
self.dyn_chain.sync_write_pos_speed(
[1, 2, 3, 4, 5], [goal1, goal23, 1023 - goal23, goal4, goal5],
[s1, s23, s23, s4, s5])
else:
self.dyn_chain.sync_write_pos_speed(
[1, 2, 3, 4, 5], [goal1, goal23, 1023 - goal23, goal4, goal5],
[speed] * 5)
def disable_all(self):
'''
Disable torque of all motors
'''
self.dyn_chain.disable()
def move_base(self, direction, speed=30):
'''
Moves the base in the given direction at the given speed
Direction = 1 / -1 for cw/ccw direction, 0 => stop
'''
if direction == 1:
self.dyn_chain.goto(1,
MOTORS[1]['min'],
speed=speed,
blocking=False)
elif direction == -1:
self.dyn_chain.goto(1,
MOTORS[1]['max'],
speed=speed,
blocking=False)
else:
self.dyn_chain.disable(1)
def get_angles(self):
'''
Estimates joints angle based on servos positions
'''
return motors_to_angles(*self.get_position())
def get_position(self):
'''
Servos positions
'''
positions = self.dyn_chain.get_position()
return (positions[1], positions[2], positions[4], positions[5])
def motors_load(self):
'''
Ratio of maximal torque of each joints.
A value of 0.5 means 50% of maximmal torque applied in clockwise direction
A value of -0.25 means 25% of maximmal torque applied in counter-clockwise Direction
Return value is (motor1, motor2, motor4, motor5)
'''
loads = []
for i in [1, 2, 4, 5, 8]:
present_load = self.dyn_chain.get_reg(i, "present_load")
ratio = (present_load & 1023) / 1023.0
direction = ((present_load >> 10) & 1) * 2 - 1
loads.append(ratio * direction)
return loads
def wait_stopped(self):
'''
Sleeps until all the motors reached their goals
'''
self.dyn_chain.wait_stopped([1, 2, 3, 4, 5])
def stop_movement(self):
'''
Sets the goal to the current position of the motors
'''
positions = self.get_position()
self.write_goal(*positions)
def wait_stopped_sleep(self, sleep=time.sleep):
'''
Sleeps using the provided function until all the motors reached their goals
'''
ids = self.dyn_chain.get_motors([1, 2, 3, 4, 5])
while True:
moving = False
for id in ids:
if self.dyn_chain.get_reg(id, 'moving') != 0:
moving = True
break
if not moving:
break
sleep(0.1)
def set_tyro_manager_state(self, state):
self.tyro_manager.state = state
def get_chain_info(self):
dyn_infos = {}
loads = self.motors_load()
positions = self.get_position()
for i in [1, 2]:
motor = {
'temp': self.dyn_chain.get_reg(i, 'present_temp'),
'load': loads[i - 1],
'present_pos': positions[i - 1]
}
dyn_infos['motor' + str(i)] = motor
for i in [4, 5]:
motor = {
'temp': self.dyn_chain.get_reg(i, 'present_temp'),
'load': loads[i - 2],
'present_pos': positions[i - 2]
}
dyn_infos['motor' + str(i)] = motor
dyn_infos['motor3'] = {
'temp': self.dyn_chain.get_reg(3, 'present_temp'),
'load': loads[1],
'present_pos': positions[1]
}
dyn_infos['motor8'] = {
'temp': self.dyn_chain.get_reg(8, 'present_temp'),
'load': loads[4],
'present_pos': 0
}
return dyn_infos
def reset_servos_torque(self):
servos = [1, 2, 3, 4, 5, 8]
self.dyn_chain.disable(servos)
for id in servos:
self.dyn_chain.set_reg(id, 'torque_limit', 1023)
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import Float64
from dxl.dxlchain import DxlChain
chain = DxlChain("/dev/ttyUSB0", rate=250000)
motors = chain.get_motor_list()
chain.set_reg(1, "torque_enable", 1)
def callback(data):
command = abs(round(data.data * 25600))
#if command > 5:
# command = command + 80
if command > 1023:
command = 1023
if data.data < 0:
command = command + 1024
#chain.goto(5, command, speed=0, blocking=False)
chain.set_reg(1, "moving_speed", command)
print command
def talker():
def saveInitialMotorsPositions(chain):
file = open(initialPosFilename, "w")
for id in allMotorsIds:
file.write(str(chain.get_reg(id, "present_position")) + "\n")
file.close()
def getInitialMotorsPositions():
out = []
file = open(initialPosFilename, "r")
for line in file.readlines():
if line != "":
out.append(float(line))
file.close()
return (out)
if __name__ == "__main__":
logging.basicConfig(format='%(levelname)s:%(message)s',
level=logging.DEBUG)
chain = DxlChain(dxlPath, rate=1000000)
chainIds = chain.get_motor_list() # Get chain ids
print(chainIds)
# Check if all needed motors are in the chain
for motor in allMotorsIds:
if not motor in chainIds:
raise ValueError("{}: motor not found in chain".format(motor))
saveInitialMotorsPositions(chain)
import time
import sys
from multiprocessing import Pool
import myo
import classify_myo
from myo_raw import Pose
from dxl.dxlchain import DxlChain
# Open the serial device
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
#def git_to(position, blocking=True):
# chain.set_position({1: position, 2: position, 3: position, 4: position, 5: position, 6: position},
# blocking=blocking)
# Load all the motors and obtain the list of IDs
motors = chain.get_motor_list(
broadcast=True) # Discover all motors on the chain and return their IDs
print("Discovering Dynamixels and dumping information...")
chain.dump()
#git_to(1000)
#git_to(0)
chain.goto(1, 1023, 1000, blocking=False)
chain.goto(2, 1023, 1000, blocking=False)
chain.goto(4, 1023, 1000, blocking=False)
chain.goto(5, 1023, 1000, blocking=False)
chain.goto(6, 1023, 1000, blocking=False)
print(chain.get_reg(1, "present_load"))
){kind=link}