Merge pull request #40 from CCGSRobotics/server-refactor

Redesign the server to focus on a modular system with readable code

.gitignore

@@ -1,13 +1,9 @@
-#Driving Code
-App/JS/Resources/CurrentEmuBotCode2
-
 # Logs
 logs
 *.log
 npm-debug.log*
 yarn-debug.log*
 yarn-error.log*
-Server/logs/
 
 # Runtime data
 pids
@@ -78,3 +74,6 @@ typings/
 
 #Build directory
 Build/
+
+# Pycache
+Server/Driving/__pycache__/

Server/Driving/Servos/ax18a.csv

@@ -0,0 +1,33 @@
+Address, Size(Byte), Data Name, Description, Access, InitialValue
+0, 2, Model Number, Model Number, R, 18
+2, 1, Firmware Version, Firmware Version, R, -
+3, 1, ID, DYNAMIXEL ID, RW, 1
+4, 1, Baud Rate, Communication Speed, RW, 1
+5, 1, Return Delay Time, Response Delay Time, RW, 250
+6, 2, CW Angle Limit, Clockwise Angle Limit, RW, 0
+8, 2, CCW Angle Limit, Counter-Clockwise Angle Limit, RW, 1023
+11, 1, Temperature Limit, Maximum Internal Temperature Limit, RW, 75
+12, 1, Min Voltage Limit, Minimum Input Voltage Limit, RW, 60
+13, 1, Max Voltage Limit, Maximum Input Voltage Limit, RW, 140
+14, 2, Max Torque, Maximun Torque, RW, 983
+16, 1, Status Return Level, Select Types of Status Return, RW, 2
+17, 1, Alarm LED, LED for Alarm, RW, 36
+18, 1, Shutdown, Shutdown Error Information, RW, 36
+24, 1, Torque Enable, Motor Torque On/Off, RW, 0
+25, 1, LED, Status LED On/Off, RW, 0
+26, 1, CW Compliance Margin, CW Compliance Margin, RW, 1
+27, 1, CCW Compliance Margin, CCW Compliance Margin, RW, 1
+28, 1, CW Compliance Slope, CW Compliance Slope, RW, 32
+29, 1, CCW Compliance Slope, CCW Compliance Slope, RW, 32
+30, 2, Goal Position, Target Position, RW, -
+32, 2, Moving Speed, Moving Speed, RW, -
+34, 2, Torque Limit, Torque Limit(Goal Torque), RW, ADD 14&15
+36, 2, Present Position, Present Position, R, -
+38, 2, Present Speed, Present Speed, R, -
+40, 2, Present Load, Present Load, R, -
+42, 1, Present Voltage, Present Voltage, R, -
+43, 1, Present Temperature, Present Temperature, R, -
+44, 1, Registered, If Instruction is registered, R, 0
+46, 1, Moving, Movement Status, R, 0
+47, 1, Lock, Locking EEPROM, RW, 0
+48, 2, Punch, Minimum Current Threshold, RW, 32

Server/Driving/Servos/xl320.csv

@@ -0,0 +1,32 @@
+Address, Size(Byte), Data Name, Description, Access, InitialValue, Min, Max
+0, 2, Model Number, Model Number, R, 350, -, -
+2, 1, Firmware Version, Firmware Version, R, -, -, -
+3, 1, ID, DYNAMIXEL ID, RW, 1, 0, 252
+4, 1, Baud Rate, Communication Speed, RW, 3, 0, 3
+5, 1, Return Delay Time, Response Delay Time, RW, 250, 0, 254
+6, 2, CW Angle Limit, Clockwise Angle Limit, RW, 0, 0, 1023
+8, 2, CCW Angle Limit, Counter-Clockwise Angle Limit, RW, 1023, 0, 1023
+11, 1, Control Mode, Control Mode, RW, 2, 1, 2
+12, 1, Temperature Limit, Maximum Internal Temperature Limit, RW, 65, 0, 150
+13, 1, Min Voltage Limit, Minimum Input Voltage Limit, RW, 60, 50, 250
+14, 1, Max Voltage Limit, Maximum Input Voltage Limit, RW, 90, 50, 250
+15, 2, Max Torque, Maximun Torque, RW, 1023, 0, 1023
+17, 1, Status Return Level, Select Types of Status Return, RW, 2, 0, 2
+18, 1, Shutdown, Shutdown Error Information, RW, 3, 0, 7
+24, 1, Torque Enable, Motor Torque On/Off, RW, 0, 0, 1
+25, 1, LED, Status LED On/Off, RW, 0, 0, 7
+27, 1, D Gain, Derivative Gain, RW, 0, 0, 254
+28, 1, I Gain, Integral Gain, RW, 0, 0, 254
+29, 1, P Gain, Proportional Gain, RW, 32, 0, 254
+30, 2, Goal Position, Desired Position, RW, -, 0, 1023
+32, 2, Moving Speed, Moving Speed(Moving Velocity), RW, -, 0, 2047
+35, 2, Torque Limit, Torque Limit(Goal Torque), RW, -, 0, 1023
+37, 2, Present Position, Present Position, R, -, -, -
+39, 2, Present Speed, Present Speed, R, -, -, -
+41, 2, Present Load, Present Load, R, -, -, -
+45, 1, Present Voltage, Present Voltage, R, -, -, -
+46, 1, Present Temperature, Present Temperature, R, -, -, -
+47, 1, Registered, If Instruction is registered, R, 0, -, -
+49, 1, Moving, Movement Status, R, 0, -, -
+50, 1, Hardware Error Status, Hardware Error Status, R, 0, -, -
+51, 2, Punch, Minimum Current Threshold, RW, 32, 0, 1023

Server/Driving/dynamixels.py

@@ -0,0 +1,249 @@
+"""This module assists in the connection between a Python script to\
+    a U2D2, which, in turn connects to Dynamixels running either protocol\
+        1 or protocol 2.
+Contains:
+    - Code to automatically connect to a U2D2 assigned under /dev/ttyUSB
+    - Code that adjusts the baudrate of the U2D2
+    - A class (Dynamixel) with functions to interface with the DynamixelSDK
+"""
+import os
+from termios import error as TERMIOS_ERROR
+from dynamixel_sdk import PortHandler, PacketHandler
+
+PORT_HANDLER = None
+
+# Try to connect to the U2D2
+for port in range(2):
+    path = '/dev/ttyUSB{}'.format(port)
+    if os.path.exists(path):
+        PORT_HANDLER = PortHandler(path)
+        print('Initialised U2D2 at {}'.format(path))
+        break
+    else:
+        print('Could not initialise U2D2 at {}'.format(path))
+
+# Could not find a USB device in range
+if PORT_HANDLER is None:
+    print('Could not initialise U2D2! Maybe increase port range?')
+    exit()
+
+# Protocol 1 packet handler:
+PROTOCOL_ONE = PacketHandler(1.0)
+# Protocol 2 packet handler:
+PROTOCOL_TWO = PacketHandler(2.0)
+
+if PORT_HANDLER.openPort():
+    pass
+else:
+    print("Failed to open the port")
+
+if PORT_HANDLER.setBaudRate(1000000):
+    pass
+else:
+    print("Failed to change the baudrate")
+
+class Dynamixel:
+    """
+    An object representing a single Dynamixel
+
+    The Dynamixel class contains the ability to read or write to any address
+    in its control table, which is specified as a CSV file interpreted through
+    the GUI.
+
+    Attributes:
+        name: The name of the CSV containing the relevant control table
+        dynamixel_id: The initial ID of the Dynamixel
+        protocol: The protocol version of the servo - either 1 or 2
+        control_table: A dictionary containing the control table, formatted as
+            control_table[data name] = {Address, size etc}. This is generated
+            by the initialise_dynamixel function
+    """
+
+    def __init__(self, name, dynamixel_id, protocol, control_table):
+        """
+        Initialises the Dynamixel class with model, ID, protocol and control table
+        """
+
+        self.name = name
+        self.dynamixel_id = dynamixel_id
+        self.protocol = protocol
+        self.control_table = control_table
+        self.is_joint = True
+        self.is_wheel = False
+
+    def query(self, item, detail):
+        """
+        Returns the value of the row (item) and column (detail)
+
+        Args:
+            item: The row in which to look in for the information
+            detail: The column in which to look in for the information
+
+        Returns:
+            The data found at row (item) and column (detail)
+        """
+        try:
+            return self.control_table[item][detail]
+        except KeyError:
+            print('Could not find a matching address!')
+
+    def write_value(self, name, value):
+        """
+        Writes value to the address of name
+
+        Args:
+            name: The name of the data in the control table
+            value: The value to write to the Dynamixel
+        """
+
+        if name in self.control_table:
+            if 'W' in self.query(name, 'Access'):
+                size = self.query(name, 'Size(Byte)')
+                if self.protocol == 1:
+                    protocol_handler = PROTOCOL_ONE
+                else:
+                    protocol_handler = PROTOCOL_TWO
+                try:
+                    if size == 1:
+                        protocol_handler.write1ByteTxRx(PORT_HANDLER, self.dynamixel_id,\
+                                self.query(name, 'Address'), value)
+                    elif size == 2:
+                        protocol_handler.write2ByteTxRx(PORT_HANDLER, self.dynamixel_id,\
+                                self.query(name, 'Address'), value)
+                    elif size == 4:
+                        protocol_handler.write4ByteTxRx(PORT_HANDLER, self.dynamixel_id,\
+                                self.query(name, 'Address'), value)
+                except TERMIOS_ERROR as err:
+                    print('An error occured when reading from Dynamixel {}'\
+                        .format(self.dynamixel_id))
+                    print('Make sure the U2D2 is connected properly!')
+                    print('Debug info:\n', err)
+            else:
+                print('Cannot write to read-only address!')
+        else:
+            print('Cannot find table value!')
+
+    def read_value(self, name):
+        """
+        Reads a value at the address of name
+
+        Args:
+            name: The name of the value to read
+
+        Returns:
+            The value at the address of name
+        """
+
+        if name in self.control_table:
+            if 'R' in self.query(name, 'Access'):
+                try:
+                    if self.protocol == 1:
+                        return str(PROTOCOL_ONE.read1ByteTxRx(PORT_HANDLER, self.dynamixel_id,\
+                             self.query(name, 'Address'))[0])
+                    return str(PROTOCOL_TWO.read1ByteTxRx(PORT_HANDLER, self.dynamixel_id,\
+                            self.query(name, 'Address')))
+                except TERMIOS_ERROR as err:
+                    print('An error occured when reading from Dynamixel {}'\
+                        .format(self.dynamixel_id))
+                    print('Make sure the U2D2 is connected properly!')
+                    print('Debug info:\n', err)
+            else:
+                print('Cannot read at address!')
+        else:
+            print('Cannot find table value!')
+        return None
+
+    def reset_value(self, name):
+        """
+        Resets a value to its default
+
+        Args:
+            name: The name of the value to reset
+        """
+
+        if name in self.control_table:
+            self.write_value(name, self.control_table[name]['InitialValue'])
+
+    def wheel_mode(self):
+        """
+        Places the dynamixel in 'wheel mode', moving using speed only
+        """
+
+        self.write_value('Torque Enable', 0)
+        self.write_value('CW Angle Limit', 0)
+        self.write_value('CCW Angle Limit', 0)
+        self.write_value('Torque Enable', 1)
+        self.is_wheel = True
+        self.is_joint = False
+
+    def joint_mode(self):
+        """
+        Places the dynamixel in 'joint mode', moving using speed and goal position
+        """
+
+        self.write_value('Torque Enable', 0)
+        self.reset_value('CW Angle Limit')
+        self.reset_value('CCW Angle Limit')
+        self.write_value('Torque Enable', 1)
+        self.is_joint = True
+        self.is_wheel = False
+
+    def restart(self):
+        """
+        Resets the values of all writeable items in the control table
+        """
+
+        self.write_value('Torque Enable', 0)
+        for value in self.control_table:
+            self.reset_value(value)
+        self.is_wheel = False
+        self.is_joint = False
+
+def find_file(filename):
+    """
+    Finds a file in the Servos/ directory
+
+    Args:
+        filename: The name of the file to find
+    """
+    for root, _, files in os.walk("Servos/", topdown=False):
+        for name in files:
+            if os.path.splitext(name)[0] == filename:
+                filepath = os.path.join(root, name)
+                return filepath
+    print('Could not find matching control table!')
+    return None
+
+def initialise_dynamixel(model, dynamixel_id, protocol):
+    """
+    Creates a Dynamixel object with a relevant control table
+
+    Args:
+        model: The model name of the servo, used to find the CSV control table
+        dynamixel_id: The initial ID of the Dynamixel
+        protocol: The protocol used by the Dynamixel, either 1 or 2
+
+    Returns:
+        A Dynamixel class containing the control table found from the CSV, as well
+        as the ID and protocol specified in its arguments
+    """
+
+    filepath = find_file(model)
+    if filepath is None:
+        return Dynamixel(None, None, None, None)
+
+    table = open(filepath, 'r').readlines()
+    first_line = table[0].split(', ')
+    control_table = {}
+    for row in range(1, len(table)):
+        line = table[row].split(', ')
+        build = {}
+        build[line[2]] = {}
+        for iterator, item in enumerate(line):
+            if line[iterator] != line[2]:
+                new = item.strip()
+                if new.isdigit():
+                    new = int(new)
+                build[line[2]][first_line[iterator].strip()] = new
+        control_table = {**control_table, **build}
+    return Dynamixel(model, dynamixel_id, protocol, control_table)