Пример #1
0
	def setVelocity(self, goal):
		m = [x for x in self.motorParams if x.name == goal.name]
		if len(m)>0:
			
			pos=np.ushort(goal.velocity)
			dynamixel.set_velocity(self.bus, (m[0].busId),pos)
			dynamixel.send_action_packet(self.bus)
Пример #2
0
def multi_set_velocity(ser, servo_id, v, t_init_sleep=0.1, t_sleep=0.05):
    """
    Set velocity of the servos. Need status return enable

    :param ser: The ``serial`` port to use.
    :param servo_id: the list of servo IDs.
    :param v: value of velocity to set.
              v can be a value or a list.
    :param t_init_sleep: pause time before start change reg values.
    :param t_sleep: pause time between settings in each servo.
    """
    n_servo = len(servo_id)

    if (type(v) == int):
        velocity = [v for i in range(n_servo)]
    elif (type(v) == list):
        if (len(v) == n_servo):
            raise ValueError("lenght servo_id and lenght v are not equal")
        velocity = v
    else:
        raise TypeError("v need to be an int or a list")

    sleep(t_init_sleep)
    for n in range(n_servo):
        dynamixel.set_velocity(ser, servo_id[n], velocity[n])
        sleep(t_sleep)
Пример #3
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
Пример #4
0
    def setVelocity(self, goal):
        m = [x for x in self.motorParams if x.name == goal.name]
        if len(m) > 0:

            pos = np.ushort(goal.velocity)
            dynamixel.set_velocity(self.bus, (m[0].busId), pos)
            dynamixel.send_action_packet(self.bus)
Пример #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
Пример #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
Пример #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)
Пример #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 ()
Пример #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
Пример #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
Пример #11
0
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
            
        time.sleep(0.1) # Sleep for a short amount of time (100 ms)
        
    # Done moving
Пример #12
0
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

        time.sleep(0.1)  # Sleep for a short amount of time (100 ms)

    # Done moving
# enable write response
for n in range(n_servo):
    reg_value = registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS
    reg_addr = registers.STATUS_RETURN_LEVEL  # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
    # goal position est sur 2 bytes !!!!
    dynamixel.set_reg_1b(ser, servo_id[n], reg_addr, reg_value,
                         False)  # revoie pas de reponse
    print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value))

### Velocity settings ###
v = 400
velocity = [v for i in range(n_servo)]
sleep(0.1)
for n in range(n_servo):
    dynamixel.set_velocity(ser, servo_id[n], velocity[n])
    #sleep(0.1)

# disable write response
for n in range(n_servo):
    reg_value = registers.STATUS_RETURN.RETURN_ONLY_FOR_READ
    reg_addr = registers.STATUS_RETURN_LEVEL  # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
    # goal position est sur 2 bytes !!!!
    dynamixel.set_reg_1b(ser, servo_id[n], reg_addr,
                         reg_value)  # revoie une reponse
    print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value))

# init position
goal_pos_vect = [
    int(round(offset + amplitude_norm * cos(omega * n)))
    for n in range(n_servo)
Пример #14
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)