Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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 ()
Ejemplo n.º 3
0
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)
Ejemplo n.º 4
0
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 ()
Ejemplo n.º 5
0
	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)
Ejemplo n.º 6
0
    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
Ejemplo n.º 8
0
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)
Ejemplo n.º 9
0
"""
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)
Ejemplo n.º 10
0
    :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)
Ejemplo n.º 11
0
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)
Ejemplo n.º 12
0
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!')
Ejemplo n.º 13
0
    :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