diff --git a/.gitignore b/.gitignore index 4772d25..6c99eb3 100644 --- a/.gitignore +++ b/.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__/ \ No newline at end of file diff --git a/Server/Driving/Servos/ax18a.csv b/Server/Driving/Servos/ax18a.csv new file mode 100644 index 0000000..967b395 --- /dev/null +++ b/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 diff --git a/Server/Driving/Servos/xl320.csv b/Server/Driving/Servos/xl320.csv new file mode 100644 index 0000000..9a88966 --- /dev/null +++ b/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 diff --git a/Server/Driving/dynamixels.py b/Server/Driving/dynamixels.py new file mode 100644 index 0000000..14cc3bb --- /dev/null +++ b/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) diff --git a/Server/Driving/experimentalServer.cpp b/Server/Driving/experimentalServer.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Server/Driving/server.py b/Server/Driving/server.py new file mode 100644 index 0000000..e31e878 --- /dev/null +++ b/Server/Driving/server.py @@ -0,0 +1,92 @@ +"""This script is used for implementing commands defined in dynamixels.py +Contains: + - A UDP Server class that handles requests +""" +import socketserver +import socket +import re +from dynamixels import initialise_dynamixel + +SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + +SERVOS = {} +REGEX = r"\((init|delete|modify|mode|read)\)" +# SERVOS[12] = initialise_dynamixel('xl320', 12, 2) + +class UDPHandler(socketserver.BaseRequestHandler): + """The UDP handler class implementing functions from dynamixels.py + """ + def handle_regex(self, data): + """Handles the 'special' commands encased in brackets + Args: + data: The data to be parsed + """ + # Fancy regular expression to check if server is sending other commands + # WARNING: This code is VERY experimental and will crash if + # The wrong input is given + + items = data.split('-') + cmd = re.match(REGEX, items[0]).group(1) + if cmd == "init": + model = items[1] + dynamixel_id, protocol = map(int, items[2:]) + SERVOS[dynamixel_id] = initialise_dynamixel(model, dynamixel_id, protocol) + SERVOS[dynamixel_id].joint_mode() + elif cmd == "modify": + dynamixel_id, item, value = map(str, items[1:]) + dynamixel_id = int(dynamixel_id) + value = int(value) + if dynamixel_id in SERVOS: + SERVOS[dynamixel_id].write_value(item, value) + elif cmd == "read": + dynamixel_id, item = map(str, items[1:]) + dynamixel_id = int(dynamixel_id) + if dynamixel_id in SERVOS: + msg = "{}:{}".format(item, SERVOS[dynamixel_id].read_value(item)) + msg = bytes(msg, 'utf-8') + SOCK.sendto(msg, (str(self.client_address[0]), 5003)) + elif cmd == "mode": + dynamixel_id, mode = map(str, items[1:]) + dynamixel_id = int(dynamixel_id) + if dynamixel_id in SERVOS: + if mode == 'wheel': + SERVOS[dynamixel_id].wheel_mode() + elif mode == 'joint': + SERVOS[dynamixel_id].joint_mode() + elif cmd == 'delete': + dynamixel_id = int(items[1]) + if dynamixel_id in SERVOS: + SERVOS.pop(dynamixel_id) + def handle(self): + self.data = self.request[0].decode('utf-8').strip() + new = self.data.split('\n') + for i in new: + if len(re.findall(REGEX, i)) == 1: + self.handle_regex(i) + elif i.count('-') == 1: + # Wheel + dynamixel_id, speed = map(int, i.split('-')) + if dynamixel_id in SERVOS and SERVOS[dynamixel_id].is_wheel: + SERVOS[dynamixel_id].write_value('Moving Speed', speed) + elif dynamixel_id not in SERVOS: + print('There is no servo of dynamixel_id {} initialised!'.format(dynamixel_id)) + else: + print('Servo {} is not a wheel!'.format(dynamixel_id)) + elif i.count('-') == 2: + # Joint + dynamixel_id, pos, speed = map(int, i.split('-')) + if dynamixel_id in SERVOS and SERVOS[dynamixel_id].is_joint: + SERVOS[dynamixel_id].write_value('Goal Position', pos) + SERVOS[dynamixel_id].write_value('Moving Speed', speed) + elif dynamixel_id not in SERVOS: + print('There is no servo of dynamixel_id {} initialised!'.format(dynamixel_id)) + else: + print('Servo {} is not a joint!'.format(dynamixel_id)) + else: + print('Invalid command! {}'.format(i)) + +socketserver.UDPServer.allow_reuse_address = True +SERVER = socketserver.UDPServer(("", 5001), UDPHandler) + +print('Dynamixel Server Started!') +SERVER.serve_forever() diff --git a/Server/Server-Sensor-Code/Grabber.py b/Server/Sensors/Manipulation/Grabber.py similarity index 100% rename from Server/Server-Sensor-Code/Grabber.py rename to Server/Sensors/Manipulation/Grabber.py diff --git a/Server/Server-Sensor-Code/GrabberWithSensors.ino b/Server/Sensors/Manipulation/GrabberWithSensors.ino similarity index 100% rename from Server/Server-Sensor-Code/GrabberWithSensors.ino rename to Server/Sensors/Manipulation/GrabberWithSensors.ino diff --git a/Server/motion.conf b/Server/Sensors/Video/motion.conf similarity index 100% rename from Server/motion.conf rename to Server/Sensors/Video/motion.conf diff --git a/Server/Server-Sensor-Code/CO2senseNEW.py b/Server/Sensors/carbon_dioxide.py similarity index 100% rename from Server/Server-Sensor-Code/CO2senseNEW.py rename to Server/Sensors/carbon_dioxide.py diff --git a/Server/Server-Code-2018/GPIO.py b/Server/Server-Code-2018/GPIO.py deleted file mode 100644 index 0f35e05..0000000 --- a/Server/Server-Code-2018/GPIO.py +++ /dev/null @@ -1,29 +0,0 @@ -import RPi.GPIO as GPIO -from time import * - -def GPIOsetup(): - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(16,GPIO.OUT) - -def SwitchON(): - GPIOsetup() - GPIO.output(16,0) - print("ON") - return - -def SwitchOFF(): - GPIOsetup() - GPIO.output(16,1) - print("OFF") - return - -def STester(): - for i in range(1000): - SwitchON() - sleep(0.5) - SwitchOFF() - sleep(0.5) - -#SwitchOFF() -#STester() diff --git a/Server/Server-Code-2018/RoboCupServer-Current.py b/Server/Server-Code-2018/RoboCupServer-Current.py deleted file mode 100644 index f672b71..0000000 --- a/Server/Server-Code-2018/RoboCupServer-Current.py +++ /dev/null @@ -1,62 +0,0 @@ -import socketserver as SocketServer -import sys -from GPIO import * -SwitchOFF() -SwitchON() -from emuBot import * - -wheelMode(1) -wheelMode(2) -wheelMode(3) -wheelMode(4) -jointMode(5) -jointMode(6) -jointMode(7) -jointMode(8) -jointMode(9) -jointMode(10) -jointMode(11) - -class MyTCPHandler(SocketServer.BaseRequestHandler): - def handle(self): - # while True: - #sc.flush() - self.data = self.request[0].decode('utf-8').strip() - new = self.data.split('\n') - for i in new: - if i != "": - if 0: - print(new) - else: - if i == "ON": - SwitchON() - elif i == "OFF": - SwitchOFF() - else: - ID = int(i[0]+i[1]) - if ID < 5: - try: - moveWheel(ID, int(i[2:])) - except e: - print(e) - print('brokeWheel ',ID) - else: - try: - moveJoint(ID, int(i[2:-4]), i[-4:]) - except: - print('BrokeJoint ', ID) - # print(i) - # else: - # print(i) - -if __name__ == "__main__": - HOST, PORT = "", 9999 - -SocketServer.UDPServer.allow_reuse_address = True -server = SocketServer.UDPServer((HOST, PORT), MyTCPHandler) - -print('Server Started') -server.serve_forever() - - - diff --git a/Server/Server-Code-2018/__pycache__/GPIO.cpython-34.pyc b/Server/Server-Code-2018/__pycache__/GPIO.cpython-34.pyc deleted file mode 100644 index 62a4dc2..0000000 Binary files a/Server/Server-Code-2018/__pycache__/GPIO.cpython-34.pyc and /dev/null differ diff --git a/Server/Server-Code-2018/__pycache__/GPIO.cpython-35.pyc b/Server/Server-Code-2018/__pycache__/GPIO.cpython-35.pyc deleted file mode 100644 index b4986eb..0000000 Binary files a/Server/Server-Code-2018/__pycache__/GPIO.cpython-35.pyc and /dev/null differ diff --git a/Server/Server-Code-2018/__pycache__/GPIO.cpython-37.pyc b/Server/Server-Code-2018/__pycache__/GPIO.cpython-37.pyc deleted file mode 100644 index dbfb920..0000000 Binary files a/Server/Server-Code-2018/__pycache__/GPIO.cpython-37.pyc and /dev/null differ diff --git a/Server/Server-Code-2018/__pycache__/emuBot.cpython-34.pyc b/Server/Server-Code-2018/__pycache__/emuBot.cpython-34.pyc deleted file mode 100644 index 1b98707..0000000 Binary files a/Server/Server-Code-2018/__pycache__/emuBot.cpython-34.pyc and /dev/null differ diff --git a/Server/Server-Code-2018/__pycache__/emuBot.cpython-37.pyc b/Server/Server-Code-2018/__pycache__/emuBot.cpython-37.pyc deleted file mode 100644 index fe5d71d..0000000 Binary files a/Server/Server-Code-2018/__pycache__/emuBot.cpython-37.pyc and /dev/null differ diff --git a/Server/Server-Code-2018/emuBot.py b/Server/Server-Code-2018/emuBot.py deleted file mode 100644 index 6f57414..0000000 --- a/Server/Server-Code-2018/emuBot.py +++ /dev/null @@ -1,117 +0,0 @@ -import time -import threading - -from dynamixel_sdk import * - -portHandler = PortHandler("/dev/ttyUSB0") -packetHandler = PacketHandler(1.0) - -if portHandler.openPort(): - # print("Succeeded to open the port" - pass -else: - print("Failed to open the port") - print("Press any key to terminate...") - getch() - quit() -if portHandler.setBaudRate(BAUDRATE): - # print("Succeeded to change the baudrate") - pass -else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - getch() - quit() - - -def jointMode(ID): - if ID < 9: - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 6, 1024) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 8, 3072) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - elif ID == 9: - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 6, 0) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 8, 2560) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - elif ID > 9: - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 6, 0) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 8, 1023) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - - -def wheelMode(ID): - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 6, 0) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 8, 0) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - -def moveJoint(ID, position,speed): - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 30, position) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, ID, 36) - if not abs(dxl_goal_position[index] - dxl_present_position) > 20: - print("Servo with ID of %03d was %03d off target, outside acceptable range" % (ID, abs(position - dxl_present_position))) - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 32, speed) - if xl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - -def moveWheel(ID, speed): - # Convert negative values of speed to between 1024 and 2047 - if speed < 0: - # Limit allowed reverse speed to prevent errors - if speed < -1024: - speed = 2047 - else: - speed = 1023 + -speed - - else: - if speed > 1023: - # Limit allowed forward speed to prevent errors - speed = 1023 - - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, 32, speed) - if xl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) diff --git a/Server/Server-Code-2018/mx28.py b/Server/Server-Code-2018/mx28.py deleted file mode 100644 index adebae1..0000000 --- a/Server/Server-Code-2018/mx28.py +++ /dev/null @@ -1,153 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -################################################################################ -# Copyright 2017 ROBOTIS CO., LTD. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -################################################################################ - -# Author: Ryu Woon Jung (Leon) - -# -# ********* Read and Write Example ********* -# -# -# Available DXL model on this example : All models using Protocol 1.0 -# This example is tested with a DXL MX-28, and an USB2DYNAMIXEL -# Be sure that DXL MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -from dynamixel_sdk import * # Uses Dynamixel SDK library - -# Control table address -# 28 = 24 # Control table address is different in Dynamixel model -# 30 = 30 -# 36 = 36 - -# Protocol version -# 1.0 = 1.0 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID : 1 -BAUDRATE = 1000000 # Dynamixel default baudrate : 57600 -DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -# 1 = 1 # Value for enabling the torque -# 0 = 0 # Value for disabling the torque -# 10 = 10 # Dynamixel will rotate between this value -# 4000 = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -# 20 = 20 # Dynamixel moving status threshold - -index = 0 -dxl_goal_position = [10, 4000] # Goal position - - -# Initialize PortHandler instance -# Set the port path -# Get methods and members of PortHandlerLinux or PortHandlerWindows -portHandler = PortHandler(DEVICENAME) - -# Initialize PacketHandler instance -# Set the protocol version -# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler -packetHandler = PacketHandler(1.0) - -# Open port -if portHandler.openPort(): - print("Succeeded to open the port") -else: - print("Failed to open the port") - print("Press any key to terminate...") - getch() - quit() - - -# Set port baudrate -if portHandler.setBaudRate(BAUDRATE): - print("Succeeded to change the baudrate") -else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - getch() - quit() - -# Enable Dynamixel Torque -dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, 28, 1) -if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) -elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) -else: - print("Dynamixel has been successfully connected") - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(0x1b): - break - - # Write goal position - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, 30, dxl_goal_position[index]) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - while 1: - # Read present position - dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, 36) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) - - if not abs(dxl_goal_position[index] - dxl_present_position) > 20: - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel Torque -dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, 28, 0) -if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) -elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - -# Close port -portHandler.closePort() diff --git a/Server/Server-Code-2019/GPIO.py b/Server/Server-Code-2019/GPIO.py deleted file mode 100644 index eb60616..0000000 --- a/Server/Server-Code-2019/GPIO.py +++ /dev/null @@ -1,18 +0,0 @@ -import RPi.GPIO as GPIO -import time - -pin = 20 - -def GPIOsetup(): - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(pin,GPIO.OUT) - -def restart(): - GPIOsetup() - GPIO.output(pin,1) - print("[Dynamixels]: OFF") - time.sleep(3) - GPIO.output(pin,0) - print("[Dynamixels]: ON") - return diff --git a/Server/Server-Code-2019/NewRoboCupServer-Current.py b/Server/Server-Code-2019/NewRoboCupServer-Current.py deleted file mode 100644 index 0c37d6d..0000000 --- a/Server/Server-Code-2019/NewRoboCupServer-Current.py +++ /dev/null @@ -1,77 +0,0 @@ -import socketserver as SocketServer -import sys -from GPIO import * -from emuBot import * -from xl320 import * - -numberOfActiveDynamixels = 13 - -def loopServoReset(setup): - for ID in range(1,numberOfActiveDynamixels+1): - try: - softwareResetServo(ID) - if ID == numberOfActiveDynamixels: - print("---------- // -----------") - if setup: - if (ID <= 4) or ID == 11: - wheelMode(ID) - elif ID == 12 or ID == 13: - cameraMode(ID) - else: - jointMode(ID) - except: - print("Setup of ID: " + str(ID) + " failed.") - -loopServoReset(False)#True) -cameraMode(12) -cameraMode(13) -lastValues = [0 for i in range(20)] - - -class MyUCPHandler(SocketServer.BaseRequestHandler): - def handle(self): - self.data = self.request[0].decode('utf-8').strip() - new = self.data.split('\n') - for i in new: - if i != "": - if 0: - print(new) - else: - if i == "restart": - restart() - elif i == "softwareResetServos": - loopServoReset(False) - else: - ID = int(i[0]+i[1]) - if (ID <= 4) or (ID == 11): - try: - if lastValues[ID] != int(i[2:]): - moveWheel(ID, int(i[2:])) - lastValues[ID] = int(i[2:]) - except: - print('brokeWheel ',ID) - else: - pos, speed = map(int, [round(float(x)) for x in i.split()[1:]]) - if ID == 12 or ID == 13: - try: - moveCamera(ID, pos) - except: - print("BrokeCamera", ID) - else: - try: - moveJoint(ID, pos, speed) - except: - print(i[2:-4]) - print('BrokeJoint ', ID) - # print(i) - # else: - # print(i) - -if __name__ == "__main__": - HOST, PORT = "", 9999 - -SocketServer.UDPServer.allow_reuse_address = True -server = SocketServer.UDPServer((HOST, PORT), MyUCPHandler) - -print('Dynamixel Server Started') -server.serve_forever() diff --git a/Server/Server-Code-2019/RoboCupServer-Current.py b/Server/Server-Code-2019/RoboCupServer-Current.py deleted file mode 100644 index 76e6f31..0000000 --- a/Server/Server-Code-2019/RoboCupServer-Current.py +++ /dev/null @@ -1,67 +0,0 @@ -import socketserver as SocketServer -import sys -from GPIO import * -from emuBot import * - -numberOfActiveDynamixels = 9 - -def loopServoReset(setup): - for ID in range(1,numberOfActiveDynamixels+1): - try: - softwareResetServo(ID) - if ID == numberOfActiveDynamixels: - print("---------- // -----------") - if setup: - if (ID <= 4) or (ID == 11): - wheelMode(ID) - else: - jointMode(ID) - except: - print("Setup of ID: " + str(ID) + " failed.") - -loopServoReset(True) - -lastValues = [0 for i in range(20)] - - -class MyUCPHandler(SocketServer.BaseRequestHandler): - def handle(self): - self.data = self.request[0].decode('utf-8').strip() - new = self.data.split('\n') - for i in new: - if i != "": - if 0: - print(new) - else: - if i == "restart": - restart() - elif i == "softwareResetServos": - loopServoReset(False) - else: - ID = int(i[0]+i[1]) - if (ID <= 4) or (ID == 11): - try: - if lastValues[ID] != int(i[2:]): - moveWheel(ID, int(i[2:])) - lastValues[ID] = int(i[2:]) - except: - print('brokeWheel ',ID) - else: - pos, speed = map(int, [round(float(x)) for x in i.split()[1:]]) - try: - moveJoint(ID, pos, speed) - except: - print(i[2:-4]) - print('BrokeJoint ', ID) - # print(i) - # else: - # print(i) - -if __name__ == "__main__": - HOST, PORT = "", 9999 - -SocketServer.UDPServer.allow_reuse_address = True -server = SocketServer.UDPServer((HOST, PORT), MyUCPHandler) - -print('Dynamixel Server Started') -server.serve_forever() diff --git a/Server/Server-Code-2019/__pycache__/GPIO.cpython-34.pyc b/Server/Server-Code-2019/__pycache__/GPIO.cpython-34.pyc deleted file mode 100644 index 62a4dc2..0000000 Binary files a/Server/Server-Code-2019/__pycache__/GPIO.cpython-34.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc deleted file mode 100644 index 1a1bb7c..0000000 Binary files a/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/GPIO.cpython-37.pyc b/Server/Server-Code-2019/__pycache__/GPIO.cpython-37.pyc deleted file mode 100644 index dbfb920..0000000 Binary files a/Server/Server-Code-2019/__pycache__/GPIO.cpython-37.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/emuBot.cpython-34.pyc b/Server/Server-Code-2019/__pycache__/emuBot.cpython-34.pyc deleted file mode 100644 index 1b98707..0000000 Binary files a/Server/Server-Code-2019/__pycache__/emuBot.cpython-34.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc deleted file mode 100644 index ddbcb21..0000000 Binary files a/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/emuBot.cpython-37.pyc b/Server/Server-Code-2019/__pycache__/emuBot.cpython-37.pyc deleted file mode 100644 index fe5d71d..0000000 Binary files a/Server/Server-Code-2019/__pycache__/emuBot.cpython-37.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc deleted file mode 100644 index 74dfc54..0000000 Binary files a/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc and /dev/null differ diff --git a/Server/Server-Code-2019/camera.py b/Server/Server-Code-2019/camera.py deleted file mode 100644 index 95c11ae..0000000 --- a/Server/Server-Code-2019/camera.py +++ /dev/null @@ -1,129 +0,0 @@ -import os, sys - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - tty.setraw(sys.stdin.fileno()) - def getch(): - return sys.stdin.read(1) - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_PRO_GOAL_POSITION = 30 -ADDR_PRO_PRESENT_POSITION = 37 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 13 # Dynamixel ID: 1 -BAUDRATE = 1000000 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8') # Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" - -TORQUE_ENABLE = 1 # Value for enabling the torque -TORQUE_DISABLE = 0 # Value for disabling the torque -DXL_MINIMUM_POSITION_VALUE = 205 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 819 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -COMM_SUCCESS = 0 # Communication Success result value -COMM_TX_FAIL = -1001 # Communication Tx Failed - -# Initialize PortHandler Structs -# Set the port path -# Get methods and members of PortHandlerLinux or PortHandlerWindows -port_num = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl_present_position = 0 # Present position - -# Open port -if dynamixel.openPort(port_num): - print("Succeeded to open the port!") -else: - print("Failed to open the port!") - print("Press any key to terminate...") - getch() - quit() - -# Set port baudrate -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: - dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) -elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: - dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) -else: - print("Dynamixel has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write goal position - dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) - if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: - dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) - elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: - dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) - - while 1: - # Read present position - dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION) - if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: - dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) - elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: - dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) - - if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: - dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) -elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: - dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/Server/Server-Code-2019/emuBot.py b/Server/Server-Code-2019/emuBot.py deleted file mode 100644 index 97409de..0000000 --- a/Server/Server-Code-2019/emuBot.py +++ /dev/null @@ -1,129 +0,0 @@ -import time -import threading - -from dynamixel_sdk import * - -BAUDRATE = 1000000 - -portHandler = PortHandler("/dev/ttyUSB2") -packetHandler = PacketHandler(1.0) - -if portHandler.openPort(): - # print("Succeeded to open the port" - pass -else: - print("Failed to open the port") - print("Press any key to terminate...") - getch() - quit() - -if portHandler.setBaudRate(BAUDRATE): - # print("Succeeded to change the baudrate") - pass -else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - getch() - quit() - -def dxlErrors(commResult, error): - if commResult != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(commResult)) - elif error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - -def softwareResetServo(ID): - #print("Restarting Servo ID: " + str(ID)) - - # Disables the torque on the motor - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 24, 0) - dxlErrors(dxl_comm_result, dxl_error) - - # Turns off the servo LED. - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 25, 0) - dxlErrors(dxl_comm_result, dxl_error) - - # Sets the torque limit back to its maximum - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 34, 1023) - dxlErrors(dxl_comm_result, dxl_error) - - - # Enables the torque on the motor - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 24, 1) - dxlErrors(dxl_comm_result, dxl_error) - - - -def jointMode(ID): - if ID < 9: - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 6, 0) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 8, 4095) - dxlErrors(dxl_comm_result, dxl_error) - - elif ID == 9: - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 6, 0) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 8, 4095) - dxlErrors(dxl_comm_result, dxl_error) - - elif ID > 9: - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 6, 0) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 8, 1023) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 24, 1) - dxlErrors(dxl_comm_result, dxl_error) - - - -def wheelMode(ID): - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 6, 0) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 8, 0) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 24, 1) - dxlErrors(dxl_comm_result, dxl_error) - - - -def moveJoint(ID, position,speed): - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 30, position) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, ID, 36) - dxlErrors(dxl_comm_result, dxl_error) - #if not abs(dxl_goal_position[index] - dxl_present_position) > 20: - #print("Servo with ID of %03d was %03d off target, outside acceptable range" % (ID, abs(position - dxl_present_position))) - -def moveWheel(ID, speed): - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, 24, 0) - dxlErrors(dxl_comm_result, dxl_error) - - # Convert negative values of speed to between 1024 and 2047 - if speed < 0: - # Limit allowed reverse speed to prevent errors - if speed < -1024: - speed = 2047 - else: - speed = 1023 + -speed - - else: - if speed > 1023: - # Limit allowed forward speed to prevent errors - speed = 1023 - - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 32, speed) - dxlErrors(dxl_comm_result, dxl_error) - - dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, ID, 30, 0) - dxlErrors(dxl_comm_result, dxl_error) - -#jointMode(6) diff --git a/Server/Server-Code-2019/xl320.py b/Server/Server-Code-2019/xl320.py deleted file mode 100644 index 52ce1cb..0000000 --- a/Server/Server-Code-2019/xl320.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -################################################################################ -# Copyright 2017 ROBOTIS CO., LTD. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -################################################################################ - -# Author: Ryu Woon Jung (Leon) - -# -# ********* Read and Write Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 2.0 -# This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(emanual.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os -from dynamixel_sdk import * # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_PRO_GOAL_POSITION = 30 -ADDR_PRO_PRESENT_POSITION = 37 - -# Protocol version -PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 13 # Dynamixel ID : 1 -BAUDRATE = 1000000 # Dynamixel default baudrate : 57600 -DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -TORQUE_ENABLE = 1 # Value for enabling the torque -TORQUE_DISABLE = 0 # Value for disabling the torque -DXL_MINIMUM_POSITION_VALUE = 205 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 819 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -index = 0 -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - - -# Initialize cameraPortHandler instance -# Set the port path -# Get methods and members of cameraPortHandlerLinux or cameraPortHandlerWindows -cameraPortHandler = PortHandler(DEVICENAME) - -# Initialize PacketHandler instance -# Set the protocol version -# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler -packetHandler = PacketHandler(PROTOCOL_VERSION) - -# Open port -if cameraPortHandler.openPort(): - #print("Succeeded to open the port") - pass -else: - print("Failed to open the port") - print("Press any key to terminate...") - getch() - quit() - - -# Set port baudrate -if cameraPortHandler.setBaudRate(BAUDRATE): - #print("Succeeded to change the baudrate") - pass -else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - getch() - quit() - -ID = 12 -goal_position = 500 -def cameraMode(ID): - # Enable Dynamixel Torque - dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) - if dxl_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("%s" % packetHandler.getRxPacketError(dxl_error)) - else: - pass - #print("Dynamixel has been successfully connected") - -cameraMode(12) -cameraMode(13) -def moveCamera(ID, goal_position): - #print("Press any key to continue! (or press ESC to quit!)") - #if getch() == chr(0x1b): - #break - # Write goal position - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(cameraPortHandler, ID, ADDR_PRO_GOAL_POSITION, goal_position) - if dxl_comm_result != COMM_SUCCESS: - print("111%s" % packetHandler.getTxRxResult(dxl_comm_result)) - elif dxl_error != 0: - print("222%s" % packetHandler.getRxPacketError(dxl_error)) - - # Read present position - dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(cameraPortHandler, ID, ADDR_PRO_PRESENT_POSITION) - #if dxl_comm_result != COMM_SUCCESS: - #print("333%s" % packetHandler.getTxRxResult(dxl_comm_result)) - if dxl_error != 0: - print("444%s" % packetHandler.getRxPacketError(dxl_error)) - - #print("555[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) - - # if not abs(goal_position - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD: - # print('Broken :(') - - -# moveCamera(12, 1023) -# moveCamera(13, 500) -# # Disable Dynamixel Torque -# dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -# if dxl_comm_result != COMM_SUCCESS: -# print("666%s" % packetHandler.getTxRxResult(dxl_comm_result)) -# elif dxl_error != 0: -# print("777%s" % packetHandler.getRxPacketError(dxl_error)) - -# # Close port -# cameraPortHandler.closePort() diff --git a/Server/Server-Sensor-Code/ClientToRecieve.py b/Server/Server-Sensor-Code/ClientToRecieve.py deleted file mode 100644 index c5415b6..0000000 --- a/Server/Server-Sensor-Code/ClientToRecieve.py +++ /dev/null @@ -1,16 +0,0 @@ -import socket -import sys - -HOST, PORT = "127.0.0.1", 25565 -data = "GET" - -# SOCK_DGRAM is the socket type to use for UDP sockets -sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - -# As you can see, there is no connect() call; UDP has no connections. -# Instead, data is directly sent to the recipient via sendto(). -sock.sendto(bytes(data + "\n", "utf-8"), (HOST, PORT)) -received = str(sock.recv(1024), "utf-8") - -print("Sent: {}".format(data)) -print("Received: {}".format(received)) diff --git a/Server/Server-Sensor-Code/__pycache__/py_read_serial.cpython-36.pyc b/Server/Server-Sensor-Code/__pycache__/py_read_serial.cpython-36.pyc deleted file mode 100644 index 70ff275..0000000 Binary files a/Server/Server-Sensor-Code/__pycache__/py_read_serial.cpython-36.pyc and /dev/null differ diff --git a/Server/Server-Sensor-Code/py_read_serial.py b/Server/Server-Sensor-Code/py_read_serial.py deleted file mode 100644 index 4ba557a..0000000 --- a/Server/Server-Sensor-Code/py_read_serial.py +++ /dev/null @@ -1,25 +0,0 @@ -import serial as s -''' "serialdata" is the reading of serial data - from the arduino,plugged in by USB ''' -serialdata = s.Serial('/dev/ttyUSB0',9600) - -def readPins(): - ''' "string" is the line of data read from the ardiuno. ''' - string = str(serialdata.readline()).replace("b'","") - ''' sensor_num is the pin number the sensor is plugged into. - sensor_value is the analog value read from the sensor. ''' - sensor_num = string[0] - sensor_value = '' - ''' This loop goes though each character of "string" - and retrives the analog value from the string ''' - for x in range(1,len(string)): - if string[x] != '\\': - sensor_value = sensor_value + string[x] - else: - break - ''' "sensor" is a dictionary that has two attributes, "value" and "num" ''' - sensor = {} - sensor['value'] = int(sensor_value) - sensor['num'] = int(sensor_num) - ''' This returns the sensor dictionay so it can be used outside the function. ''' - return sensor diff --git a/Server/Server-Sensor-Code/sensors.py b/Server/Server-Sensor-Code/sensors.py deleted file mode 100644 index 7863f2a..0000000 --- a/Server/Server-Sensor-Code/sensors.py +++ /dev/null @@ -1,33 +0,0 @@ -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -from py_read_serial import * -import socket - -UDP_IP_ADDRESS = '127.0.0.1' -UDP_PORT_NO = 6789 -clientSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - -messages = { - 0: 'CO2: ', - 1: 'Temperature: ' -} - -numOfSensors = len(messages) - -while 1: - for x in range(numOfSensors): - while 1: - try: - sensor = readPins() # sensor = {'num': sensorID, 'value': some number} - break - except: - pass - if sensor['num'] == 1: - sensor['value'] = round((57*(sensor['value']-20))/100,1) - print(messages[sensor['num']][0], sensor['value'], messages[sensor['num']][1]) - # output = "" - # output += messages[sensor['num']][0] - # output += sensors['value'] - # clientSock.sendto(bytes(, 'utf-8'), (UDP_IP_ADDRESS, UDP_PORT_NO)) diff --git a/Server/index.js b/Server/index.js new file mode 100644 index 0000000..339daec --- /dev/null +++ b/Server/index.js @@ -0,0 +1,49 @@ +const { spawn } = require('child_process'); +const dgram = require('dgram'); +const server = dgram.createSocket('udp4'); +let client = ""; + +const commands = { + 'Driving': [ + 'python3', + ['-u', './Driving/server.py'] + ] +} + +let children = {}; + +function handleListeners(data, index, name) { + console.log(`${index} ${name}: ${data}`); + server.send(`${index} ${name}: ${data}`, 5002, client); +} + +function addListeners(index) { + const outputs = [children[index].stdout, children[index].stderr]; + for (let i = 0; i < outputs.length; i++) { + outputs[i].on('data', (data) => + handleListeners(data, index, 'stdout')); + } + + children[index].on('close', (code) => { + console.log(`Process '${index}' exited with code ${code}`); + server.send(`Process '${index}' exited with code ${code}`, 5002, client); + }); +} + +server.on('listening', () => { + console.log(`Server listening on port ${server.address().port}`); +}); + +server.on('message', (msg, rinfo) => { + if (client !== rinfo.address) { + console.log(`Switching saved IP from '${client}' to '${rinfo.address}'`); + client = rinfo.address; + } + if (commands.hasOwnProperty(msg)) { + children[msg] = spawn(commands[msg][0], commands[msg][1]); + addListeners(msg); + console.log(`Started function: ${msg}`) + } +}); + +server.bind(5000); \ No newline at end of file diff --git a/Server/package-lock.json b/Server/package-lock.json deleted file mode 100644 index 0f31d20..0000000 --- a/Server/package-lock.json +++ /dev/null @@ -1,414 +0,0 @@ -{ - "name": "Server-Code", - "version": "1.0.0", - "lockfileVersion": 1, - "requires": true, - "dependencies": { - "accepts": { - "version": "1.3.7", - "resolved": "https://registry.npmjs.org/accepts/-/accepts-1.3.7.tgz", - "integrity": "sha512-Il80Qs2WjYlJIBNzNkK6KYqlVMTbZLXgHx2oT0pU/fjRHyEp+PEfEPY0R3WCwAGVOtauxh1hOxNgIf5bv7dQpA==", - "requires": { - "mime-types": "~2.1.24", - "negotiator": "0.6.2" - } - }, - "array-flatten": { - "version": "1.1.1", - "resolved": "https://registry.npmjs.org/array-flatten/-/array-flatten-1.1.1.tgz", - "integrity": "sha1-ml9pkFGx5wczKPKgCJaLZOopVdI=" - }, - "body-parser": { - "version": "1.19.0", - "resolved": "https://registry.npmjs.org/body-parser/-/body-parser-1.19.0.tgz", - "integrity": "sha512-dhEPs72UPbDnAQJ9ZKMNTP6ptJaionhP5cBb541nXPlW60Jepo9RV/a4fX4XWW9CuFNK22krhrj1+rgzifNCsw==", - "requires": { - "bytes": "3.1.0", - "content-type": "~1.0.4", - "debug": "2.6.9", - "depd": "~1.1.2", - "http-errors": "1.7.2", - "iconv-lite": "0.4.24", - "on-finished": "~2.3.0", - "qs": "6.7.0", - "raw-body": "2.4.0", - "type-is": "~1.6.17" - } - }, - "bytes": { - "version": "3.1.0", - "resolved": "https://registry.npmjs.org/bytes/-/bytes-3.1.0.tgz", - "integrity": "sha512-zauLjrfCG+xvoyaqLoV8bLVXXNGC4JqlxFCutSDWA6fJrTo2ZuvLYTqZ7aHBLZSMOopbzwv8f+wZcVzfVTI2Dg==" - }, - "callsite": { - "version": "1.0.0", - "resolved": "https://registry.npmjs.org/callsite/-/callsite-1.0.0.tgz", - "integrity": "sha1-KAOY5dZkvXQDi28JBRU+borxvCA=" - }, - "colors": { - "version": "1.3.3", - "resolved": "https://registry.npmjs.org/colors/-/colors-1.3.3.tgz", - "integrity": "sha512-mmGt/1pZqYRjMxB1axhTo16/snVZ5krrKkcmMeVKxzECMMXoCgnvTPp10QgHfcbQZw8Dq2jMNG6je4JlWU0gWg==" - }, - "content-disposition": { - "version": "0.5.3", - "resolved": "https://registry.npmjs.org/content-disposition/-/content-disposition-0.5.3.tgz", - "integrity": "sha512-ExO0774ikEObIAEV9kDo50o+79VCUdEB6n6lzKgGwupcVeRlhrj3qGAfwq8G6uBJjkqLrhT0qEYFcWng8z1z0g==", - "requires": { - "safe-buffer": "5.1.2" - } - }, - "content-type": { - "version": "1.0.4", - "resolved": "https://registry.npmjs.org/content-type/-/content-type-1.0.4.tgz", - "integrity": "sha512-hIP3EEPs8tB9AT1L+NUqtwOAps4mk2Zob89MWXMHjHWg9milF/j4osnnQLXBCBFBk/tvIG/tUc9mOUJiPBhPXA==" - }, - "cookie": { - "version": "0.4.0", - "resolved": "https://registry.npmjs.org/cookie/-/cookie-0.4.0.tgz", - "integrity": "sha512-+Hp8fLp57wnUSt0tY0tHEXh4voZRDnoIrZPqlo3DPiI4y9lwg/jqx+1Om94/W6ZaPDOUbnjOt/99w66zk+l1Xg==" - }, - "cookie-signature": { - "version": "1.0.6", - "resolved": "https://registry.npmjs.org/cookie-signature/-/cookie-signature-1.0.6.tgz", - "integrity": "sha1-4wOogrNCzD7oylE6eZmXNNqzriw=" - }, - "debug": { - "version": "2.6.9", - "resolved": "https://registry.npmjs.org/debug/-/debug-2.6.9.tgz", - "integrity": "sha512-bC7ElrdJaJnPbAP+1EotYvqZsb3ecl5wi6Bfi6BJTUcNowp6cvspg0jXznRTKDjm/E7AdgFBVeAPVMNcKGsHMA==", - "requires": { - "ms": "2.0.0" - } - }, - "depd": { - "version": "1.1.2", - "resolved": "https://registry.npmjs.org/depd/-/depd-1.1.2.tgz", - "integrity": "sha1-m81S4UwJd2PnSbJ0xDRu0uVgtak=" - }, - "destroy": { - "version": "1.0.4", - "resolved": "https://registry.npmjs.org/destroy/-/destroy-1.0.4.tgz", - "integrity": "sha1-l4hXRCxEdJ5CBmE+N5RiBYJqvYA=" - }, - "ee-first": { - "version": "1.1.1", - "resolved": "https://registry.npmjs.org/ee-first/-/ee-first-1.1.1.tgz", - "integrity": "sha1-WQxhFWsK4vTwJVcyoViyZrxWsh0=" - }, - "encodeurl": { - "version": "1.0.2", - "resolved": "https://registry.npmjs.org/encodeurl/-/encodeurl-1.0.2.tgz", - "integrity": "sha1-rT/0yG7C0CkyL1oCw6mmBslbP1k=" - }, - "escape-html": { - "version": "1.0.3", - "resolved": "https://registry.npmjs.org/escape-html/-/escape-html-1.0.3.tgz", - "integrity": "sha1-Aljq5NPQwJdN4cFpGI7wBR0dGYg=" - }, - "etag": { - "version": "1.8.1", - "resolved": "https://registry.npmjs.org/etag/-/etag-1.8.1.tgz", - "integrity": "sha1-Qa4u62XvpiJorr/qg6x9eSmbCIc=" - }, - "express": { - "version": "4.17.1", - "resolved": "https://registry.npmjs.org/express/-/express-4.17.1.tgz", - "integrity": "sha512-mHJ9O79RqluphRrcw2X/GTh3k9tVv8YcoyY4Kkh4WDMUYKRZUq0h1o0w2rrrxBqM7VoeUVqgb27xlEMXTnYt4g==", - "requires": { - "accepts": "~1.3.7", - "array-flatten": "1.1.1", - "body-parser": "1.19.0", - "content-disposition": "0.5.3", - "content-type": "~1.0.4", - "cookie": "0.4.0", - "cookie-signature": "1.0.6", - "debug": "2.6.9", - "depd": "~1.1.2", - "encodeurl": "~1.0.2", - "escape-html": "~1.0.3", - "etag": "~1.8.1", - "finalhandler": "~1.1.2", - "fresh": "0.5.2", - "merge-descriptors": "1.0.1", - "methods": "~1.1.2", - "on-finished": "~2.3.0", - "parseurl": "~1.3.3", - "path-to-regexp": "0.1.7", - "proxy-addr": "~2.0.5", - "qs": "6.7.0", - "range-parser": "~1.2.1", - "safe-buffer": "5.1.2", - "send": "0.17.1", - "serve-static": "1.14.1", - "setprototypeof": "1.1.1", - "statuses": "~1.5.0", - "type-is": "~1.6.18", - "utils-merge": "1.0.1", - "vary": "~1.1.2" - } - }, - "finalhandler": { - "version": "1.1.2", - "resolved": "https://registry.npmjs.org/finalhandler/-/finalhandler-1.1.2.tgz", - "integrity": "sha512-aAWcW57uxVNrQZqFXjITpW3sIUQmHGG3qSb9mUah9MgMC4NeWhNOlNjXEYq3HjRAvL6arUviZGGJsBg6z0zsWA==", - "requires": { - "debug": "2.6.9", - "encodeurl": "~1.0.2", - "escape-html": "~1.0.3", - "on-finished": "~2.3.0", - "parseurl": "~1.3.3", - "statuses": "~1.5.0", - "unpipe": "~1.0.0" - } - }, - "forwarded": { - "version": "0.1.2", - "resolved": "https://registry.npmjs.org/forwarded/-/forwarded-0.1.2.tgz", - "integrity": "sha1-mMI9qxF1ZXuMBXPozszZGw/xjIQ=" - }, - "fresh": { - "version": "0.5.2", - "resolved": "https://registry.npmjs.org/fresh/-/fresh-0.5.2.tgz", - "integrity": "sha1-PYyt2Q2XZWn6g1qx+OSyOhBWBac=" - }, - "http-errors": { - "version": "1.7.2", - "resolved": "https://registry.npmjs.org/http-errors/-/http-errors-1.7.2.tgz", - "integrity": "sha512-uUQBt3H/cSIVfch6i1EuPNy/YsRSOUBXTVfZ+yR7Zjez3qjBz6i9+i4zjNaoqcoFVI4lQJ5plg63TvGfRSDCRg==", - "requires": { - "depd": "~1.1.2", - "inherits": "2.0.3", - "setprototypeof": "1.1.1", - "statuses": ">= 1.5.0 < 2", - "toidentifier": "1.0.0" - } - }, - "iconv-lite": { - "version": "0.4.24", - "resolved": "https://registry.npmjs.org/iconv-lite/-/iconv-lite-0.4.24.tgz", - "integrity": "sha512-v3MXnZAcvnywkTUEZomIActle7RXXeedOR31wwl7VlyoXO4Qi9arvSenNQWne1TcRwhCL1HwLI21bEqdpj8/rA==", - "requires": { - "safer-buffer": ">= 2.1.2 < 3" - } - }, - "inherits": { - "version": "2.0.3", - "resolved": "https://registry.npmjs.org/inherits/-/inherits-2.0.3.tgz", - "integrity": "sha1-Yzwsg+PaQqUC9SRmAiSA9CCCYd4=" - }, - "ipaddr.js": { - "version": "1.9.0", - "resolved": "https://registry.npmjs.org/ipaddr.js/-/ipaddr.js-1.9.0.tgz", - "integrity": "sha512-M4Sjn6N/+O6/IXSJseKqHoFc+5FdGJ22sXqnjTpdZweHK64MzEPAyQZyEU3R/KRv2GLoa7nNtg/C2Ev6m7z+eA==" - }, - "media-typer": { - "version": "0.3.0", - "resolved": "https://registry.npmjs.org/media-typer/-/media-typer-0.3.0.tgz", - "integrity": "sha1-hxDXrwqmJvj/+hzgAWhUUmMlV0g=" - }, - "merge-descriptors": { - "version": "1.0.1", - "resolved": "https://registry.npmjs.org/merge-descriptors/-/merge-descriptors-1.0.1.tgz", - "integrity": "sha1-sAqqVW3YtEVoFQ7J0blT8/kMu2E=" - }, - "methods": { - "version": "1.1.2", - "resolved": "https://registry.npmjs.org/methods/-/methods-1.1.2.tgz", - "integrity": "sha1-VSmk1nZUE07cxSZmVoNbD4Ua/O4=" - }, - "mime": { - "version": "1.6.0", - "resolved": "https://registry.npmjs.org/mime/-/mime-1.6.0.tgz", - "integrity": "sha512-x0Vn8spI+wuJ1O6S7gnbaQg8Pxh4NNHb7KSINmEWKiPE4RKOplvijn+NkmYmmRgP68mc70j2EbeTFRsrswaQeg==" - }, - "mime-db": { - "version": "1.40.0", - "resolved": "https://registry.npmjs.org/mime-db/-/mime-db-1.40.0.tgz", - "integrity": "sha512-jYdeOMPy9vnxEqFRRo6ZvTZ8d9oPb+k18PKoYNYUe2stVEBPPwsln/qWzdbmaIvnhZ9v2P+CuecK+fpUfsV2mA==" - }, - "mime-types": { - "version": "2.1.24", - "resolved": "https://registry.npmjs.org/mime-types/-/mime-types-2.1.24.tgz", - "integrity": "sha512-WaFHS3MCl5fapm3oLxU4eYDw77IQM2ACcxQ9RIxfaC3ooc6PFuBMGZZsYpvoXS5D5QTWPieo1jjLdAm3TBP3cQ==", - "requires": { - "mime-db": "1.40.0" - } - }, - "minimist": { - "version": "0.0.8", - "resolved": "https://registry.npmjs.org/minimist/-/minimist-0.0.8.tgz", - "integrity": "sha1-hX/Kv8M5fSYluCKCYuhqp6ARsF0=" - }, - "mkdirp": { - "version": "0.5.1", - "resolved": "https://registry.npmjs.org/mkdirp/-/mkdirp-0.5.1.tgz", - "integrity": "sha1-MAV0OOrGz3+MR2fzhkjWaX11yQM=", - "requires": { - "minimist": "0.0.8" - } - }, - "moment": { - "version": "2.24.0", - "resolved": "https://registry.npmjs.org/moment/-/moment-2.24.0.tgz", - "integrity": "sha512-bV7f+6l2QigeBBZSM/6yTNq4P2fNpSWj/0e7jQcy87A8e7o2nAfP/34/2ky5Vw4B9S446EtIhodAzkFCcR4dQg==" - }, - "ms": { - "version": "2.0.0", - "resolved": "https://registry.npmjs.org/ms/-/ms-2.0.0.tgz", - "integrity": "sha1-VgiurfwAvmwpAd9fmGF4jeDVl8g=" - }, - "negotiator": { - "version": "0.6.2", - "resolved": "https://registry.npmjs.org/negotiator/-/negotiator-0.6.2.tgz", - "integrity": "sha512-hZXc7K2e+PgeI1eDBe/10Ard4ekbfrrqG8Ep+8Jmf4JID2bNg7NvCPOZN+kfF574pFQI7mum2AUqDidoKqcTOw==" - }, - "on-finished": { - "version": "2.3.0", - "resolved": "https://registry.npmjs.org/on-finished/-/on-finished-2.3.0.tgz", - "integrity": "sha1-IPEzZIGwg811M3mSoWlxqi2QaUc=", - "requires": { - "ee-first": "1.1.1" - } - }, - "parseurl": { - "version": "1.3.3", - "resolved": "https://registry.npmjs.org/parseurl/-/parseurl-1.3.3.tgz", - "integrity": "sha512-CiyeOxFT/JZyN5m0z9PfXw4SCBJ6Sygz1Dpl0wqjlhDEGGBP1GnsUVEL0p63hoG1fcj3fHynXi9NYO4nWOL+qQ==" - }, - "path-to-regexp": { - "version": "0.1.7", - "resolved": "https://registry.npmjs.org/path-to-regexp/-/path-to-regexp-0.1.7.tgz", - "integrity": "sha1-32BBeABfUi8V60SQ5yR6G/qmf4w=" - }, - "proxy-addr": { - "version": "2.0.5", - "resolved": "https://registry.npmjs.org/proxy-addr/-/proxy-addr-2.0.5.tgz", - "integrity": "sha512-t/7RxHXPH6cJtP0pRG6smSr9QJidhB+3kXu0KgXnbGYMgzEnUxRQ4/LDdfOwZEMyIh3/xHb8PX3t+lfL9z+YVQ==", - "requires": { - "forwarded": "~0.1.2", - "ipaddr.js": "1.9.0" - } - }, - "qs": { - "version": "6.7.0", - "resolved": "https://registry.npmjs.org/qs/-/qs-6.7.0.tgz", - "integrity": "sha512-VCdBRNFTX1fyE7Nb6FYoURo/SPe62QCaAyzJvUjwRaIsc+NePBEniHlvxFmmX56+HZphIGtV0XeCirBtpDrTyQ==" - }, - "range-parser": { - "version": "1.2.1", - "resolved": "https://registry.npmjs.org/range-parser/-/range-parser-1.2.1.tgz", - "integrity": "sha512-Hrgsx+orqoygnmhFbKaHE6c296J+HTAQXoxEF6gNupROmmGJRoyzfG3ccAveqCBrwr/2yxQ5BVd/GTl5agOwSg==" - }, - "raw-body": { - "version": "2.4.0", - "resolved": "https://registry.npmjs.org/raw-body/-/raw-body-2.4.0.tgz", - "integrity": "sha512-4Oz8DUIwdvoa5qMJelxipzi/iJIi40O5cGV1wNYp5hvZP8ZN0T+jiNkL0QepXs+EsQ9XJ8ipEDoiH70ySUJP3Q==", - "requires": { - "bytes": "3.1.0", - "http-errors": "1.7.2", - "iconv-lite": "0.4.24", - "unpipe": "1.0.0" - } - }, - "safe-buffer": { - "version": "5.1.2", - "resolved": "https://registry.npmjs.org/safe-buffer/-/safe-buffer-5.1.2.tgz", - "integrity": "sha512-Gd2UZBJDkXlY7GbJxfsE8/nvKkUEU1G38c1siN6QP6a9PT9MmHB8GnpscSmMJSoF8LOIrt8ud/wPtojys4G6+g==" - }, - "safer-buffer": { - "version": "2.1.2", - "resolved": "https://registry.npmjs.org/safer-buffer/-/safer-buffer-2.1.2.tgz", - "integrity": "sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg==" - }, - "scribe-js": { - "version": "2.0.4", - "resolved": "https://registry.npmjs.org/scribe-js/-/scribe-js-2.0.4.tgz", - "integrity": "sha1-a1GbdVdqCXjCd/ghanRSVs/yTQs=", - "requires": { - "callsite": "^1.0.0", - "colors": "^1.0.0", - "express": "^4.0.0", - "mkdirp": "^0.5.0", - "moment": "^2.8.0" - } - }, - "send": { - "version": "0.17.1", - "resolved": "https://registry.npmjs.org/send/-/send-0.17.1.tgz", - "integrity": "sha512-BsVKsiGcQMFwT8UxypobUKyv7irCNRHk1T0G680vk88yf6LBByGcZJOTJCrTP2xVN6yI+XjPJcNuE3V4fT9sAg==", - "requires": { - "debug": "2.6.9", - "depd": "~1.1.2", - "destroy": "~1.0.4", - "encodeurl": "~1.0.2", - "escape-html": "~1.0.3", - "etag": "~1.8.1", - "fresh": "0.5.2", - "http-errors": "~1.7.2", - "mime": "1.6.0", - "ms": "2.1.1", - "on-finished": "~2.3.0", - "range-parser": "~1.2.1", - "statuses": "~1.5.0" - }, - "dependencies": { - "ms": { - "version": "2.1.1", - "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.1.tgz", - "integrity": "sha512-tgp+dl5cGk28utYktBsrFqA7HKgrhgPsg6Z/EfhWI4gl1Hwq8B/GmY/0oXZ6nF8hDVesS/FpnYaD/kOWhYQvyg==" - } - } - }, - "serve-static": { - "version": "1.14.1", - "resolved": "https://registry.npmjs.org/serve-static/-/serve-static-1.14.1.tgz", - "integrity": "sha512-JMrvUwE54emCYWlTI+hGrGv5I8dEwmco/00EvkzIIsR7MqrHonbD9pO2MOfFnpFntl7ecpZs+3mW+XbQZu9QCg==", - "requires": { - "encodeurl": "~1.0.2", - "escape-html": "~1.0.3", - "parseurl": "~1.3.3", - "send": "0.17.1" - } - }, - "setprototypeof": { - "version": "1.1.1", - "resolved": "https://registry.npmjs.org/setprototypeof/-/setprototypeof-1.1.1.tgz", - "integrity": "sha512-JvdAWfbXeIGaZ9cILp38HntZSFSo3mWg6xGcJJsd+d4aRMOqauag1C63dJfDw7OaMYwEbHMOxEZ1lqVRYP2OAw==" - }, - "statuses": { - "version": "1.5.0", - "resolved": "https://registry.npmjs.org/statuses/-/statuses-1.5.0.tgz", - "integrity": "sha1-Fhx9rBd2Wf2YEfQ3cfqZOBR4Yow=" - }, - "toidentifier": { - "version": "1.0.0", - "resolved": "https://registry.npmjs.org/toidentifier/-/toidentifier-1.0.0.tgz", - "integrity": "sha512-yaOH/Pk/VEhBWWTlhI+qXxDFXlejDGcQipMlyxda9nthulaxLZUNcUqFxokp0vcYnvteJln5FNQDRrxj3YcbVw==" - }, - "type-is": { - "version": "1.6.18", - "resolved": "https://registry.npmjs.org/type-is/-/type-is-1.6.18.tgz", - "integrity": "sha512-TkRKr9sUTxEH8MdfuCSP7VizJyzRNMjj2J2do2Jr3Kym598JVdEksuzPQCnlFPW4ky9Q+iA+ma9BGm06XQBy8g==", - "requires": { - "media-typer": "0.3.0", - "mime-types": "~2.1.24" - } - }, - "unpipe": { - "version": "1.0.0", - "resolved": "https://registry.npmjs.org/unpipe/-/unpipe-1.0.0.tgz", - "integrity": "sha1-sr9O6FFKrmFltIF4KdIbLvSZBOw=" - }, - "utils-merge": { - "version": "1.0.1", - "resolved": "https://registry.npmjs.org/utils-merge/-/utils-merge-1.0.1.tgz", - "integrity": "sha1-n5VxD1CiZ5R7LMwSR0HBAoQn5xM=" - }, - "vary": { - "version": "1.1.2", - "resolved": "https://registry.npmjs.org/vary/-/vary-1.1.2.tgz", - "integrity": "sha1-IpnwLG3tMNSllhsLn3RSShj2NPw=" - } - } -} diff --git a/Server/package.json b/Server/package.json index 8a17c02..39c2a6d 100644 --- a/Server/package.json +++ b/Server/package.json @@ -1,16 +1,11 @@ { - "name": "Server-Code", + "name": "robohud-server", "version": "1.0.0", - "description": "", - "main": "server.js", + "description": "The server side of the CCGS Robotics RoboHUD stack", + "main": "index.js", "scripts": { - "test": "echo \"Error: no test specified\" && exit 1", - "start": "rm -rf logs/; node test.js", - "clear-logs": "rm -rf logs/" + "test": "echo \"Error: no test specified\" && exit 1" }, - "author": "", - "license": "ISC", - "dependencies": { - "scribe-js": "^2.0.4" - } + "author": "Angus Finch", + "license": "ISC" } diff --git a/Server/sensors.js b/Server/sensors.js deleted file mode 100644 index 2b4c446..0000000 --- a/Server/sensors.js +++ /dev/null @@ -1,19 +0,0 @@ -const { StringDecoder } = require('string_decoder'); -const decoder = new StringDecoder('utf-8'); - -var dgram = require('dgram'); -var server = dgram.createSocket('udp4'); - -var PORT = 6789; -var HOST = '127.0.0.1'; - -server.on('listening', function () { - var address = server.address(); - console.log('UDP Server listening on ' + address.address + ":" + address.port); -}); - -server.on('message', function (message, remote) { - console.log(decoder.end(Buffer.from(message))); -}); - -server.bind(PORT, HOST); \ No newline at end of file diff --git a/Server/server.js b/Server/server.js deleted file mode 100644 index f7b9d11..0000000 --- a/Server/server.js +++ /dev/null @@ -1,65 +0,0 @@ -var net = require('net'); -var execute = require('child_process').exec; - -function execute(command) { - command = String(command); - - exec(command, function (error, stdout, stderr) { - console.log("Stdout: " + stdout); - console.error("Stderr: " + stderr); - if (error !== null) { - if (hasErrored < 999999999) { - console.error("Execution Error. Try looking at the documentation for possible sources of error. If the problem persists, please report this with the following error messages") - hasErrored += 1 - } else { - console.error("Execution Error, but max prompt limit reached. " + error) - } - } - }); -}; - -console.log('Server interface started and listening on port 5000') -net.createServer(function(socket) { - console.log("New client connected"); - execute("pkill python3; pkill raspivid; pkill nc"); - socket.on('data', function(data) { - var parsedData = data.toString() - console.log(parsedData) - if (parsedData == 'START') { - console.log('Starting Python server...'); - var serverCode = execute('python3 Server-Code-2019/RoboCupServer-Current.py') - - serverCode.stdout.on('data', function (data) { - console.log('stdout: ' + data.toString()); - }); - - serverCode.stderr.on('data', function (data) { - console.log('stderr: ' + data.toString()); - }); - - serverCode.on('exit', function (code) { - console.log('child process exited with code ' + code.toString()); - }); - } else if (parsedData == 'VIDEO') { - console.log('Streaming video to client...'); - var raspivid = execute('raspivid -t 0 -o - -w 960 -h 540 -fps 25 -pf baseline -vf -hf | nc -k -l 5002'); - raspivid.stdout.on('data', function (data) { - console.log('stdout: ' + data.toString()); - }); - - raspivid.stderr.on('data', function (data) { - console.log('stderr: ' + data.toString()); - }); - - raspivid.on('exit', function (code) { - console.log('child process exited with code ' + code.toString()); - }); - } - }) - // TODO: Terminate the child process directly instead of using pkill - socket.on('end', function() { - console.log("Client has terminated connection, killing Python server..."); - execute("pkill python3"); - execute("pkill raspivid; pkill nc"); - }) -}).listen(5000) diff --git a/Server/test.js b/Server/test.js deleted file mode 100644 index 90b3fc7..0000000 --- a/Server/test.js +++ /dev/null @@ -1,119 +0,0 @@ -var net = require('net'); -const { spawn } = require('child_process') - -// console.log('Server interface started and listening on port 5000') -// var code = execute('python3 test.py') -// code.stdout.on("data", function(data) { -// console.log(data) -// }) - -var scribe = require('scribe-js')(), - console = process.console, - express = require('express'), - app = express(); - - -app.set('port', (process.env.PORT || 5000)); - -app.get('/', function(req, res) { - res.send('Hello world, see you at /logs'); -}); - -app.use('/logs', scribe.webPanel()); - -// Feel free to mess around with the colours, this is just what I could think of -console.addLogger('debug', 'purple'); -// --------------------------------- -console.addLogger('stdout', 'green'); -console.addLogger('out', 'green'); -// --------------------------------- -console.addLogger('stderr', 'red'); -console.addLogger('error', 'red'); -// --------------------------------- -console.addLogger('client', 'blue'); - -var port = app.get("port"); - -app.listen(port, function() { - console.time().debug('Server listening at port ' + port); -}); - -function spawnNewChild(command, args) { - var child = spawn(command, args); - - child.stdout.on('data', (data) => { - console.time().stdout(`Child stdout:\n${data}`); - }); - - child.stderr.on('data', (data) => { - console.time().stderr(`Child stderr:\n${data}`); - }); - - child.on('exit', function (code, signal) { - if (signal == null) console.time().stdout('Child process terminated successfully'); - else console.time().debug(`Child process exited with code ${code} and signal ${signal}`); - }); - - child.on('error', function(data) { - console.time().err(data) - }) - - return child; -} -setTimeout(function() { - // var pythonServer = spawnNewChild('python3', ['./Server-Code-2018/RoboCupServer-Current.py']); - spawnNewChild('python3', ['count.py']) -}, 1500) - -// console.time().fun('hello world'); -// console.tag('This is a test').debug('A test'); -// console.tag('An object').log({ -// a: 'b', -// c: [1, 2, 3] -// }); - -// setTimeout() -// net.createServer(function(socket) { -// console.log("New client connected"); -// execute("pkill python3; pkill raspivid; pkill nc"); -// socket.on('data', function(data) { -// var parsedData = data.toString() -// console.log(parsedData) -// if (parsedData == 'START') { -// console.log('Starting Python server...'); -// var serverCode = execute('python3 Server-Code-2018/RoboCupServer-Current.py') -// -// serverCode.stdout.on('data', function (data) { -// console.log('stdout: ' + data.toString()); -// }); -// -// serverCode.stderr.on('data', function (data) { -// console.log('stderr: ' + data.toString()); -// }); -// -// serverCode.on('exit', function (code) { -// console.log('child process exited with code ' + code.toString()); -// }); -// } else if (parsedData == 'VIDEO') { -// console.log('Streaming video to client...'); -// var raspivid = execute('raspivid -t 0 -o - -w 960 -h 540 -fps 25 -pf baseline -vf -hf | nc -k -l 5002'); -// raspivid.stdout.on('data', function (data) { -// console.log('stdout: ' + data.toString()); -// }); -// -// raspivid.stderr.on('data', function (data) { -// console.log('stderr: ' + data.toString()); -// }); -// -// raspivid.on('exit', function (code) { -// console.log('child process exited with code ' + code.toString()); -// }); -// } -// }) -// // TODO: Terminate the child process directly instead of using pkill -// socket.on('end', function() { -// console.log("Client has terminated connection, killing Python server..."); -// execute("pkill python3"); -// execute("pkill raspivid; pkill nc"); -// }) -// }).listen(5000)