diff --git a/compLib/Servo.py b/compLib/Servo.py
index cf50d43..69cae04 100644
--- a/compLib/Servo.py
+++ b/compLib/Servo.py
@@ -1,36 +1,64 @@
-# from compLib.PCA9685 import PCA9685
-
-# SERVO_COUNT = 10
-
-# pwm = PCA9685(0x40, debug=True)
-# pwm.setPWMFreq(50)
-
-# MIN_ANGLE = -90.0
-# MAX_ANGLE = 90.0
+from compLib.Spi import Spi, Register
 
 
-# class Servo:
-#     """Control the servo ports on the robot
-#     """
+SERVO_COUNT = 8
 
-#     @staticmethod
-#     def set_position(channel: int, angle: int, offset: float =90):
-#         """Set position of servo connected to port
+MIN_ANGLE = -90.0
+MAX_ANGLE = 90.0
 
-#         :param channel: channel between 0 and 7
-#         :param angle: Angle of servo
-#         """
-#         if channel < 0 or channel >= SERVO_COUNT:
-#             raise IndexError("Invalid Servo channel specified!")
+
+def map(x, in_min, in_max, out_min, out_max):
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
+
+
+class Servo:
+    """Control the servo ports on the robot
+    """
+
+    @staticmethod
+    def set_position(port: int, angle: float, offset: float = 0.0):
+        """Set position of servo connected to port
+
+        :param port: port between 1 and 8
+        :param angle: Angle of servo
+        """
+        if port <= 0 or port > SERVO_COUNT:
+            raise IndexError("Invalid Servo port specified!")
         
-#         angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
-        
-#         pwm.setServoPulse(channel + 8, 500+int((angle+offset)/0.09))
+        angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
 
-#     @staticmethod
-#     def setup_position():
-#         """Set position of servos to the position used during the setup process
-#         """
+        mapped_angle = int(map(angle, MIN_ANGLE, MAX_ANGLE, 0, 65535))
+        print(mapped_angle)
+
+        if port == 1:
+            Spi.write(Register.SERVO_1_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_1_CTRL, 1, 4)
+        elif port == 2:
+            Spi.write(Register.SERVO_2_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_1_CTRL, 1, 4)
+        elif port == 3:
+            Spi.write(Register.SERVO_3_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_2_CTRL, 1, 4)
+        elif port == 4:
+            Spi.write(Register.SERVO_4_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_2_CTRL, 1, 4)
+        elif port == 5:
+            Spi.write(Register.SERVO_5_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_3_CTRL, 1, 4)
+        elif port == 6:
+            Spi.write(Register.SERVO_6_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_3_CTRL, 1, 4)
+        elif port == 7:
+            Spi.write(Register.SERVO_7_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_4_CTRL, 1, 4)
+        elif port == 8:
+            Spi.write(Register.SERVO_8_PWM_H, 2, mapped_angle)
+            Spi.write(Register.PWM_4_CTRL, 1, 4)
+
+    @staticmethod
+    def setup_position():
+        """Set position of servos to the position used during the setup process
+        """
         
-#         Servo.set_position(0, 0)
-#         Servo.set_position(1, 0)
+        for i in range(1, SERVO_COUNT + 1):
+            Servo.set_position(i, 0)