def main(settings):
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=1)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
# Populate our network with dynamixel objects
for servoId in settings['servoIds']:
newDynamixel = dynamixel.Dynamixel(servoId, net)
net._dynamixel_map[servoId] = newDynamixel
if not net.get_dynamixels():
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
# Prompt the user to move servos.
answer = int(raw_input("Input an integer between 0 and 1024: "))
if (0 < answer < 1024):
# Set up the servos
for actuator in net.get_dynamixels():
actuator.moving_speed = 500
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
actuator.goal_position = answer
# Send all the commands to the servos.
net.synchronize()
def __init__(self, port='/dev/ttyUSB1'):
self.baudrate = 1000000
self.port = port
self.maxID = 18
self.actuators = [None] * (self.maxID + 1)
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
self.serial = dynamixel.SerialStream(port=self.port,
baudrate=self.baudrate,
timeout=1)
# Instantiate our network object
self.net = dynamixel.DynamixelNetwork(self.serial)
# Populate our network with dynamixel objects
for servoId in xrange(1, self.maxID + 1):
newDynamixel = dynamixel.Dynamixel(servoId, self.net)
self.net._dynamixel_map[servoId] = newDynamixel
self.actuators[servoId] = newDynamixel
# Verify Initialization
if not self.net.get_dynamixels():
print 'No Dynamixels Found!'
# TODO: Throw exception instead
sys.exit(0)
else:
print "Dynamixels connected."
# Initialize servos
for servoId in xrange(1, self.maxID + 1):
dyn = self.actuators[servoId]
dyn.torque_enable = True
dyn.goal_position = dyn.current_position
def __init__(self, settings):
self.portName = settings['port']
self.baudRate = settings['baudRate']
self.motorConfig = settings['motorConfig']
self.motors = []
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=self.portName,
baudrate=self.baudRate,
timeout=1)
self.net = dynamixel.DynamixelNetwork(serial)
self.idList = []
self.nameList = []
print "Scanning for Dynamixels..."
for conf in self.motorConfig:
self.idList += [conf[0]]
self.nameList += [conf[3]]
self.net.scan(min(self.idList), max(self.idList))
for conf in self.motorConfig:
if conf[0] in [motor.id for motor in self.net.get_dynamixels()]:
print "motor " + str(conf[3]) + " has been found"
self.motors += [
Motor(self.net[conf[0]], conf[3], conf[1], conf[2])
]
else:
print "motor " + str(conf[3]) + " has not been found"
self.motors += [None]
self.setAllSpeed()
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Create our sensor object. Sensor is assumed to be at id 100
sensor = dynamixel.SensorModule(100, net)
######################
# Playing sounds
# How long we want the note to play (.3s to 5s in tenths of seconds)
# Notes:
# This resets after the note is played
# Use 254 as the value for continual play and then 0 to turn it off.
sensor.buzzer_time = 10
# Play a note (0-41)
print('Playing a note ...')
sensor.buzzer_index = 19
time.sleep(2)
# Play one of the short sound sequences the S1 knows
print("Playing a tune ...")
sensor.buzzer_time = 255
sensor.buzzer_index = 1
time.sleep(2)
#######################
# Reading values from environment
print('Reading values from the world ...')
# The sensor can hear itself play notes, so reset the counter
sensor.sound_detected_count = 0
print("Left IR \tCenter IR\tRight IR\tTemperature\tVoltage")
for i in count(1):
if i % 10 == 0:
print("CLAP TWICE TO EXIT")
if sensor.sound_detected_count >= 2:
sys.exit(0)
# Get our sensor values
lir = sensor.left_ir_sensor_value
cir = sensor.center_ir_sensor_value
rir = sensor.right_ir_sensor_value
temp = sensor.current_temperature
volts = sensor.current_voltage
print(lir, '\t\t', cir, '\t\t', rir, '\t\t', temp, '\t\t', volts)
time.sleep(.5)
def dynSetup():
global param
# Look for a settings.yaml file
settingsFile = 'settings.yaml'
if os.path.exists(settingsFile):
with open(settingsFile, 'r') as fh:
settings = yaml.load(fh)
# If we were asked to bypass, or don't have settings
else:
settings = {}
settings['port'] = '/dev/ttyUSB' + str(param.com)
# Baud rate
baudRate = param.baud
print "##### baud = ", baudRate
settings['baudRate'] = baudRate
# Servo ID
highestServoId = getMaxID(param)
settings['highestServoId'] = highestServoId
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=0.02)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
settings['servoIds'] = []
print "Found the following Dynamixels IDs: "
for dyn in net.get_dynamixels():
print dyn.id
settings['servoIds'].append(dyn.id)
# Make sure we actually found servos
if not settings['servoIds']:
print 'No Dynamixels Found!'
sys.exit(0)
# Save the output settings to a yaml file
with open(settingsFile, 'w') as fh:
yaml.dump(settings, fh)
print("Your settings have been saved to 'settings.yaml'. \nTo " +
"change them in the future either edit that file or run " +
"this example with -c.")
mainSetup(settings)
def __init__(self):
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=config.port,
baudrate=config.baudRate,
timeout=1)
# Instantiate our network object
self.net = dynamixel.DynamixelNetwork(serial)
# Populate our network with dynamixel objects
for servoId in config.wheels[0] + config.wheels[1] + config.joints:
newDynamixel = dynamixel.Dynamixel(servoId, self.net)
self.net._dynamixel_map[servoId] = newDynamixel
# Initial read in
for actuator in self.net.get_dynamixels():
print 'Currently on motor %d' % actuator.id
actuator.read_all()
if actuator.id in config.wheels[0]:
print ' Left wheel'
actuator.cw_angle_limit = 0
actuator.ccw_angle_limit = 0
# actuator.moving_speed = 200
elif actuator.id in config.wheels[1]:
print ' Right wheel'
actuator.cw_angle_limit = 0
actuator.ccw_angle_limit = 0
# actuator.moving_speed = 1223
elif actuator.id in config.joints:
print ' Joint'
actuator.cw_angle_limit = 0
actuator.ccw_angle_limit = 1023
# actuator.moving_speed = 100
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
# actuator.goal_position = 512
else:
print ' Something is wrong'
# Some state information of the Mantis robot
self.moveVelocity = 0
self.skidVelocity = 0
self.currentAngle = self.net[config.joints[0]].current_position
self.turnStopped = False
self.liftStopped = False
# Start the scheduled synchronize every 0.2 seconds
self.synchronize(0.2)
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 50
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
# Randomly vary servo position within a small range
print "Servo \tPosition"
while True:
for actuator in myActuators:
actuator.goal_position = random.randrange(450, 600)
net.synchronize()
for actuator in myActuators:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
print actuator._id, "\t", actuator.current_position
time.sleep(2)
def __init__(self, settings, errorlog):
try:
self.name = settings['name']
except:
self.name = "RULS_DEFAULT"
self.clientsocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.connected = False
self.configurations = {}
self.configuration_ids = []
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
try:
self.serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=1)
except:
errorlog.write("ERROR: No Dynamixels Found!\n")
printdt("No Dynamixels Found!")
device_controller.restart_program()
# Instantiate our network object
self.net = dynamixel.DynamixelNetwork(self.serial)
# Populate our network with dynamixel objects
for servoId in settings['servoIds']:
newDynamixel = dynamixel.Dynamixel(servoId, self.net)
self.net._dynamixel_map[servoId] = newDynamixel
# Get all the dynamixels in the network
if not self.net.get_dynamixels():
errorlog.write("ERROR: No Dynamixels Found!\n")
printdt("No Dynamixels Found!")
device_controller.restart_program()
else:
printdt("Dynamixels found, network initialized")
for actuator in self.net.get_dynamixels():
actuator._set_to_wheel_mode()
actuator.moving_speed = 1024
actuator.torque_enable = False
actuator.torque_limit = 1023
actuator.max_torque = 1023
actuator.goal_position = 512
self.net.synchronize()
def __init__(self,
servo_map,
theta0,
speed=80,
grasp_angle=10,
release_angle=-30,
port="COM15",
bps=1000000):
self.port = port
self.theta0 = theta0
self.speed = speed
self.bps = bps
self.grasp_angle = grasp_angle
self.release_angle = release_angle
self.servos = {}
self.servo_num = 0
lastServoId = max(servo_map) + 1
self.serial = dynamixel.SerialStream(port=self.port,
baudrate=self.bps,
timeout=1)
self.net = dynamixel.DynamixelNetwork(self.serial)
self.net.scan(1, lastServoId)
print "Scanning for servos..."
for dyn in self.net.get_dynamixels():
print "Found", dyn.id
self.servos[servo_map.index(dyn.id)] = self.net[dyn.id]
self.servo_num += 1
if not self.servos:
print 'No servos found! Check USB2Dynamixel connection.'
sys.exit(0)
else:
print "...Done!"
for id, servo in self.servos.items():
servo.moving_speed = self.speed
servo.synchronized = True
servo.torque_enable = True
servo.torque_limit = 800
servo.max_torque = 800
def main(settings):
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=1)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
# Populate our network with dynamixel objects
for servoId in settings['servoIds']:
newDynamixel = dynamixel.Dynamixel(servoId, net)
net._dynamixel_map[servoId] = newDynamixel
if not net.get_dynamixels():
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
# Prompt the user to move servos.
answer = raw_input("Would you like to move all the servos to the home position?"
"\nWARNING: If servos are obstructed this could damage them "
"\nor things in their path. [y/N] ")
if answer in ['y', 'Y', 'yes', 'Yes', 'YES']:
# Set up the servos
for actuator in net.get_dynamixels():
actuator._set_to_joint_mode()
actuator.moving_speed = 50
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
actuator.goal_position = 512
# Send all the commands to the servos.
net.synchronize()
print("Congratulations! Read the code to find out how that happened!")
def mainSetup(settings):
global robot
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=0.02)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
# Populate our network with dynamixel objects
for servoId in settings['servoIds']:
newDynamixel = dynamixel.Dynamixel(servoId, net)
net._dynamixel_map[servoId] = newDynamixel
if not net.get_dynamixels():
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
robot = net
def __init__(self, parameters=0):
# The number of Dynamixels on our bus.
nServos = 10
# Set your serial port accordingly.
if os.name == "posix":
portName = "/dev/tty.usbserial-A94N75T1"
else:
portName = "COM3"
# Default baud rate of the USB2Dynamixel device.
baudRate = 1000000
self.serial = dynamixel.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
self.net = dynamixel.DynamixelNetwork(self.serial)
sys.stdout.flush()
self.net.scan(0, nServos)
myActuators = list()
for dyn in self.net.get_dynamixels():
myActuators.append(dyn)
myActuators[-1].moving_speed = 50
myActuators[-1].synchronized = True
myActuators[-1].torque_enable = True
myActuators[-1].torque_limit = 800
myActuators[-1].max_torque = 800
self.listeMoteurs = myActuators
self.parameters = list()
'''Define parameters for angles: list: 1 row=> 1 motor, first column: 0 angle, second column positive/negative rotation'''
if parameters != 0:
self.parameters = parameters
else:
for act in range(0, len(self.listeMoteurs)):
self.parameters.append([0, 1])
return False
# The number of Dynamixels on our bus.
nServos = 11
# Set your serial port accordingly.
if os.name == "posix":
portName = "/dev/ttyUSB0"
else:
portName = "COM11"
# Default baud rate of the USB2Dynamixel device.
baudRate = 1000000
serial = dynamixel.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel.DynamixelNetwork(serial)
net.scan(1, nServos)
myActuators = list()
print "Scanning for Dynamixels...",
for dyn in net.get_dynamixels():
print dyn.id,
myActuators.append(net[dyn.id])
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 50
actuator.synchronized = True
actuator.torque_enable = True
FLAGS = gflags.FLAGS
gflags.DEFINE_string('port', '/dev/ttyUSB0', 'dynamixel port')
gflags.DEFINE_integer('baud', 1000000, 'baud rate')
gflags.DEFINE_integer('min_id', 1, 'lowest dynamixel ID')
gflags.DEFINE_integer('max_id', 8, 'highest dynamixel ID')
gflags.DEFINE_enum('command', '',
['', 'position', 'torque_on', 'torque_off', 'control'],
'command to execute')
if __name__ == '__main__':
flags = FLAGS(sys.argv)
serial = dynamixel.SerialStream(port=FLAGS.port,
baudrate=FLAGS.baud,
timeout=1)
# Instantiate network object
net = dynamixel.DynamixelNetwork(serial)
# Populate network with dynamixel objects
for servoId in range(FLAGS.min_id, FLAGS.max_id + 1):
newDynamixel = dynamixel.Dynamixel(servoId, net)
net._dynamixel_map[servoId] = newDynamixel
servos = net.get_dynamixels()
print('network initialised')
if FLAGS.command == '' or FLAGS.command == 'torque_off':
for d in servos:
d.torque_enable = False
def main(settings):
# Open Hubo-Ach feed-forward and feed-back (reference and state) channels
# s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
# e = ach.Channel(ha.HUBO_CHAN_ENC_NAME)
# r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
#s.flush()
#r.flush()
# feed-forward will now be refered to as "state"
# state = ha.HUBO_STATE()
# encoder channel will be refered to as "encoder"
# encoder = ha.HUBO_ENC()
# feed-back will now be refered to as "ref"
# ref = ha.HUBO_REF()
# Get the current feed-forward (state)
# [statuss, framesizes] = s.get(state, wait=False, last=True)
# [statuss, framesizes] = e.get(encoder, wait=False, last=True)
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Before search you must enable the dyn
# This is untested but should turn the actuators on (i.e. the lights should blink once)
enable(serial,net)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 50
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
# Randomly vary servo position within a small range
print myActuators
print "Hubo-Ach neck server active"
# print "Servo \tPosition"
while True:
# Get the current feed-forward (state)
# [statuss, framesizes] = s.get(state, wait=False, last=True)
for actuator in myActuators:
if ( actuator.id == 1):
actuator.goal_position = rad2dyn(0.0) # set id 1 to 0.0 rad
if ( actuator.id == 2):
actuator.goal_position = rad2dyn(0.0) # set id 2 to 0.0 rad
if ( actuator.id == 15):
actuator.goal_position = rad2dyn(0.0) # set id 15 to 0.0 rad
actuator.goal_position = rad2dyn(0.0) # set all ids in range to 0.0 rad
# send to robot
net.synchronize()
# Get actuators current position
for actuator in myActuators:
actuator.read_all()
time.sleep(0.01)
if ( actuator.id == 1):
enc = dyn2rad(actuator.current_position) # get enc pos of id 1
#encoder.enc[ha.NKY] = dyn2rad(actuator.current_position)
if ( actuator.id == 2):
enc = dyn2rad(actuator.current_position) # get enc pos of id 2
#encoder.enc[ha.NK1] = dyn2rad(actuator.current_position)
if ( actuator.id == 15):
enc = dyn2rad(actuator.current_position) # get enc pos of id 15
#encoder.enc[ha.NK2] = dyn2rad(actuator.current_position)
enc = dyn2rad(actuator.current_position) # get enc pos of id actuator.id
# print encoder.enc[ha.NKY], " : ", encoder.enc[ha.NK1], " : ", encoder.enc[ha.NK2]
# e.put(encoder)
time.sleep(0.02)
def buscarServos(self):
"""
busca los servos, los inicia e inicializa el electroiman
La primera vez que se ejecuta guarda la configuracion en un archivo settins.yaml
para futuros usos, asi no hay de volver a buscar y solo carga la ultima configuracion
"""
self.iman = Arduino()
parser = optparse.OptionParser()
parser.add_option(
"-c",
"--clean",
action="store_true",
dest="clean",
default=False,
help=
"Ignore the settings.yaml file if it exists and prompt for new settings."
)
(options, args) = parser.parse_args()
#Busca el archivo settings.yaml
settingsFile = 'settings.yaml'
if not options.clean and os.path.exists(settingsFile):
with open(settingsFile, 'r') as fh:
self.settings = yaml.load(fh)
#Si no lo encuentra
else:
self.settings = {}
self.settings['port'] = '/dev/ttyUSB0'
# Baud rate
baudRate = self.validateInput(1000000, 9600, 1000000)
self.settings['baudRate'] = baudRate
# Servo ID
highestServoId = None
while not highestServoId:
hsiTest = raw_input(
"Please enter the highest ID of the connected servos: ")
highestServoId = self.validateInput(hsiTest, 1, 255)
self.settings['highestServoId'] = highestServoId
highestServoId = self.settings['highestServoId']
#Establece una conexion serial con dynamixel network
#Requiere un USB2Dynamixel
serial = dynamixel.SerialStream(port=self.settings['port'],
baudrate=self.settings['baudRate'],
timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
self.settings['servoIds'] = []
print "Found the following Dynamixels IDs: "
for dyn in net.get_dynamixels():
print dyn.id
self.settings['servoIds'].append(dyn.id)
# Make sure we actually found servos
if not self.settings['servoIds']:
print 'No Dynamixels Found!'
sys.exit(0)
# Save the output settings to a yaml file
with open(settingsFile, 'w') as fh:
yaml.dump(self.settings, fh)
print(
"Your settings have been saved to 'settings.yaml'. \nTo " +
"change them in the future either edit that file or run " +
"this example with -c.")
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=self.settings['port'],
baudrate=self.settings['baudRate'],
timeout=1)
# Instantiate our network object
self.net = dynamixel.DynamixelNetwork(serial)
# Populate our network with dynamixel objects
for servoId in self.settings['servoIds']:
newDynamixel = dynamixel.Dynamixel(servoId, self.net)
self.net._dynamixel_map[servoId] = newDynamixel
def main(settings):
# Open Hubo-Ach feed-forward and feed-back (reference and state) channels
# s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
# e = ach.Channel(ha.HUBO_CHAN_ENC_NAME)
# r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
#s.flush()
#r.flush()
# feed-forward will now be refered to as "state"
# state = ha.HUBO_STATE()
# encoder channel will be refered to as "encoder"
# encoder = ha.HUBO_ENC()
# feed-back will now be refered to as "ref"
# ref = ha.HUBO_REF()
# Get the current feed-forward (state)
# [statuss, framesizes] = s.get(state, wait=False, last=True)
# [statuss, framesizes] = e.get(encoder, wait=False, last=True)
enable()
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Before search you must enable the dyn
# This is untested but should turn the actuators on (i.e. the lights should blink once)
### enable(serial,net)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 50
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
# Set servo pos to 0.0
while True:
# Get the current feed-forward (state)
# [statuss, framesizes] = s.get(state, wait=False, last=True)
for actuator in myActuators:
actuator.goal_position = rad2dyn(0.0) # set all ids in range to 0.0 rad
if ( actuator.id == 5):
actuator.goal_position = rad2dyn(-3.14/2.0) # set id 5 to -pi/2 rad
if ( actuator.id == 6):
actuator.goal_position = rad2dyn(3.14/2.0) # set id 6 to pi/2 rad
if ( actuator.id == 3):
actuator.goal_position = rad2dyn(3.14/4.0) # set id 3 to pi/4 rad
if(actuator.id == 4):
actuator.goal_position = rad2dyn(-3.14/4.0) # set id 4 to -pi/4 rad
# send to robot
net.synchronize()
#!/usr/bin/env python2
# Import the dynamixel library
import dynamixel
# Import the time library for it's sleep functionality
import time
# Open a connection to the USB2Dynamixel device
serial = dynamixel.SerialStream(port="/dev/ttyUSB0",
baudrate=1000000,
timeout=1)
# Configure a DynamixelNetwork
net = dynamixel.DynamixelNetwork(serial)
# Configure the motor servo actuator
actuator = dynamixel.Dynamixel(1, net)
net._dynamixel_map[1] = actuator
# Configure the actuator such that it is in free running mode, and at a standstill
# The speed of the motor is configured in steps
# The values 0-1023 sets the motor to rotate clockwise at the given speed (1023 max)
# The values 1024-2047 sets the motor to rotate counterclockwise at the given speed - 1024
# The value 1024 represents stopped
actuator.moving_speed = 1024
# Enable torque for the motor
actuator.torque_enable = True
# Set the maximum torque to maximum
actuator.torque_limit = 1023
actuator.max_torque = 1023
# Set the goal position to 512 to enable free running mode
actuator.goal_position = 512
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
print actuator
actuator.moving_speed = 250
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
# Randomly vary servo position within a small range
print myActuators
print "PT server active"
# print "Servo \tPosition"
while True:
[statuss, framesizes] = s.get(state, wait=False, last=False)
for actuator in myActuators:
if (actuator.id == 1):
actuator.goal_position = rad2dyn(state.pan / 180.0 * np.pi)
#actuator.goal_position = rad2dyn(state.joint[ha.NKY].ref)
if (actuator.id == 2):
actuator.goal_position = rad2dyn(state.joint[ha.NK1].ref)
if (actuator.id == 3):
actuator.goal_position = rad2dyn(state.joint[ha.NK2].ref)
if (actuator.id == 5):
actuator.goal_position = rad2dyn(-state.tilt / 180.0 * np.pi)
print state.pan, ' - ', state.tilt
net.synchronize()
for actuator in myActuators:
actuator.read_all()
time.sleep(0.01)
# if ( actuator.id == 1):
# encoder.enc[ha.NKY] = dyn2rad(actuator.current_position)
# if ( actuator.id == 2):
# encoder.enc[ha.NK1] = dyn2rad(actuator.current_position)
# if ( actuator.id == 3):
# encoder.enc[ha.NK2] = dyn2rad(actuator.current_position)
# print encoder.enc[ha.NKY], " : ", encoder.enc[ha.NK1], " : ", encoder.enc[ha.NK2]
time.sleep(0.02)
import dynamixel import sys import time serial = dynamixel.SerialStream(port=sys.argv[1], baudrate=1000000, timeout=1) net = dynamixel.DynamixelNetwork(serial) pan = dynamixel.Dynamixel(1, net) net._dynamixel_map[1] = pan tilt = dynamixel.Dynamixel(2, net) net._dynamixel_map[2] = tilt pan.torque_enable = True tilt.torque_enable = True pan.moving_speed = 100 tilt.moving_speed = 100 pan.goal_position = 512 tilt.goal_position = 512 net.synchronize()
def initServos(self):
# Set your serial port accordingly.
if os.name == "posix":
possibilities = ['/dev/ttyUSB0', '/dev/ttyUSB1']
portName = None
for pos in possibilities:
if os.path.exists(pos):
portName = pos
if portName is None:
raise Exception('Could not find any of %s' % repr(possibilities))
else:
portName = "COM11"
serial = dynamixel.SerialStream(port = portName,
baudrate = self.baudRate,
timeout = 1)
self.net = dynamixel.DynamixelNetwork(serial)
#print 'Prescan...'
#print self.net.get_dynamixels()
print "Scanning for Dynamixels...",
self.net.scan(min(self.expectedIds), max(self.expectedIds))
self.actuators = []
self.actuatorIds = []
for dyn in self.net.get_dynamixels():
print dyn.id,
self.actuatorIds.append(dyn.id)
self.actuators.append(self.net[dyn.id])
print "...Done"
if len(self.actuators) != self.nServos and not self.silentNetFail:
raise RobotFailure('Expected to find %d servos on network, but only got %d (%s)'
% (self.nServos, len(self.actuators), repr(self.actuatorIds)))
for actuator in self.actuators:
#actuator.moving_speed = 90
#actuator.synchronized = True
#actuator.torque_enable = True
#actuator.torque_limit = 1000
#actuator.max_torque = 1000
actuator.moving_speed = 250
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 1000
actuator.max_torque = 1000
actuator.ccw_compliance_margin = 3
actuator.cw_compliance_margin = 3
self.net.synchronize()
#print 'options are:'
#for op in dir(self.actuators[0]):
# print ' ', op
#for ac in self.actuators:
# print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load
# ac.read_all()
self.currentPos = None
self.resetClock()
# Convert the left range into a 0-1 range (float)
valueScaled = float(value - leftMin) / float(leftSpan)
# Convert the 0-1 range into a value in the right range.
return rightMin + (valueScaled * rightSpan)
def translate_to_dynamixel(value):
k = translate(value, -150, 150, 0, 1023)
return int(k)
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port='/dev/ttyUSB0',
baudrate='1000000',
timeout=2)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
Dynamixel1 = dynamixel.Dynamixel(1, net)
Dynamixel2 = dynamixel.Dynamixel(2, net)
Dynamixel3 = dynamixel.Dynamixel(3, net)
Dynamixel4 = dynamixel.Dynamixel(4, net)
Dynamixel5 = dynamixel.Dynamixel(5, net)
net._dynamixel_map[1] = Dynamixel1
net._dynamixel_map[2] = Dynamixel2
net._dynamixel_map[3] = Dynamixel3
net._dynamixel_map[4] = Dynamixel4
net._dynamixel_map[5] = Dynamixel5
def refresh_net(data):
for i in xrange(len(net.get_dynamixels())):
actId = int(data.name[i])
net[actId].moving_speed = int(data.velocity[i])
net[actId].torque_limit = int(data.effort[i])
net[actId].goal_position = int(data.position[i])
net.synchronize()
if __name__ == '__main__':
try:
rospy.init_node('network_node')
# Initializing the serial port connection
serial = dynamixel.SerialStream(port=rospy.get_param("~port"),
baudrate=rospy.get_param("~baudRate"),
timeout=1)
# Initializing the dynamixel network
net = dynamixel.DynamixelNetwork(serial)
# Populating the created network with specified servo ids
for servoId in rospy.get_param("~servoIds"):
newDynamixel = dynamixel.Dynamixel(servoId, net)
net._dynamixel_map[servoId] = newDynamixel
sub = rospy.Subscriber('cmd_pos', JointState, refresh_net)
rospy.spin()
except rospy.ROSInterruptException:
pass
# Servo ID
highestServoId = None
while not highestServoId:
hsiTest = raw_input(
"Please enter the highest ID of the connected servos: ")
highestServoId = validateInput(hsiTest, 1, 255)
settings['highestServoId'] = highestServoId
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = dynamixel.SerialStream(port=settings['port'],
baudrate=settings['baudRate'],
timeout=1)
# Instantiate our network object
net = dynamixel.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
settings['servoIds'] = []
print "Found the following Dynamixels IDs: "
for dyn in net.get_dynamixels():
print dyn.id
settings['servoIds'].append(dyn.id)
# Make sure we actually found servos
'--id',
dest='id',
type=int,
help='the AX12 id [default=%(default)s]',
default=1)
parser.add_argument('-b',
'--baudrate',
dest='baudrate',
type=int,
help='the bus speed [default=%(default)s]',
default=1000000)
args = parser.parse_args()
serial = dynamixel.SerialStream(port=args.port,
baudrate=args.baudrate,
timeout=1)
net = dynamixel.DynamixelNetwork(serial)
net.scan(args.id, args.id)
ax12 = net[args.id]
if ax12:
ax12.read_all()
# list the registers sorted by their num
for regid, regnum, regdesc in sorted(dynamixel.defs.REGISTER.items(),
key=lambda x: x[1]):
regvalue = ax12.cache[regnum]
if regid in encoded_regs:
s = decode_reg_value(regid, regvalue)
else:
