Esempio n. 1
0
    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)
Esempio n. 2
0
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()
Esempio n. 3
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
Esempio n. 4
0
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()
Esempio n. 5
0
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()
Esempio n. 6
0
    def test_example2_hard(self):
        """Check the example 2 from the Dynamixel user guide: "Reading the
        internal temperature of the Dynamixel actuator with an ID of 1"
        (p.20).

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0"  # TODO
        baudrate = 57600  # TODO
        timeout = 0.1  # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make the instruction packet (based on the example 2 of the
        # Dynamixel user guide: "Reading the internal temperature of the
        # Dynamixel actuator with an ID of 1" (p.20))
        dynamixel_id = 1
        data = (0x02, 0x2b, 0x01)
        instruction_packet = Packet(dynamixel_id, data)

        try:
            serial_connection.send(instruction_packet)
        except (TypeError, ValueError, StatusPacketError):
            self.fail("Encountered an unexpected exception.")
Esempio n. 7
0
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()
Esempio n. 8
0
    def test_example2_hard(self):
        """Check the example 2 from the Dynamixel user guide: "Reading the
        internal temperature of the Dynamixel actuator with an ID of 1"
        (p.20).

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0" # TODO
        baudrate = 57600      # TODO
        timeout = 0.1         # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make the instruction packet (based on the example 2 of the
        # Dynamixel user guide: "Reading the internal temperature of the
        # Dynamixel actuator with an ID of 1" (p.20))
        dynamixel_id = 1
        data = (0x02, 0x2b, 0x01)
        instruction_packet = Packet(dynamixel_id, data)

        try:
            serial_connection.send(instruction_packet)
        except (TypeError, ValueError, StatusPacketError):
            self.fail("Encountered an unexpected exception.")
Esempio n. 9
0
def readMotors():
    serial_connection = Connection(port='/dev/ttyUSB0', baudrate=1000000)

    t1 = serial_connection.get_control_table_tuple(1)
    t2 = serial_connection.get_control_table_tuple(2)
    t3 = serial_connection.get_control_table_tuple(3)
    t4 = serial_connection.get_control_table_tuple(4)

    return json.dumps([t1, t2, t3, t4])
Esempio n. 10
0
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()
Esempio n. 11
0
def connect():
    """
	serial connection handling
	"""
    dxl2 = 2
    connection = Connection(port="/dev/ttyACM0",
                            baudrate=1000000,
                            timeout=0.2,
                            waiting_time=0.02)
    dxl1 = 1
    connection.pretty_print_control_table(int(dxl1))
Esempio n. 12
0
    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()
Esempio n. 13
0
    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()
Esempio n. 14
0
    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()
Esempio n. 15
0
File: pun.py Progetto: pun2pun/TR_01
    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() 
Esempio n. 16
0
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()
Esempio n. 17
0
    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)
Esempio n. 18
0
def connect():
	#connection=Connection(port="/dev/ttyACM0", baudrate = 100000, timeout = 0.2, waiting_time = 0.02)
	"""
	serial connection handling
	"""
	dxl1=1
	dxl2=2
	cccombo = ""
	while cccombo == "" or None or Null or 0:
		try:
			connection.close()
			connection = Connection(port="/dev/ttyACM0", baudrate = 57600, timeout = 0.2, waiting_time= 0.02)
			cccombo+=(str(connection.pretty_print_control_table(dxl1) + str(connection.pretty_print_control_table(dxl2))))
		except:
			pass
	return cccombo
Esempio n. 19
0
    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)
Esempio n. 20
0
def connect():
    print("connect function is running!")
    try:
        #	flush the buffer of the connection and see if that allows it to reassert itself
        #serial_connection.flush()
        serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600)
        print(str(serial_connection.ping(1)))


#		try:  					#	try first with default port name (ttyACM0) before trying elsewhere
#			serial_connection = Connection(port="/dev/ttyAMA0", baudrate = 100000)
#		except:					# 	current "elsewhere" definition confined to sequential suffix numbers
#			for x in range (0,99):
#				location_string = "/dev/ttyAMA" + str(x)
#				serial_connection = Connection (location_string, baudrate = 100000)
    except SerialException:  #	if everything's f****d:
        return "current generation of game-stopping error. Physically reset."
Esempio n. 21
0
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()
Esempio n. 22
0
def main():
    # Parse options
    parser = common_argument_parser(desc=main.__doc__)
    args = parser.parse_args()
    # Connect to the serial port
    serial_connection = Connection(port='/dev/ttyS0',
                                   baudrate=1000000,
                                   timeout=0.01,
                                   rpi_gpio=18)
Esempio n. 23
0
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()
Esempio n. 24
0
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()
Esempio n. 25
0
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()
Esempio n. 26
0
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()
Esempio n. 27
0
    def test_checksum_error(self):
        """Check that the "InstructionChecksumError" exception is raised when
        the "checksum error" flag is ON in the StatusPacket's "error" byte.

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0"  # TODO
        baudrate = 57600  # TODO
        timeout = 0.1  # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make a wrong instruction packet (based on the example 2 of the
        # Dynamixel user guide: "Reading the internal temperature of the
        # Dynamixel actuator with an ID of 1" (p.20))
        packet = bytes((0xff, 0xff, 0x01, 0x04, 0x02, 0x2b, 0x01, 0))

        # Send the wrong instruction packet and get the response
        with self.assertRaises(InstructionChecksumError):
            serial_connection.send(packet)
Esempio n. 28
0
    def test_checksum_error(self):
        """Check that the "InstructionChecksumError" exception is raised when
        the "checksum error" flag is ON in the StatusPacket's "error" byte.

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0" # TODO
        baudrate = 57600      # TODO
        timeout = 0.1         # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make a wrong instruction packet (based on the example 2 of the
        # Dynamixel user guide: "Reading the internal temperature of the
        # Dynamixel actuator with an ID of 1" (p.20))
        packet = bytes((0xff, 0xff, 0x01, 0x04, 0x02, 0x2b, 0x01, 0))

        # Send the wrong instruction packet and get the response
        with self.assertRaises(InstructionChecksumError):
            serial_connection.send(packet)
Esempio n. 29
0
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()
Esempio n. 30
0
    def test_range_error(self):
        """Check that the "RangeError" exception is raised when the
        "range error" flag is ON in the StatusPacket's "error" byte.

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0"  # TODO
        baudrate = 57600  # TODO
        timeout = 0.1  # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make a wrong instruction packet (set the LED byte in the control
        # table)
        dynamixel_id = 1
        data = (0x03, 0x19, 0xff)  # wrong parameter value (the last byte)
        instruction_packet = Packet(dynamixel_id, data)

        # Send the wrong instruction packet and get the response
        with self.assertRaises(RangeError):
            serial_connection.send(instruction_packet)
Esempio n. 31
0
    def test_range_error(self):
        """Check that the "RangeError" exception is raised when the
        "range error" flag is ON in the StatusPacket's "error" byte.

        This test requires to be connected to the Dynamixel number 1 using
        port "/dev/ttyUSB0" at 57600 baud (thus it only works on Unix systems
        with FTDI devices)."""

        # Connect to the serial port
        port = "/dev/ttyUSB0" # TODO
        baudrate = 57600      # TODO
        timeout = 0.1         # TODO
        serial_connection = Connection(port, baudrate, timeout)

        # Make a wrong instruction packet (set the LED byte in the control
        # table)
        dynamixel_id = 1
        data = (0x03, 0x19, 0xff)   # wrong parameter value (the last byte)
        instruction_packet = Packet(dynamixel_id, data)

        # Send the wrong instruction packet and get the response
        with self.assertRaises(RangeError):
            serial_connection.send(instruction_packet)
Esempio n. 32
0
    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 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()
Esempio n. 34
0
 def start(self):
     """Metode til at finde USB-porten, fx COM12 paa Win, ttyUSB0 paa linux"""
     #sc = Connection(port="/dev/ttyUSB0", baudrate=57600)
     sc = Connection(port="/dev/ttyUSB0", baudrate=1000000)
     #sc = Connection(port="COM12", baudrate=1000000)
     Emu.sc.flush()
Esempio n. 35
0
class Emu():
    """Klasse til at kontrollere Dynamixel AX12 actuators"""
    sc = Connection(port="/dev/ttyUSB0", baudrate=1000000)

    # sc = Connection(port="COM12", baudrate=1000000)

    def __init__(self):
        pass

    def start(self):
        """Metode til at finde USB-porten, fx COM12 paa Win, ttyUSB0 paa linux"""
        #sc = Connection(port="/dev/ttyUSB0", baudrate=57600)
        sc = Connection(port="/dev/ttyUSB0", baudrate=1000000)
        #sc = Connection(port="COM12", baudrate=1000000)
        Emu.sc.flush()

    def scanUnits(self):
        """Scanner dynamixel motorer og returnere deres id'er i en liste"""
        ids = Emu.sc.scan()
        return ids

    def getTuple(self, ID):
        Emu.sc.flush()
        tup = Emu.sc.get_control_table_tuple(ID)
        return tup

    def readDxl(self, ID):
        """Printer oplysninger motoren med ID"""
        Emu.sc.flush()
        Emu.sc.pretty_print_control_table(ID)

    def jointMode(self, ID):
        """Skifter motoren med ID til joint mode"""
        Emu.sc.set_cw_angle_limit(ID, 0, False)
        Emu.sc.set_ccw_angle_limit(ID, 1023, False)

    def wheelMode(self, ID):
        """Skifter motoren med ID til wheelmode"""
        Emu.sc.set_ccw_angle_limit(ID, 0, False)
        Emu.sc.set_cw_angle_limit(ID, 0, False)

    def moveJoint(self, ID, position):
        """Flytter motoren med ID til position"""
        Emu.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
        Emu.sc.flush()
        Emu.sc.goto(ID, 0, int(speed), degrees=False)

    def stop(self):
        """Lukker forbindelsen gennem USB-porten til dynamixlerne"""
        Emu.sc.close()

    def getPos(self, ID):
        """Returnere motoren med ID's position"""
        position = Emu.sc.get_present_position(ID, True)
        return position
Esempio n. 36
0
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()
Esempio n. 37
0
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)
Esempio n. 38
0
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")
Esempio n. 39
0
import time

#กำหนด ID ของมอเตอร์แต่ละตัว
motor_1 = 7
motor_2 = 11
motor_3 = 12
motor_4 = 13
motor_5 = 15
motor_6 = 18

#กำหนดความเร็วในการหมุนของ Motor (0 - 512)
speed = 100

try:
    #ตั้งค่าการเชื่อต่อกับแขนกลผ่าน USB (Port ใช้ตาม DeviceManager, Baudrate = 1000000)
    serial_connection = Connection(port="COM4", baudrate=1000000)
    #หน่วงเวลาป้องกันระบบแฮ้ง
    time.sleep(0.5)
    print("\n\n********** ROBOT START **********\n\n")
except:
    print("Please connect USB or Power ! :( ")


def connectDisable():
    #ปิดการเชื่อมต่อ USB
    serial_connection.close()
    print("Dis-connected !!")


def grapOn(p6):
    serial_connection.goto(motor_6, p6, speed=speed, degrees=True)
Esempio n. 40
0
def open_port():
    global serial_connection
    # Connect to the serial port
    serial_connection = Connection(port="COM3", baudrate=1000000)