Exemplo n.º 1
0
	def setPosition(self, goal):
		m = [x for x in self.motorParams if x.name == goal.name]
		if len(m)>0:
			position=(goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618)
			pos=np.ushort(position)
			dynamixel.set_position(self.bus, (m[0].busId),pos)
			dynamixel.send_action_packet(self.bus)
Exemplo n.º 2
0
 def ComprobarLista2(self):
     if len(self.L_L_Goals) == 0:
         return
     with QtCore.QMutexLocker(self.mutex_bus):
         try:
             for x in range(0, len(self.L_L_Goals)):
                 with QtCore.QMutexLocker(self.mutex_goals):
                     m = self.L_L_Goals.pop(0)
                 for goal in m:
                     for x in self.motorParams:
                         if x.name == goal.name:
                             busId = x.busId
                             break
                     if x.invertedSign == "true":
                         goal.position = -goal.position
                     pos = np.ushort((goal.position + 2.618) * (1023 - 0) /
                                     (2.618 + 2.618))
                     vel = np.ushort(goal.maxSpeed)
                     dynamixel.set_velocity(self.bus,
                                            busId,
                                            vel,
                                            False,
                                            num_error_attempts=1)
                     dynamixel.set_position(self.bus,
                                            busId,
                                            pos,
                                            False,
                                            num_error_attempts=1)
                 dynamixel.send_action_packet(self.bus)
         except Ice.Exception, e:
             traceback.print_exc()
             print e
Exemplo n.º 3
0
 def setPosition(self, goal):
     m = [x for x in self.motorParams if x.name == goal.name]
     if len(m) > 0:
         position = (goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618)
         pos = np.ushort(position)
         dynamixel.set_position(self.bus, (m[0].busId), pos)
         dynamixel.send_action_packet(self.bus)
Exemplo n.º 4
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 ()
Exemplo n.º 5
0
	def ComprobarLista(self):	
		
		if len(self.lisPos) == 0:
			return
		try:
			m = self.lisPos.popleft()
			for x in self.motorParams:
				if x.name == m.name:
					busId = x.busId
					break
			pos = np.ushort((m.position + 2.618) * (1023 - 0) / (2.618 + 2.618))
			vel = np.ushort(m.maxSpeed)
			dynamixel.set_velocity(self.bus,busId, vel)
			dynamixel.set_position(self.bus, busId, pos)
			dynamixel.send_action_packet(self.bus)
		except Ice.Exception, e:
			traceback.print_exc()
			print e
Exemplo n.º 6
0
 def ComprobarLista(self):
     if len(self.lisPos) == 0:
         return
     try:
         m = self.lisPos.popleft()
         for x in self.motorParams:
             if x.name == m.name:
                 busId = x.busId
                 break
         pos = np.ushort(
             (m.position + 2.618) * (1023 - 0) / (2.618 + 2.618))
         vel = np.ushort(m.maxSpeed)
         dynamixel.set_velocity(self.bus, busId, vel)
         dynamixel.set_position(self.bus, busId, pos)
         dynamixel.send_action_packet(self.bus)
     except Ice.Exception, e:
         traceback.print_exc()
         print e
Exemplo n.º 7
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)
Exemplo n.º 8
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 ()
Exemplo n.º 9
0
	def ComprobarLista2(self):
		if len(self.L_L_Goals) == 0:
			return
		with QtCore.QMutexLocker(self.mutex_bus):
			try:
				for x in range(0, len(self.L_L_Goals)):
					m = self.L_L_Goals.pop(0)
					for goal in m:
						for x in self.motorParams:
							if x.name == goal.name:
								busId = x.busId
								break
						if x.invertedSign == "true":
							goal.position = -goal.position
						pos = np.ushort((goal.position + 2.618) * (1023 - 0) / (2.618 + 2.618))
						vel = np.ushort(goal.maxSpeed)
						dynamixel.set_velocity(self.bus, busId, vel, False, num_error_attempts=1)
						dynamixel.set_position(self.bus, busId, pos, False, num_error_attempts=1)
					dynamixel.send_action_packet(self.bus)
			except Ice.Exception, e:
				traceback.print_exc()
				print e
Exemplo n.º 10
0
	def compute(self):
		x=len(self.lisPos)
		y=len(self.lisVel)
		if(x!=0 or y!=0):
			self.timer.timeout=0
			try:
				if(x!=0):
					m=self.lisPos.popleft()
					busId=self.motores[m.name].busId

					pos=mapear(m.position, -1,1, 0,1023)
					dynamixel.set_position(self.ser, busId, pos)
					dynamixel.send_action_packet(self.ser)
				if(y!=0):
					m=self.lisVel.popleft()
					busId=self.motores[m.name].busId

					vel=mapear(m.velocity, 0,1, 0,1023)
					dynamixel.set_velocity(self.ser, busId, vel)
					dynamixel.send_action_packet(self.ser)
			except Ice.Exception, e:
				traceback.print_exc()
				print e
def multi_init_pos(t_final_sleep=2, t_sleep=0.05):
    """
    Initialize snake position

    :param t_init_sleep: pause time before start change goal position values.
    :param t_sleep: pause time between goal position settings in each servo.
    """
    # init goal position vector
    global goal_pos_vect
    goal_pos_vect = [
        int(round(offset + amplitude_norm * sin(omega * n)))
        for n in range(n_servo)
    ]
    # 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):
        sleep(t_sleep)
        dynamixel.set_position(ser, servo_id[n], goal_pos_vect[n])

    sleep(t_final_sleep)
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
servo_id = 1
target_position = 768  # (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

try:
    ser = dynamixel.get_serial_for_url(serial_port)

    if first_move == True:
        dynamixel.init(ser, servo_id)

    dynamixel.set_position(ser, servo_id, target_position)
    dynamixel.send_action_packet(ser)

    print('Success!')

except Exception as e:
    print('Unable to move to desired position.')
    print(e)
Exemplo n.º 13
0
# If this is the first time the robot was powered on, we need to read 
# and set the current position.
# (See the documentation above.)
first_move = True

try:
    ser = dynamixel.get_serial_for_url(serial_port)
    
    # Turn the LED off
    dynamixel.set_led(ser, servo_id, registers.LED_STATE.OFF)
    
    if first_move == True:
        dynamixel.init(ser, servo_id)
    
    # Set the desired position
    dynamixel.set_position(ser, servo_id, target_position)
    
    # Set the velocity
    dynamixel.set_velocity(ser, servo_id, velocity)
    
    # Move to the desired position
    dynamixel.send_action_packet(ser)
    
    # Wait for the arm to stop moving
    print('Waiting...')
    
    # Loop until the robot is done moving.
    while True:
        if dynamixel.get_is_moving(ser, servo_id) == False:
            break
            
Exemplo n.º 14
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)
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
Exemplo n.º 16
0
def update_dynamixel(id, val, velocity=10):
    dyn_raw.set_velocity(dyn_serial, id, velocity, verbose=True)
    dyn_raw.set_position(dyn_serial, id, val, verbose=True)
    dyn_raw.send_action_packet(dyn_serial)