def init_dynamixel_serial(port):
global dyn_serial
try:
dyn_serial = dyn_raw.get_serial_for_url(port)
dyn_serial.baudrate = 117647
except dyn_raw.DynamixelException as e:
print(e)
def home():
serial_port = '/dev/ttyUSB0'
servo1_id = 1
servo2_id = 2
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_position(ser, servo1_id, 3071)
dynamixel.set_position(ser, servo2_id, 1010)
dynamixel.send_action_packet(ser)
tlm.zap(0)
mighty.CloseMightyZap
return ()
def goToPosition(position):
servo_id1 = 1
servo_id2 = 2
try:
ser = dynamixel.get_serial_for_url(serial_port)
except:
print "failed to get motor driver"
print
velocity = 1 # slow, 4.266 seconds
dynamixel.set_position(ser, servo_id2,
int(position['TU']['LR'] * 1.0 * 1023 / 360 + 512))
dynamixel.set_position(ser, servo_id1,
int(position['TU']['UD'] * 1.0 * 1023 / 360 + 512))
dynamixel.send_action_packet(ser)
while dynamixel.get_is_moving(ser, servo_id1) == True:
velocity += velocity
dynamixel.set_velocity(ser, servo_id2, velocity)
dynamixel.set_velocity(ser, servo_id1, velocity)
dynamixel.send_action_packet(ser)
def move(sv1, sv2):
serial_port = '/dev/ttyUSB0'
servo1_id = 1
servo2_id = 2
k = 0
i = 0
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_velocity(ser, servo1_id, 70)
dynamixel.set_velocity(ser, servo2_id, 125)
while (i < len(sv1)):
if mt.isnan(sv1[k]) == False and mt.isnan(sv2[k]) == False:
servoPos1 = int(sv1[k])
servoPos2 = int(sv2[k])
dynamixel.set_position(ser, servo1_id, servoPos1)
dynamixel.set_position(ser, servo2_id, servoPos2)
dynamixel.send_action_packet(ser)
if k == 0:
t.sleep(0.5)
tlm.zap1(0)
t.sleep(2)
tlm.zap(0)
t.sleep(0.5)
ismoving = 1
while (ismoving == 1):
move_1 = dynamixel.get_is_moving(ser, servo1_id)
move_2 = dynamixel.get_is_moving(ser, servo2_id)
if move_1 == False or move_2 == False:
ismoving = 0
t.sleep(1)
tlm.zap(
0.6) # the variable argument is actuation length in inches
t.sleep(2)
tlm.zap(0)
t.sleep(0.75)
print('Success')
i = i + 1
k = k + 1
return ()
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.mutmState=mutex()
self.busParams=BusParams()
self.motores={}
self.SDK=False
self.lisPos=collections.deque()
self.lisVel=collections.deque()
self.mstateMap ={}
self.setParams()
# try:
self.ser = dynamixel.get_serial_for_url(self.busParams.device,self.busParams.baudRate)
for m in self.motores:
dynamixel.init(self.ser,m.busId)
# except Exception as e:
# print('Unable to move to desired position.')
# print e
self.timer.timeout.connect(self.compute)
self.Period = 100
self.timer.start(self.Period)
def setParams(self, params):
i = 0
with open(
"/home/odroid/robocomp/components/hexapod-robot/dynamixelpython/etc/config",
"r") as f:
for linea in f.readlines():
if "Device" in linea:
sep = linea.split("=")
self.serial_port = sep[1].replace("\n", "")
else:
self.serial_port = '/dev/ttyUSB0'
#if "NumMotors" in linea:
#separacion = linea.split("=")
#motorParams = separacion[1]
if "Params_" in linea:
separacion = linea.split("=")
#Buscando ID
parametros = separacion[1].split(",")
name = parametros[0]
param = MotorParams()
param.name = name
param.busId = int(parametros[1])
param.invertedSign = parametros[2]
param.minPos = float(parametros[3])
param.maxPos = float(parametros[4])
param.zero = parametros[5]
param.maxVel = parametros[6]
param.stepsRev = parametros[7]
param.maxDegrees = float(parametros[8].replace("\n", ""))
self.motorParams.append(param)
self.bus = dynamixel.get_serial_for_url(self.serial_port)
return True
def setParams(self, params):
i = 0
with open("/home/odroid/RoboticaAvanzada/dynamixelpython/etc/config",
"r") as f:
for linea in f.readlines():
if "Device" in linea:
sep = linea.split("=")
self.serial_port = sep[1].replace("\n", "")
else:
self.serial_port = '/dev/ttyUSB0'
#if "NumMotors" in linea:
#separacion = linea.split("=")
#motorParams = separacion[1]
if "Params_" in linea:
separacion = linea.split("=")
#Buscando ID
parametros = separacion[1].split(",")
name = parametros[0]
param = MotorParams()
param.name = name
param.busId = int(parametros[1])
param.invertedSign = parametros[2]
param.minPos = float(parametros[3])
param.maxPos = float(parametros[4])
param.zero = parametros[5]
param.maxVel = parametros[6]
param.stepsRev = parametros[7]
param.maxDegrees = float(parametros[8].replace("\n", ""))
self.motorParams.append(param)
self.bus = dynamixel.get_serial_for_url(self.serial_port)
return True
from pydynamixel import dynamixel, registers
# You'll need to change this to the serial port of your USB2Dynamixel
serial_port = '/dev/tty.usbserial-A921X77J'
# You'll need to change this to the ID of your servo
servo_id = 9
# Turn the LED on
led_value = dynamixel.registers.LED_STATE.ON
try:
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_led(ser, servo_id, led_value)
print('LED set successfully!')
except Exception as e:
print('Unable to set LED.')
print(e)
"""
The following code can be used to turn on the LED on a connected servo
(on a POSIX-compliant platform.)
"""
from pydynamixel import dynamixel, registers
# You'll need to change this to the serial port of your USB2Dynamixel
serial_port = '/dev/tty.usbserial-A921X77J'
# You'll need to change this to the ID of your servo
servo_id = 9
# Turn the LED on
led_value = registers.LED_STATE.ON
try:
ser = dynamixel.get_serial_for_url(serial_port)
dynamixel.set_led(ser, servo_id, led_value)
print('LED set successfully!')
except Exception as e:
print('Unable to set LED.')
print(e)
:param joints: A list of servo IDs.
:param verbose: If True, status information will be printed. Default: ``VERBOSE``.
:param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.
:returns: ``None``
"""
# Clear any data in the serial buffer
dynamixel.flush_serial(ser)
s = 'Press <ENTER> to display current position. Use q<ENTER> to quit.'
while True:
i = raw_input(s)
if i == 'q':
sys.exit(0)
vector = chain.read_position(ser, joints, verbose, num_error_attempts)
s = str(vector)
if __name__ == '__main__':
url = '/dev/tty.usbserial-A9SFBTPX'
ser = dynamixel.get_serial_for_url(url)
verbose = False
joints = [1, 2, 3, 4, 5, 6, 7]
num_error_attempts = 10
display_position(ser, joints, num_error_attempts)
def init_snake(
serial_port='/dev/ttyUSB0',
servo_id=[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12],
n_period=1,
id_bloque=0, # pas de servo bloqué par défaut
angle_bloque=512,
amplitude=300,
offset=512,
t_final_sleep=2,
t_sleep=0.05):
"""
Initialize snake position for individual testing
:param serial_port: The ``serial`` object to use.
:param servo_id: the list of servo IDs.
:param n_period: number of "period" in the snake mouvement shape.
:param id_bloque: servo id of the servo which simulate a breakage.
:param angle_bloque: angle of the servo which simulate a breakage.
:param amplitude: amplitude of snake mouvement shape.
:param offset: curvature of snake mouvement shape.
:param t_final_sleep: pause time after the end of the init.
:param t_sleep: pause time between settings in each servo.
"""
ser = dynamixel.get_serial_for_url(serial_port)
n_servo = len(servo_id)
# pré-calculs
amplitude_norm = amplitude * (float(n_period) / n_servo) * 1024. / 300.
omega = 2 * pi * n_period / float(n_servo)
# enable write response for all instructions ()# gestion servo bloqué
multi_set_status_return(ser, servo_id,
registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS)
# Set velocity for init
multi_set_velocity(ser, servo_id, 200)
goal_pos_vect = [
int(round(offset + amplitude_norm * sin(omega * n)))
for n in range(n_servo)
]
# gestion servo bloqué
if (id_bloque != 0):
goal_pos_vect[servo_id.index(id_bloque)] = angle_bloque
# Angle safe check (can slow execution)
assert abs(max(goal_pos_vect) - 512
) < 1024. / 300. * ANGLE_MAX, "Angle max (={}°) dépassé".format(
ANGLE_MAX)
"""
for n in range(n_servo):
sum = 0
for z in range(n,n_servo):
sum += (goal_pos_vect[z]-512)
assert sum < 1024./300.*200. , "Le serpent allait se mordre la queue"
"""
# Move the snake in init position
for n in range(n_servo):
sleep(t_sleep)
dynamixel.set_position(ser, servo_id[n], goal_pos_vect[n])
sleep(t_final_sleep)
# Set velocity for movement
multi_set_velocity(ser, servo_id, 400)
# disable write response for read instructions
multi_set_status_return(ser, servo_id,
registers.STATUS_RETURN.RETURN_ONLY_FOR_READ)
def move_snake(
serial_port='/dev/ttyUSB0',
servo_id=[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12],
tick_period=0.01,
nb_tick=900,
resolution=300,
n_period=1,
id_bloque=0, # pas de servo bloqué par défaut
amplitude=300,
offset=512):
"""
Do the snake mouvement for individual testing
:param serial_port: The ``serial`` object to use.
:param servo_id: the list of servo IDs.
:param tick_period: period of the mouvement management loop.
:param nb_tick: number of iteration of the mouvement management loop.
:param resolution: resolution for snake mouvement sampling.
If resolution increase, mouvement speed decrease.
:param n_period: number of "period" in the snake mouvement shape.
:param id_bloque: servo id of the servo which simulate a breakage.
:param amplitude: amplitude of snake mouvement shape.
:param offset: curvature of snake mouvement shape.
"""
ser = dynamixel.get_serial_for_url(serial_port)
move = -1
n_servo = len(servo_id)
goal_pos_vect = []
# pré-calculs
mvt_speed = 2 * pi * 1. / resolution
amplitude_norm = amplitude * (float(n_period) / n_servo) * 1024. / 300.
omega = 2 * pi * n_period / float(n_servo)
t = time()
tick = 0
for i in range(1, nb_tick):
tick += move
if (goal_pos_vect != []):
for n in range(n_servo):
# gestion servo bloqué
if (servo_id[n] == id_bloque):
continue
dynamixel.set_position_no_response(ser, servo_id[n],
goal_pos_vect[n])
#print " ", time()-t # DEBUG
#print " ", time()-t # DEBUG
# compute next goal position vector
goal_pos_vect = [
int(
round(offset +
amplitude_norm * sin(omega * n + tick * mvt_speed)))
for n in range(n_servo)
]
# ne pas recréer la liste mais réécrire dedans pour opti ???
# Angle safe check (can slow execution)
assert abs(
max(goal_pos_vect) - offset
) < 1024. / 300. * ANGLE_MAX, "Angle max (={}°) dépassé".format(
ANGLE_MAX)
"""
for n in range(n_servo):
sum = 0
for z in range(n,n_servo):
sum += (goal_pos_vect[z]-512)
assert sum < 1024./300.*200. , "Le serpent allait se mordre la queue"
"""
# Gestion control serpent
"""
for event in pygame.event.get(): #Attente des événements
# if event.type == QUIT:
# continuer = 0
if (event.type == KEYDOWN):
if (event.key == K_UP ):
move = min( move+1, 1)
elif (event.key == K_DOWN ):
move = max( move-1, -1)
elif (event.key == K_LEFT ):
if (offset > 460):
offset -= 5
elif (event.key == K_RIGHT):
if (offset < 564):
offset += 5
textsurface = myfont.render("OFFSET = {} ".format(offset), False, (255, 255, 255), (0,0,0) )
fenetre.blit(textsurface,(0,0))
if (move== 0):
textsurface = myfont.render("DON'T MOVE ", False, (255, 255, 255), (0,0,0) )
elif (move== 1):
textsurface = myfont.render("MOVE FORWARD ", False, (255, 255, 255), (0,0,0) )
elif (move==-1):
textsurface = myfont.render("MOVE BACKWARD ", False, (255, 255, 255), (0,0,0) )
fenetre.blit(textsurface,(0,30))
pygame.display.flip()
"""
# Wait next tick
while (time() < t + i * tick_period): # tick => i
pass
#if ( tick%10 == 0 ): #
#sleep(sleep_time) # utile si tick_period>0.001 ?
# print( time()-t ) # DEBUG
print('Success!')
:param ser: The ``serial`` port to use.
:param joints: A list of servo IDs.
:param verbose: If True, status information will be printed. Default: ``VERBOSE``.
:param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.
:returns: ``None``
"""
# Clear any data in the serial buffer
dynamixel.flush_serial(ser)
s = 'Press <ENTER> to display current position. Use q<ENTER> to quit.'
while True:
i = raw_input(s)
if i == 'q':
sys.exit(0)
vector = chain.read_position(ser, joints, verbose, num_error_attempts)
s = str(vector)
if __name__ == '__main__':
url = '/dev/tty.usbserial-A9SFBTPX'
ser = dynamixel.get_serial_for_url(url)
verbose = False
joints = [1,2,3,4,5,6,7]
num_error_attempts = 10
display_position(ser, joints, num_error_attempts)
from pydynamixel import dynamixel
# You'll need to change this to the serial port of your USB2Dynamixel
serial_port = "/dev/ttyUSB0"
# You'll need to modify these for your setup
target_position = 512 # (range: 0 to 1023)
# If this is the first time the robot was powered on,
# you'll need to read and set the current position.
# (See the documentation above.)
first_move = True
ser = dynamixel.get_serial_for_url(serial_port, baudrate=1000000)
for i in range(1, 19):
dynamixel.init(ser, i)
for i in range(1, 19):
try:
p = dynamixel.get_position(ser, i, num_error_attempts=10)
print "motor", i, "pos", p
dynamixel.set_position(ser, i, 512)
dynamixel.send_action_packet(ser)
# print('Success!')
except Exception as e:
# print('Unable to move to desired position.')
# print(e)
pass