diff --git a/code/tcp-server/ServoClient.py b/code/tcp-server/ServoClient.py
new file mode 100644
index 0000000000000000000000000000000000000000..40048e6467cdf890b1a6c72290c626960eef898e
--- /dev/null
+++ b/code/tcp-server/ServoClient.py
@@ -0,0 +1,63 @@
+# general
+from time import sleep
+
+#server
+import socket
+
+class ServoClient(object):
+ """
+ A class that implements a servo-client which is capable of sending servo speed-settings
+ to an omnibot.
+
+ HOST: Host name of server (string)
+ PORT: Port to connect to (int)
+ verbose: choose level of chit-chat (boolean)
+ """
+ def __init__(self,HOST,PORT,verbose=False):
+ self.HOST = HOST
+ self.PORT = PORT
+ self.verbose=verbose
+ self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+ def __enter__(self):
+ """
+ Boot up the socket and connect to the servo server
+ """
+ if self.verbose:
+ print("Connecting to HOST: " + str(self.HOST) + ", PORT: " + str(self.PORT))
+
+ self.s.connect((self.HOST, self.PORT)) # Connect to the robot
+ if self.verbose:
+ print("Connection successful.")
+
+ sleep(0.1)# sleep to let servos boot
+ return self
+
+ def __exit__(self, exc_type, exc_value, exc_tb):
+ """
+ Close down the socket after closing the client
+ """
+ if self.verbose:
+ print("Closing down socket")
+ self.s.close()
+
+ def get_max_velocity(self):
+ """TODO: function for retreving max velocity of servos"""
+ pass
+
+ def set_speed(self,i,v):
+ """
+ Set the speed of servo i to v
+ """
+ package = str(i)+":"+str(v)
+ if self.verbose:
+ print("Sending " + package)
+
+ # Send the package
+ self.s.sendall(bytes(package,'utf-8'))
+ # Receive confirmation
+ ret=self.s.recv(1024).decode('utf-8')
+ if ret[0] == "e":
+ raise Exception(ret)
+ elif self.verbose:
+ print(ret)
diff --git a/code/tcp-server/omnibot_controller.py b/code/tcp-server/omnibot_controller.py
new file mode 100644
index 0000000000000000000000000000000000000000..2184949d47a2bf6b50e3181f03dac154fac081af
--- /dev/null
+++ b/code/tcp-server/omnibot_controller.py
@@ -0,0 +1,126 @@
+# Import package for communicating with omnibot
+from ServoClient import ServoClient
+
+# general
+import numpy as np
+import sys
+from time import time, sleep
+
+# threads
+import threading
+import logging
+
+# Controller
+from inputs import get_gamepad
+
+
+class ControllerState:
+ """
+ Class that logs the controller state
+ """
+ def __init__(self):
+ """ Initialize the controller """
+ self.x = 128
+ self.y = 128
+ self.r = 128
+ self.on = True
+
+ def set_x(self,x):
+ self.x = x
+ def set_y(self,y):
+ self.y = y
+ def set_r(self,r):
+ self.r = r
+ def state_data(self):
+ return 'x'+str(self.x)+'y'+str(self.y)+'r'+str(self.r)
+ def set_state(self,data):
+ s = str(data)
+ ind = [s.find(i) for i in ['x','y','r']]
+ self.set_x(int(s[ind[0]+1:ind[1]]))
+ self.set_y(int(s[ind[1]+1:ind[2]]))
+ self.set_r(int(s[ind[2]+1:-1]))
+
+def log_controller(state):
+ """
+ Function that continuously listens to any event from the controller, and logs the change in
+ the controller state "state"
+ """
+ logging.info("Starting controller logging")
+ while 1:
+ events = get_gamepad()
+ for event in events:
+ if event.code == "ABS_X":
+ state.set_x(event.state)
+ elif event.code == "ABS_Y":
+ state.set_y(event.state)
+ elif event.code == "ABS_Z":
+ state.set_r(event.state)
+ elif event.code == "BTN_PINKIE" and event.state == 1:
+ # The "BTN_PINKIE" (right index trigger)
+ # is currently used as an off-button.
+ state.on = False
+
+ logging.info("Stopping controller logging")
+
+
+if __name__ == '__main__':
+
+ # Logger format
+ format = "%(asctime)s: %(message)s"
+ logging.basicConfig(format=format, level=logging.INFO,datefmt="%H:%M:%S")
+
+ # Server settings
+ HOST, PORT = "localhost", 9998
+
+ if len(sys.argv) > 1:
+ # If an input is given to the script, it will be interpreted as the intended
+ # IP-address. Baseline is localhost.
+ HOST = sys.argv[1]
+
+ logging.info(f"HOST: \t {HOST}")
+ logging.info(f"PORT: \t {PORT}")
+
+
+ # Robot parameters
+ R = 0.16 # Distance between center of robot and wheels
+ a1 = 2*np.pi/3 # Angle between x axis and first wheel
+ a2 = 4*np.pi/3 # Angle between x axis and second wheel
+ r = 0.028*0.45/18 # Wheel radius. Has been fudge-factored because the actual velocity of the wheels did not align with the set-points.
+
+
+ vel_factor = 4 # Fudge factor that changes the relative velocity of the bot
+
+ def phidot(xdot,ang):
+ """Returns reference velocities for the wheels, given current system state and reference velocities"""
+ M = -1/r*np.array([[-np.sin(ang), np.cos(ang), R ],[-np.sin(ang+a1), np.cos(ang+a1), R],[-np.sin(ang+a2), np.cos(ang+a2), R]])
+ return M.dot(xdot)
+
+ # Start loggin controller
+ state = ControllerState()
+ t = threading.Thread(target=log_controller, args=(state,), daemon=True)
+ t.start()
+
+ ts = 0.1 # sampling time
+
+ with ServoClient(HOST,PORT,verbose=True) as s:
+
+ no_error = True
+ while state.on and no_error:
+ t0 = time()
+
+ # Get gamepad updates.
+ # Use some scaling to turn the controller state values into
+ # reasonable velocities.
+ w = (128-state.r)*np.pi/1000
+ vy = (128-state.x)/1500
+ vx = (128-state.y)/1500
+ dphi = vel_factor*phidot([vx,vy,w],0.0)
+
+ for i,v in enumerate(dphi):
+ s.set_speed(i,int(np.round(v)))
+
+ sleep(max(0,t0+ts-time()))
+
+
+
+
diff --git a/code/tcp-server/servo_server.py b/code/tcp-server/servo_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..1429f250e0c79fd729e6fbbe9bcc1c47475f8cd2
--- /dev/null
+++ b/code/tcp-server/servo_server.py
@@ -0,0 +1,92 @@
+import numpy as np
+import sys
+from time import time, sleep
+import logging
+
+# Dynamixel packages
+from dynamixel.model.xm430_w210_t_r import XM430_W210_T_R
+import dynamixel.channel
+VEL_MAX = 1022 #maximum velocity set-point (max possible value is 1022)
+
+#server
+import socket
+
+def load_servos():
+ """
+ Servo load function, based on the dynamixel implementation of Anders Blomdell.
+ """
+
+ # The speed and channel numbers can be found and checked via the dynamixel software "dynamixel wizard".
+ # The device can be found by e.g. lsusb, and is currently adapted for the three-color robots.
+ channel = dynamixel.channel.Channel(speed=57600,device='/dev/ttyACM0')
+ servos = [ XM430_W210_T_R(channel, 1),
+ XM430_W210_T_R(channel, 2),
+ XM430_W210_T_R(channel, 3) ]
+ clear_servos(servos)
+
+ return servos
+
+def clear_servos(servos):
+ """
+ Clear whatchdog error and write maximum velocity to the servos.z
+ """
+ for s in servos:
+ s.torque_enable.write(0)
+ print(s.model_number.read(), s.id.read())
+ s.velocity_limit.write(VEL_MAX+1) # +1 for padding because it seemed to struggle with negative values
+ s.operating_mode.write(1)
+ s.bus_watchdog.write(0) # Clear old watchdog error
+ s.bus_watchdog.write(100) # 2 second timeout
+ s.torque_enable.write(1)
+
+if __name__ == "__main__":
+
+ # Set IP (HOST) and port number (PORT)
+ # "" means listening to any available IP
+ HOST, PORT = "", 9998
+
+ # Define loggin format
+ format = "%(asctime)s: %(message)s"
+ logging.basicConfig(format=format, level=logging.INFO,datefmt="%H:%M:%S")
+
+ # Create socket s
+ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+ s.bind((HOST, PORT)) # Bind the socket to specified port and ip
+ s.listen(1) # Start listening for connections
+ logging.info("Waiting for connection")
+ while True:
+ conn, addr = s.accept() # Wait for connection
+
+ with conn: # Connection received
+ logging.info(f"Connection with {addr}")
+ logging.info("Loading servos")
+ servos = load_servos() # Load the servos
+
+ while True:
+ data = conn.recv(1024)
+ if not data:
+ logging.info("Empty input, stopping connection")
+ break
+
+ try:
+ inp = data.decode('utf-8')
+ logging.info(f"Received {inp}")
+ i = int(inp[0])
+ v = int(inp[2:])
+
+ # Check if received velocity is out of range. If so, return
+ # info that velocity was changed
+ if np.abs(v) > VEL_MAX:
+ conn.sendall(b"i:velocity")
+ v = VEL_MAX*np.sign(v)
+ else:
+ conn.sendall(data)
+ servos[i].goal_velocity.write(v)
+
+ except Exception as e:
+ # If an exception is encountered, send it to the client.
+ logging.info(f"Exception encountered: {e}")
+ message = "e:"+str(e)
+ conn.sendall(message.encode('utf-8'))
+
+
\ No newline at end of file
diff --git a/code/tcp-server/servo_server_launcher.sh b/code/tcp-server/servo_server_launcher.sh
new file mode 100644
index 0000000000000000000000000000000000000000..3f5395f9ab9cf4a8aec423df2599d531e1c16ca1
--- /dev/null
+++ b/code/tcp-server/servo_server_launcher.sh
@@ -0,0 +1,6 @@
+#!/bin/sh
+# servo_server_launcher.sh
+# This script navigates to the ServoServer directory and then launches the server.
+
+cd ~/ServoServer
+sudo python servo_server.py
\ No newline at end of file