def main(): """ A PyAX-12 demo. Blink (only once) the LED of the specified Dynamixel unit. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(args.port, args.baudrate, args.timeout) # Switch ON the LED serial_connection.write_data(args.dynamixel_id, pk.LED, 1) # Wait 2 seconds time.sleep(2) # Switch OFF the LED serial_connection.write_data(args.dynamixel_id, pk.LED, 0) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Blink (only once) the LED of the specified Dynamixel unit. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Switch ON the LED serial_connection.write_data(args.dynamixel_id, pk.LED, 1) # Wait 2 seconds time.sleep(2) # Switch OFF the LED serial_connection.write_data(args.dynamixel_id, pk.LED, 0) # Close the serial connection serial_connection.close()
def main(): """ Set the *return delay time* for the specified Dynamixel unit i.e. the time (in microsecond) for the status packets to return after the instruction packet is sent. """ # Parse options parser = common_argument_parser(desc=main.__doc__) parser.add_argument("--return-delay-time", "-r", help="The new return delay time assigned to the " "selected Dynamixel unit. It must be in range " "(0, 500). The default value is {default}µs " "({default} microseconds).".format(default=DEFAULT_VALUE), type=int, metavar="INT", default=DEFAULT_VALUE) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) serial_connection.set_return_delay_time(args.dynamixel_id, args.return_delay_time) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Ping the specified Dynamixel unit. Returns "True" if the specified unit is available, "False" otherwise. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Ping the dynamixel unit(s) is_available = serial_connection.ping(args.dynamixel_id) print(is_available) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Ping the specified Dynamixel unit. Returns "True" if the specified unit is available, "False" otherwise. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Ping the dynamixel unit(s) is_available = serial_connection.ping(args.dynamixel_id) print(is_available) # Close the serial connection serial_connection.close()
def main(): """ Set the *ID* for the specified Dynamixel unit i.e. the unique ID number assigned to actuators for identifying them. Different IDs are required for each Dynamixel actuators that are on the same network. """ # Parse options parser = common_argument_parser(desc=main.__doc__) parser.add_argument("--new-id", "-n", help="The new unique ID assigned to the selected " "Dynamixel unit. It must be in range (0, 0xFE).", type=int, metavar="INT", default=1) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) serial_connection.set_id(args.dynamixel_id, args.new_id) # Close the serial connection serial_connection.close()
def main(): """Set the *baud rate* for the specified Dynamixel unit i.e. set the connection speed with the actuator in kbps (kilo bauds per second). """ # Parse options parser = common_argument_parser(desc=main.__doc__) parser.add_argument("--new-baudrate", "-n", help="the new baud rate assigned to the selected " "Dynamixel unit (in kbps)." "The default value is {default}kbps.".format(default=DEFAULT_VALUE), type=float, metavar="FLOAT", default=DEFAULT_VALUE) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) serial_connection.set_baud_rate(args.dynamixel_id, args.new_baudrate) # Close the serial connection serial_connection.close()
def main(*args): # Connect to the serial port serial_connection = Connection(port="/dev/ttyACM0", baudrate=100000, timeout=0.1, waiting_time=0.02) print("scanning") # Ping the dynamixel unit(s) ids_available = serial_connection.scan() print("Scanned") for dynamixel_id in ids_available: print("there's something here") counter = 0 while counter < 60: # Go to -45 (45 CW) serial_connection.goto(dynamixel_id, degrees_to_dxl_angle(counter), speed=1024, degrees=False) time.sleep(1) # Wait 1 second serial_connection.goto(dynamixel_id, degrees_to_dxl_angle(-(counter)), speed=768, degrees=False) time.sleep(1) counter += 1 # Close the serial connection serial_connection.close()
def main(): """ Set the *ID* for the specified Dynamixel unit i.e. the unique ID number assigned to actuators for identifying them. Different IDs are required for each Dynamixel actuators that are on the same network. """ # Parse options parser = common_argument_parser(desc=main.__doc__) parser.add_argument("--new-id", "-n", help="The new unique ID assigned to the selected " "Dynamixel unit. It must be in range (0, 0xFE).", type=int, metavar="INT", default=1) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) serial_connection.set_id(args.dynamixel_id, args.new_id) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Prints the ID list of available Dynamixel units. """ # Parse options parser = common_argument_parser(desc=main.__doc__, id_arg=False) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Ping the dynamixel unit(s) ids_available = serial_connection.scan() for dynamixel_id in ids_available: print(dynamixel_id) # Close the serial connection serial_connection.close()
class Emu(): """Klasse til at kontrollere Dynamixel AX12 actuators""" def start(self): """Metode til at finde USB-porten, fx COM12 på Win, ttyUSB0 på linux""" #self.sc = Connection(port="/dev/ttyUSB0", baudrate=57600) self.sc = Connection(port="/dev/ttyUSB0", baudrate=1000000) #self.sc = Connection(port="COM12", baudrate=1000000) self.sc.flush() def scanUnits(self): """Scanner dynamixel motorer og returnere deres id'er i en liste""" ids = self.sc.scan() return ids def readDxl(self, ID): """Printer oplysninger motoren med ID""" self.sc.flush() self.sc.pretty_print_control_table(ID) def jointMode(self, ID): """Skifter motoren med ID til joint mode""" self.sc.set_cw_angle_limit(ID, 0, False) self.sc.set_ccw_angle_limit(ID, 1023, False) def wheelMode(self, ID): """Skifter motoren med ID til wheelmode""" self.sc.set_ccw_angle_limit(ID, 0, False) self.sc.set_cw_angle_limit(ID, 0, False) def moveJoint(self, ID, position): """Flytter motoren med ID til position""" self.sc.goto(ID, position, speed=512, degrees=True) time.sleep(1) def moveWheel(self, ID, speed): """Starter en motor i wheelmode med hastigheden speed""" if speed < 0: if speed < -1024: speed = 2047 else: speed = 1023 + -speed else: if speed > 1023: speed = 1023 self.sc.flush() self.sc.goto(ID, 0, int(speed), degrees=False) def stop(self): """Lukker forbindelsen gennem USB-porten til dynamixlerne""" self.sc.close() def getPos(self, ID): """Returnere motoren med ID's position""" position = self.sc.get_present_position(ID, True) return position
def test_init_wrong_timeout_type_str(self): """Check that the pyax12.connection.Connection initialization fails when the "timeout" argument's type is wrong (string).""" port = None baudrate = 57600 timeout = "0.1" # wrong type with self.assertRaises(ValueError): serial_connection = Connection(port, baudrate, timeout) serial_connection.close()
def test_init_wrong_port_type(self): """Check that the pyax12.connection.Connection initialization fails when the "port" argument's type is wrong.""" port = False # wrong type (expected: str) baudrate = 57600 timeout = 0.1 with self.assertRaises(serial.serialutil.SerialException): serial_connection = Connection(port, baudrate, timeout) serial_connection.close()
def test_init_wrong_port_value(self): """Check that the pyax12.connection.Connection initialization fails when the "port" argument's value is wrong (/dev/null).""" port = '/dev/null' # wrong value baudrate = 57600 timeout = 0.1 with self.assertRaises(serial.serialutil.SerialException): serial_connection = Connection(port, baudrate, timeout) serial_connection.close()
def gotoJoint(self,p1,p2,p3,p4,p5): robot = Connection(port=self.port, baudrate=self.bps) time.sleep(0.5) robot.goto(self.motor_1,p1, speed=self.speed, degrees=True) robot.goto(self.motor_2,p2, speed=self.speed, degrees=True) robot.goto(self.motor_3,p3, speed=self.speed, degrees=True) robot.goto(self.motor_4,p4, speed=self.speed, degrees=True) robot.goto(self.motor_5,p5, speed=self.speed, degrees=True) time.sleep(0.5) robot.close()
def setReady(self): robot = Connection(port=self.port, baudrate=self.bps) time.sleep(0.5) robot.goto(self.motor_2,-60, speed=self.speed, degrees=True) time.sleep(1.5) robot.goto(self.motor_1,0, speed=self.speed, degrees=True) robot.goto(self.motor_3,-90, speed=self.speed, degrees=True) robot.goto(self.motor_4,-90, speed=self.speed, degrees=True) robot.goto(self.motor_5,45, speed=self.speed, degrees=True) time.sleep(0.5) robot.close()
def gotoHome(self): robot = Connection(port=self.port, baudrate=self.bps) robot.goto(self.motor_5, 45, speed=self.speed+20, degrees=True) time.sleep(1.5) robot.goto(self.motor_6,0, speed=self.speed, degrees=True) robot.goto(self.motor_3,-130, speed=self.speed, degrees=True) robot.goto(self.motor_2,-90, speed=self.speed, degrees=True) time.sleep(3) robot.goto(self.motor_1,-90, speed=self.speed, degrees=True) time.sleep(1) robot.goto(self.motor_4, -90, speed=self.speed, degrees=True) robot.close()
def main(): """ A PyAX-12 demo. Print the control table of the specified Dynamixel unit. """ # Parse options parser = common_argument_parser(desc=main.__doc__, id_arg_mandatory=True) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(args.port, args.baudrate, args.timeout) # Print the control table of the specified Dynamixel unit serial_connection.pretty_print_control_table(args.dynamixel_id) # Close the serial connection serial_connection.close()
def main(): """ This is an *endless turn mode* demo. If both values for the *CW angle limit* and *CCW angle limit* are set to 0, an *endless turn mode* can be implemented by setting the *goal speed*. This feature can be used for implementing a continuously rotating wheel. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) dynamixel_id = args.dynamixel_id # Set the "wheel mode" serial_connection.set_cw_angle_limit(dynamixel_id, 0, degrees=False) serial_connection.set_ccw_angle_limit(dynamixel_id, 0, degrees=False) # Activate the actuator (speed=512) serial_connection.set_speed(dynamixel_id, 512) # Lets the actuator turn 5 seconds time.sleep(5) # Stop the actuator (speed=0) serial_connection.set_speed(dynamixel_id, 0) # Leave the "wheel mode" serial_connection.set_ccw_angle_limit(dynamixel_id, 1023, degrees=False) # Go to the initial position (0 degree) serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Print the internal temperature (in degrees celsius) of the specified Dynamixel unit. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(args.port, args.baudrate, args.timeout) # Print the present internal temperature print(serial_connection.get_present_temperature(args.dynamixel_id)) # Close the serial connection serial_connection.close()
def moveAngle(thetar, V, dir): if thetar < -75: thetar = -75 if thetar > 75: thetar = 75 theta = thetar / 180 * math.pi if V > 0: dir = 0 elif V < 0: dir = 1 V = abs(V) if V > 200: V = 200 elif V < 0: V = 0 serial_connection = Connection(port='/dev/ttyUSB0', baudrate=1000000) tp = computeAngles(theta, V) pR = tp[0] pL = tp[1] vR = tp[2] vL = tp[3] serial_connection.goto(2, pR, speed=250, degrees=False) serial_connection.goto(4, pL, speed=250, degrees=False) #vR = 200 if vR>200 else vR #vR = 0 if vR<0 else vR #if vL>200: vL = 200; #if vL<0: vL=0; serial_connection.set_cw_angle_limit(1, 0) serial_connection.set_ccw_angle_limit(1, 0) #set to wheel mode - CW and CCW angles limits to zeros sets wheel mode serial_connection.write_data( 1, 32, pyax12.utils.int_to_little_endian_bytes(vR + 1024 * dir)) serial_connection.set_cw_angle_limit(3, 0) serial_connection.set_ccw_angle_limit(1, 0) serial_connection.write_data( 3, 32, pyax12.utils.int_to_little_endian_bytes(vL + 1024 * (1 - dir))) serial_connection.flush() serial_connection.close()
def main(): """ A PyAX-12 demo. Print the internal temperature (in degrees celsius) of the specified Dynamixel unit. """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Print the present internal temperature print(serial_connection.get_present_temperature(args.dynamixel_id)) # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Move the specified Dynamixel unit to 0° (0) then go to 300° (1023) then go back to 150° (511). """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) ### dynamixel_id = args.dynamixel_id # Go to 0° serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -45° (45° CW) serial_connection.goto(dynamixel_id, -45, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -90° (90° CW) serial_connection.goto(dynamixel_id, -90, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -135° (135° CW) serial_connection.goto(dynamixel_id, -135, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -150° (150° CW) serial_connection.goto(dynamixel_id, -150, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +150° (150° CCW) serial_connection.goto(dynamixel_id, 150, speed=512, degrees=True) time.sleep(2) # Wait 2 seconds # Go to +135° (135° CCW) serial_connection.goto(dynamixel_id, 135, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +90° (90° CCW) serial_connection.goto(dynamixel_id, 90, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +45° (45° CCW) serial_connection.goto(dynamixel_id, 45, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go back to 0° serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True) ### # Close the serial connection serial_connection.close()
#input value will be between 0 to 180, eg, info 180 = 90 in data #1data=1AngleInfo #AX12 Limits if Angle3 < -90: A3 = -90 elif Angle3 > 90: A3 = 90 else: A3 = Angle3 serial_connection.goto(dynamixel_id3, A3, speed=speed3, degrees=True) # #HomeConfiguration # serial_connection.goto(dynamixel_id1, 300, speed=speed1, degrees=True) # serial_connection.goto(dynamixel_id2, 30, speed=speed2, degrees=True) # serial_connection.goto(dynamixel_id3, 0, speed=speed3, degrees=True) # # #YesMotionFunction # serial_connection.goto(dynamixel_id1, 200, speed=speed1, degrees=True) # serial_connection.goto(dynamixel_id2, 30, speed=speed2, degrees=True) # serial_connection.goto(dynamixel_id2, 180, speed=speed2, degrees=True) # serial_connection.goto(dynamixel_id2, 30, speed=speed2, degrees=True) # # #NoMotionFuntion # serial_connection.goto(dynamixel_id2, 30, speed=speed2, degrees=True) # serial_connection.goto(dynamixel_id1, 200, speed=speed1, degrees=True) # serial_connection.goto(dynamixel_id1, 500, speed=speed1, degrees=True) # serial_connection.goto(dynamixel_id1, 200, speed=speed1, degrees=True) # Close the serial connection serial_connection.close()
def Graper(self,p6): robot = Connection(port=self.port, baudrate=self.bps) robot.goto(self.motor_6, p6, speed=self.speed, degrees=True) robot.close()
from pyax12.connection import Connection # Connect to the serial port # serial_connection = Connection(port="/dev/ttyUSB0", baudrate=57600) sc = Connection(port="/dev/ttyACM0", baudrate=1000000) ids = sc.scan() for id in ids: print(id) sc.close() input()
def Rotate_hnad(self,p7): robot = Connection(port=self.port, baudrate=self.bps) robot.goto(self.motor_5, p7, speed=self.speed, degrees=True) robot.close()
class DynamixelAX12(object): def __init__(self): # Connect to the serial port self.connection = Connection(port="/dev/ttyUSB0", baudrate=57600) self.dynamixel_x_axis_id = 3 self.dynamixel_z_axis_id = 4 # Setup the angle limits fot the X axis self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id, -90, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id, 90, degrees=True) # Setup the angle limits fot the Z axis self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id, -90, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id, 90, degrees=True) # Goto the initial position (-45°, 0°) self.connection.goto(self.dynamixel_x_axis_id, -45, speed=255, degrees=True) self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255, degrees=True) # TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms) # (+ memorize the original values to set them back) def __del__(self): # TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms) # Restore the default position (0°, 0°) self.connection.goto(self.dynamixel_x_axis_id, 0, speed=255, degrees=True) self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255, degrees=True) # Setup the angle limits for the X axis self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id, -150, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id, 150, degrees=True) # Setup the angle limits for the Z axis self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id, -150, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id, 150, degrees=True) self.connection.close() def apply_control(self, control_vect): pos_x = self.connection.get_present_position(self.dynamixel_x_axis_id) pos_z = self.connection.get_present_position(self.dynamixel_z_axis_id) print(pos_x, pos_z, control_vect) # The x_axis controls up/down movements new_pos_x = pos_x + int(control_vect[1] * 20.) # The z_axis controls left/right movements # Warning: movements are inverted on the z axis # (negative commands move the frame to the right) new_pos_z = pos_z - int(control_vect[0] * 20.) speed_x = abs(int(control_vect[1] * 1023.)) # 300 speed_z = abs(int(control_vect[0] * 1023.)) # 300 try: self.connection.goto(self.dynamixel_x_axis_id, new_pos_x, speed=speed_x) self.connection.goto(self.dynamixel_z_axis_id, new_pos_z, speed=speed_z) except AngleLimitError: print("Angle limit")
LSU = 6 # range -113 to 40 LSS = 7 # range -49 to 81 LER = 8 # range -90 to 98 LEU = 9 # range -133 to 86 HD = 10 # range -39 to 36 speed = 200 def send_pos(mid, deg): serial_connection.goto(mid, deg, speed, degrees=True) time.sleep(1) #` Wait 1 second def initall(): for dynamixel_id in range(1, 11): serial_connection.goto(dynamixel_id, 0, speed, degrees=True) #time.sleep(1) # Wait 1 second # Go to 0° ## POSE 1 #dynamixel_id = RSU #pos = -35 for mid in range(1, 11): pos = chain.get_present_position(mid, True) print(mid, " ", pos) # Close the serial connection chain.close()
class driverLidars: def __init__(self): self.lidars = [] self.serial_connection = [] self.areConnected = 0 def connect(self): cust_print('Connecting to lidars and servos...') self.lidars = [] self.serial_connection = [] try: # Connect to the serial port self.serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600, rpi_gpio=True) except: cust_print(' Cannot open serial connection for servos!') #return -1 for lidarNb in range(constants.nb_of_lidars): try: self.lidars.append(RPLidar(constants.serialPort[lidarNb],baudrate=115200)) self.lidars[lidarNb].connect() except: cust_print(' Cannot connect to the lidar' + str(lidarNb+1) + '!') return -1 cust_print(' Connected to lidar' + str(lidarNb+1)) try: # Try to ping the motor if self.serial_connection.ping(constants.servosIDs[lidarNb]) == False: raise self.serial_connection.set_speed(BROADCAST_ID,constants.servosSpeed) self.servos_goto(constants.servosIDs[lidarNb],0) except: cust_print(' Cannot connect to the servo' + str(lidarNb+1) + '!') return -1 cust_print(' Connected to servo' + str(lidarNb+1)) time.sleep(0.25) # To avoid a too high current drain self.areConnected = 1 return 0 def disconnect(self): cust_print('Disconnecting to lidars and servos...') self.serial_connection.close() cust_print(' Disconnected to servos serial') for lidarNb in range(constants.nb_of_lidars): self.lidars[lidarNb].stop() self.lidars[lidarNb].disconnect() time.sleep(0.25) # To avoid to drain too much current cust_print(' Disconnected to lidar' + str(lidarNb+1)) self.lidars[:] = [] self.areConnected = 0 def servos_goto(self,servosID,position): self.serial_connection.goto(servosID,position,degrees=True) def check_link_state(self): for lidarNb in range(constants.nb_of_lidars): try: self.lidars[lidarNb].stop() self.lidars[lidarNb].get_health() except: cust_print('Link error with lidar' + str(lidarNb+1)) self.disconnect() return -1 try: if self.serial_connection.ping(constants.servosIDs[lidarNb]) == False: raise except: cust_print('Link error with servo' + str(lidarNb+1)) self.disconnect() return -1 return 0 def stop_motors(self): for lidarNb in range(constants.nb_of_lidars): self.lidars[lidarNb].stop_motor() def start_motors(self): for lidarNb in range(constants.nb_of_lidars): self.lidars[lidarNb].start_motor() time.sleep(0.25) # To avoid to drain too much current def wait_servos_moving(self): for lidarNb in range(constants.nb_of_lidars): timeout = 100 # 10 seconds while True: try: while self.serial_connection.is_moving(constants.servosIDs[lidarNb]) == True: timeout -= 1 if timeout <= 0: cust_print(" Problem with the servo" + str(lidarNb+1) + "!") self.disconnect() return -1 time.sleep(0.1) break except: pass return 0 def single_scan(self,current_angle_z,meas_nb,erase_file): file = [] iterMeas = [] datasLeft = [] done = [] for lidarNb in range(constants.nb_of_lidars): try: path = constants.dataPath + r'/DatasL' + str(lidarNb+1) + '.txt' if erase_file == True: file.append(open(path,'w')) else: file.append(open(path,'a')) except: cust_print(' Cannot open file for lidar' + str(lidarNb+1) + '!') self.disconnect() return -1 try: iterMeas.append(self.lidars[lidarNb].iter_measures(max_buf_meas=10000)) except: cust_print(' Cannot communicate with lidar' + str(lidarNb+1) + '!') return -1 datasLeft.append(constants.nbOfDatasToRetrieve) done.append(False) try: while False in done: for lidarNb in range(constants.nb_of_lidars): if done[lidarNb] == False: datas = next(iterMeas[lidarNb]) angle = -1*((datas[2]+constants.offset_angle_lidar+180.0)%360 - 180.0) dist = datas[3] # First selection of points if angle >= -30 and angle <= +30 and dist > 0: file[lidarNb].write(str(angle) + ' ' + str(current_angle_z) + ' ' + str(dist) + '\n') datasLeft[lidarNb] -= 1 if datasLeft[lidarNb] < 1: done[lidarNb] = True except: cust_print(' Cannot retrieve datas on lidar ' + str(lidarNb+1) + '!') self.disconnect() return -1 for lidarNb in range(constants.nb_of_lidars): try: self.lidars[lidarNb].stop() except: cust_print(' Cannot communicate with lidar'+ str(lidarNb+1) + '!') return -1 file[lidarNb].close() return 0 def scan_datas(self,stature): cust_print('Datas scanning...') # Get height from stature lin_coeffs_navel_height = [0.66410011, -105.89395578] height = (stature*10)*lin_coeffs_navel_height[0] + lin_coeffs_navel_height[1] cust_print(" Target: " + str(int(height/10)) + "cm") self.start_motors() # Clean the infos file try: infoFile = open(constants.dataPath + r'/scan_infos.txt','w') except: cust_print(" Cannot open infos file") return -1 [inclination_array,nbMes_array] = compute_angle_array_for_scan(height) for i in range(len(inclination_array)): try: # Broadcast moving action to all servos self.servos_goto(BROADCAST_ID,inclination_array[i]) except: cust_print(" Problem with the serial connection!") self.disconnect() return -1 if self.wait_servos_moving() != 0: return -1 if i == 0: if self.single_scan(inclination_array[i],nbMes_array[i],True) != 0: return -1 else: if self.single_scan(inclination_array[i],nbMes_array[i],False) != 0: return -1 # Write the height of scan in the info file try: infoFile = open(constants.dataPath + r'/scan_infos.txt','a') infoFile.write(str(height)) except: cust_print(" Cannot open infos file") return -1 try: # Broadcast moving action to all servos self.servos_goto(BROADCAST_ID,0) except: cust_print(" Problem with the serial connection!") self.disconnect() return -1 cust_print(' All the datas have been successfully retrieved.') self.stop_motors() return 0
from pyax12.connection import Connection conn = Connection(port="/dev/ttyACM0", baudrate=57600) dxlid = 1 print(conn.ping(1)) #conn.pretty_print_control_table(dxlid) conn.close()
def main(): """ A PyAX-12 demo. Move the specified Dynamixel unit to 0° (0) then go to 300° (1023) then go back to 150° (511). """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(args.port, args.baudrate, args.timeout) ### dynamixel_id = 1 # Go to 0° serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -45° (45° CW) serial_connection.goto(dynamixel_id, -45, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -90° (90° CW) serial_connection.goto(dynamixel_id, -90, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -135° (135° CW) serial_connection.goto(dynamixel_id, -135, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to -150° (150° CW) serial_connection.goto(dynamixel_id, -150, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +150° (150° CCW) serial_connection.goto(dynamixel_id, 150, speed=512, degrees=True) time.sleep(2) # Wait 2 seconds # Go to +135° (135° CCW) serial_connection.goto(dynamixel_id, 135, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +90° (90° CCW) serial_connection.goto(dynamixel_id, 90, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go to +45° (45° CCW) serial_connection.goto(dynamixel_id, 45, speed=512, degrees=True) time.sleep(1) # Wait 1 second # Go back to 0° serial_connection.goto(dynamixel_id, 0, speed=512, degrees=True) ### # Close the serial connection serial_connection.close()
def main(): """ A PyAX-12 demo. Find all the currently connected AX12s and jog them back and forth through a series of turns """ # Parse options parser = common_argument_parser(desc=main.__doc__) args = parser.parse_args() # Connect to the serial port serial_connection = Connection(port=args.port, baudrate=args.baudrate, timeout=args.timeout, rpi_gpio=args.rpi) # Ping the dynamixel unit(s) ids_available = serial_connection.scan() print("Found %d servos with ids %s" % (len(ids_available), ids_available)) prev_pos = [ int(serial_connection.get_present_position(i, degrees=False)) for i in ids_available ] print(prev_pos) #type(prev_pos[0]) for dynamixel_id in ids_available: print("Starting position for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=True))) # Go to -45° serial_connection.goto(dynamixel_id, -45, speed=512, degrees=True) print("-45 angle for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=False))) time.sleep(1) # Wait 1 second # Go to -90° (90° CW) serial_connection.goto(dynamixel_id, -90, speed=512, degrees=True) time.sleep(1) # Wait 1 second print("-90 angle for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=False))) # Go to +90° (90° CCW) serial_connection.goto(dynamixel_id, 45, speed=512, degrees=True) time.sleep(1) # Wait 1 second print("+45 angle for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=False))) # Go to +90° (90° CCW) serial_connection.goto(dynamixel_id, 90, speed=512, degrees=True) time.sleep(1) # Wait 1 second print("+90 angle for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=False))) serial_connection.goto(dynamixel_id, 340, speed=512, degrees=False) time.sleep(1) # Wait 1 second print("Home angle for servo %d is %d" % (dynamixel_id, serial_connection.get_present_position(dynamixel_id, degrees=False))) # Close the serial connection serial_connection.close()
class DynamixelAX12(object): def __init__(self): # Connect to the serial port self.connection = Connection(port="/dev/ttyACM0", baudrate=57600) self.dynamixel_x_axis_id = 3 self.dynamixel_z_axis_id = 4 # Setup the angle limits fot the X axis self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id, -90, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id, 90, degrees=True) # Setup the angle limits fot the Z axis self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id, -90, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id, 90, degrees=True) # Goto the initial position (-45°, 0°) self.connection.goto(self.dynamixel_x_axis_id, -45, speed=255, degrees=True) self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255, degrees=True) # TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms) # (+ memorize the original values to set them back) def __del__(self): # TODO: set CW_ANGLE_LIMIT and CCW_ANGLE_LIMIT (+ alarms) # Restore the default position (0°, 0°) self.connection.goto(self.dynamixel_x_axis_id, 0, speed=255, degrees=True) self.connection.goto(self.dynamixel_z_axis_id, 0, speed=255, degrees=True) # Setup the angle limits for the X axis self.connection.set_cw_angle_limit(self.dynamixel_x_axis_id, -150, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_x_axis_id, 150, degrees=True) # Setup the angle limits for the Z axis self.connection.set_cw_angle_limit(self.dynamixel_z_axis_id, -150, degrees=True) self.connection.set_ccw_angle_limit(self.dynamixel_z_axis_id, 150, degrees=True) self.connection.close() def apply_control(self, control_vect): pos_x = self.connection.get_present_position(self.dynamixel_x_axis_id) pos_z = self.connection.get_present_position(self.dynamixel_z_axis_id) print(pos_x, pos_z, control_vect) # The x_axis controls up/down movements new_pos_x = pos_x + int(control_vect[1] * 20.) # The z_axis controls left/right movements # Warning: movements are inverted on the z axis # (negative commands move the frame to the right) new_pos_z = pos_z - int(control_vect[0] * 20.) speed_x = abs(int(control_vect[1] * 1023.)) # 300 speed_z = abs(int(control_vect[0] * 1023.)) # 300 try: self.connection.goto(self.dynamixel_x_axis_id, new_pos_x, speed=speed_x) self.connection.goto(self.dynamixel_z_axis_id, new_pos_z, speed=speed_z) except AngleLimitError: print("Angle limit")
class MotorManager: ''' Class to manage motor port on Windows should be 'COMX' port on Posix should be '/dev/ttyUSBX' ''' def __init__(self, p="/dev/ttyUSB0", baudRate=1000000, scanLimit=20, defaultSpeed=90): self.serial_connection = Connection(port=p, baudrate=baudRate) self.connected_motor_ids = [] self.scan_limit = scanLimit self.default_speed = defaultSpeed self.available_baudrate = '''+------+-----------+------------+ | Data | Set BPS | Target BPS | +------+-----------+------------+ | 1 | 1000000.0 | 1000000.0 | | 3 | 500000.0 | 500000.0 | | 4 | 400000.0 | 400000.0 | | 7 | 250000.0 | 250000.0 | | 9 | 200000.0 | 200000.0 | | 16 | 117647.1 | 115200.0 | | 34 | 57142.9 | 57600.0 | | 103 | 19230.8 | 19200.0 | | 207 | 9615.4 | 9600.0 | +------+-----------+------------+''' self.available_baud_ids = [1, 3, 4, 7, 9, 16, 34, 103, 207] def terminateConnection(self): self.serial_connection.close() print("Connection is closed.") ########################### # Get Connected Motor Ids # ########################### def getConnectIds(self): self.connected_motor_ids = list( self.serial_connection.scan(range(self.scan_limit))) print("Available ids are", self.connected_motor_ids) ########### # Set Ids # ########### def setNewIdOf(self, motor_id, new_id): if motor_id == new_id or new_id < 1 or new_id > 254: print( "Please use id in range of 1-254 and not duplicated with old id: {0}" .format(motor_id)) return self.serial_connection.set_id(motor_id, new_id) print("Motor id {0} is changed to id {1}.".format(motor_id, new_id)) ##################### # Print Information # ##################### def printAvailableBaudrates(self): print(self.available_baudrate) def printControlTable(self, motor_id): self.serial_connection.pretty_print_control_table(motor_id) #################### # Position Control # #################### def setPositionOf(self, motor_id, pos=512, speed=90): self.serial_connection.goto(motor_id, pos, speed=speed) def setPositionAll(self, pos=512, speed=90): for motorId in self.connected_motor_ids: self.serial_connection.goto(motorId, pos, speed) def getPositionOf(self, motor_id): return self.serial_connection.get_goal_position(motor_id) def getPositionAll(self): temp = {} for motorId in self.connected_motor_ids: temp[motorId] = self.serial_connection.get_goal_position(motorId) return temp ############### # Limit Angle # ############### def setAngleLimitOf(self, motor_id, cwLim, ccwLim): # set limit for cw rotation self.serial_connection.set_cw_angle_limit(motor_id, cwLim) # set limit for ccw rotation self.serial_connection.set_ccw_angle_limit(motor_id, ccwLim) print("Angle limit is set at: {0}(cw), {1}(ccw)".format(cwLim, ccwLim)) def getAngleLimitOf(self, motor_id): cwLim = self.serial_connection.get_cw_angle_limit(motor_id) ccwLim = self.serial_connection.get_ccw_angle_limit(motor_id) print("Current angle limit is at: {0}(cw), {1}(ccw)".format( cwLim, ccwLim)) #################### # Baudrate Control # #################### def getBaudrateOf(self, motor_id): return self.serial_connection.get_baud_rate(motor_id) def getBaudrateAll(self): temp = {} for motorId in self.connected_motor_ids: temp[motorId] = self.serial_connection.get_baud_rate(motorId) return temp def setBaudrateOf(self, motor_id, baudId=1): # check if input baudId is in range if baudId not in self.available_baud_ids: print("Please use baudId got from 'Data' column below.") self.printAvailableBaudrates() else: self.serial_connection.set_baud_rate(motor_id, baudId, unit="internal") print("Motor id {0} baudrate is set to {1}: {2}.".format( motor_id, baudId, round(2000000. / (baudId + 1)))) def setBaudrateAll(self, baudId=1): # check if input baudId is in range if baudId not in self.available_baud_ids: print("Please use baudId got from 'Data' column below.") self.printAvailableBaudrates() else: for motorId in self.connected_motor_ids: self.serial_connection.set_baud_rate(motorId, baudId, unit="internal") print("Motor id {0} baudrate is set to {1}: {2}.".format( motorId, baudId, round(2000000. / (baudId + 1)))) ######### # Helps # ######### @staticmethod def help(): txt = '''List of methods available. ====== Connection ====== --- Import from dxlcli import MotorManager --- Instantiate class m = MotorManager(p="/dev/ttyUSB0", baudRate=1000000, scanLimit=20, defaultSpeed=90) *** USE COMXX on Windows *** --- Terminate connection m.terminateConnection() --- Get connected motor ids m.getConnectIds() ====== ID settings ====== --- Set new id to specified motor id m.setNewIdOf(motor_id, new_id): ====== Printing Information ====== --- All available baudrates m.printAvailableBaudrates() --- Motor status table m.printControlTable(motor_id) ====== Position Control ====== --- Get m.getPositionOf(motor_id) m.getPositionAll() --- Set m.setPositionOf(motor_id, pos=512, speed=90) m.setPositionAll(pos=512, speed=90) ====== Baudrate Control ====== --- Get getBaudrateOf(motor_id) getBaudrateAll() --- Set setBaudrateOf(motor_id, baudId=1) setBaudrateAll(baudId=1) ''' print(txt)
class Robot(object): def __init__(self, speed=150, port='/dev/ttyACM0', baudrate=1000000, start_id=2, debug=False): self.speed = speed self.connection = Connection(port=port, baudrate=baudrate) self.legs = Legs(connection=self.connection, speed=self.speed, start_id=start_id) self.debug = debug self.connection.flush() if (self.debug): print("[debug] connected to port", port, "at", baudrate, baudrate) def reset(self): if (self.debug): print("[debug] scanning") self.connection.scan() if (self.debug): print("[debug] ready") def close(self): self.connection.close() if (self.debug): print("[debug] closed") def stand(self): if (self.debug): print("[debug] stand") self.legs.all.hip.goto(45) self.legs.all.knee.goto(-60) self.legs.all.ankle.goto(-30) def kneel(self): if (self.debug): print("[debug] kneel") self.legs.all.hip.goto(-45) self.legs.all.knee.goto(-90) self.legs.all.ankle.goto(-90) def flat(self): if (self.debug): print("[debug] flat") self.legs.all.hip.goto(90) self.legs.all.knee.goto(0) self.legs.all.ankle.goto(0) def walk(self, steps=1, direction="forward", knee_up=False): if (self.debug): print("[debug] walk", direction, steps, "steps") if knee_up == False: walk = Walk(self) else: walk = Walk(self, knee_up=knee_up) for i in range(0, steps): walk.step_vertical(["fl", "rr"], "down") walk.step_horizontal("fl", "rl", direction) walk.step_horizontal("rr", "fl", direction) walk.step_vertical(["fl", "rr"], "up") walk.step_vertical(["fr", "rl"], "down") walk.step_horizontal("fr", "rr", direction) walk.step_horizontal("rl", "fr", direction) walk.step_vertical(["fr", "rl"], "up") def get_status(self): servos = [2, 3, 5, 6, 8, 9, 11, 12] min_voltage = 999 max_voltage = 0 sum_voltage = 0 good_count = 0 for servo in servos: try: if not self.connection.ping(servo): print("ping failed servo", servo) voltage = self.connection.get_present_voltage(servo) if voltage > max_voltage: max_voltage = voltage elif voltage < min_voltage: min_voltage = voltage sum_voltage += voltage good_count = good_count + 1 except: print("[error] cannot connect to", servo) if good_count > 0: print(min_voltage, max_voltage, sum_voltage / good_count) else: print("no servos online")