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