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)
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
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)
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 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
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
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 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
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)
# 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
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
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)