diff --git a/.gitignore b/.gitignore
index e69de29..9f5fbbb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -0,0 +1,3 @@
+.vscode
+
+**/_pycache_/
\ No newline at end of file
diff --git a/__pycache__/StatProfiler.cpython-39.pyc b/__pycache__/StatProfiler.cpython-39.pyc
deleted file mode 100644
index f193269..0000000
Binary files a/__pycache__/StatProfiler.cpython-39.pyc and /dev/null differ
diff --git a/__pycache__/TControl.cpython-38.pyc b/__pycache__/TControl.cpython-38.pyc
deleted file mode 100644
index 90818e2..0000000
Binary files a/__pycache__/TControl.cpython-38.pyc and /dev/null differ
diff --git a/__pycache__/TControl.cpython-39.pyc b/__pycache__/TControl.cpython-39.pyc
deleted file mode 100644
index c2af075..0000000
Binary files a/__pycache__/TControl.cpython-39.pyc and /dev/null differ
diff --git a/__pycache__/TMotorManager.cpython-39.pyc b/__pycache__/TMotorManager.cpython-39.pyc
deleted file mode 100644
index 1d31127..0000000
Binary files a/__pycache__/TMotorManager.cpython-39.pyc and /dev/null differ
diff --git a/demos/servo_serial/demo_2DOF_duty_servo_serial.py b/demos/servo_serial/demo_2DOF_duty_servo_serial.py
new file mode 100644
index 0000000..e6d35c9
--- /dev/null
+++ b/demos/servo_serial/demo_2DOF_duty_servo_serial.py
@@ -0,0 +1,23 @@
+from TMotorCANControl.servo_serial import *
+from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
+
+duty = 0.1
+
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev1:
+    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', motor_params=Servo_Params_Serial['AK80-9']) as dev2:
+        loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
+        dev1.set_zero_position()
+        dev2.set_zero_position()
+        dev1.update()
+        dev2.update()
+        
+        dev1.enter_duty_cycle_control()
+        dev2.enter_duty_cycle_control()
+        for t in loop:
+            dev1.set_duty_cycle_percent(duty)
+            dev2.set_duty_cycle_percent(-duty)
+            dev1.update()
+            dev2.update()
+            print(f"\r {dev1} {dev2}", end='')
+
+        
\ No newline at end of file
diff --git a/demos/servo_serial/demo_2DOF_servo_serial.py b/demos/servo_serial/demo_2DOF_servo_serial.py
index 4ab0836..e217172 100644
--- a/demos/servo_serial/demo_2DOF_servo_serial.py
+++ b/demos/servo_serial/demo_2DOF_servo_serial.py
@@ -1,12 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 vel = 1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev1:
-    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev2:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev1:
+    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', motor_params=Servo_Params_Serial['AK80-9']) as dev2:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev1.enter_position_control()
         dev2.enter_idle_mode()
diff --git a/demos/servo_serial/demo_PD_current_servo_serial.py b/demos/servo_serial/demo_PD_current_servo_serial.py
index d5891b5..4bcd447 100644
--- a/demos/servo_serial/demo_PD_current_servo_serial.py
+++ b/demos/servo_serial/demo_PD_current_servo_serial.py
@@ -1,8 +1,7 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
-import numpy as np
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
+import numpy as np
+
 current = 0
 
 Pdes = 0
@@ -11,7 +10,7 @@
 P = 2
 D = 0.3
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.02, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
@@ -20,7 +19,7 @@
         
         for t in loop:
             Pdes = 3*np.sin(t)
-            cmd = P*(Pdes - dev.θ ) + D*(Vdes - dev.θd)
+            cmd = P*(Pdes - dev.position ) + D*(Vdes - dev.velocity)
             dev.current_qaxis = cmd
             dev.update()
             print(f"\r {dev}", end='')
diff --git a/demos/servo_serial/demo_PD_duty_servo_serial.py b/demos/servo_serial/demo_PD_duty_servo_serial.py
index 89abebf..f8cc162 100644
--- a/demos/servo_serial/demo_PD_duty_servo_serial.py
+++ b/demos/servo_serial/demo_PD_duty_servo_serial.py
@@ -1,5 +1,3 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
@@ -12,7 +10,7 @@
 P = 0.1
 D = 0.001
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
diff --git a/demos/servo_serial/demo_ROS2_node_servo_serial.py b/demos/servo_serial/demo_ROS2_node_servo_serial.py
new file mode 100644
index 0000000..13a7943
--- /dev/null
+++ b/demos/servo_serial/demo_ROS2_node_servo_serial.py
@@ -0,0 +1,141 @@
+# Example ROS2 Node that can switch between control modes via ros topics 
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+from std_msgs.msg import Float32MultiArray
+
+from TMotorCANControl.servo_serial import *
+
+CONTROL_MODES = {
+    "POSITION": 0,
+    "VELOCITY": 1,
+    "CURRENT":  2,
+    "DUTY":     3,
+}
+
+class MotorNode(Node):
+    def __init__(self, device):
+        super().__init__('motor_node')
+
+        self.device = device
+        self.dev_name = "dev"
+        self.mode = 10   
+        self.motor_data = 0
+
+        self.device.set_zero_position()
+        self.device.update()
+
+        self.velocity_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/velocity",
+            self.velocity_cb,
+            10
+        )
+
+        self.duty_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/duty",
+            self.duty_cb,
+            10
+        )
+
+        self.position_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/position",
+            self.position_cb,
+            10
+        )
+
+        self.current_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/current",
+            self.current_cb,
+            10
+        )
+
+        self.output_pub = self.create_publisher(
+            Float32MultiArray,
+            f"{self.dev_name}/output",
+            10
+        )
+        
+        self.timer = self.create_timer(0.01, self.timer_callback)
+
+    def timer_callback(self):
+        if self.mode == CONTROL_MODES["VELOCITY"]:
+            self.device.set_output_velocity_radians_per_second(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["DUTY"]:
+            self.device.set_duty_cycle_percent(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["POSITION"]:
+            self.device.set_output_angle_radians(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["CURRENT"]:
+            self.device.current_qaxis = self.motor_data
+            self.device.update()
+
+        output_angle = self.device.get_output_angle_radians()
+        output_velocity = self.device.get_output_velocity_radians_per_second()
+        output_acceleration = self.device.get_output_acceleration_radians_per_second_squared()
+        output_torque = self.device.get_output_torque_newton_meters()
+        
+        output_msg = Float32MultiArray()
+        output_msg.data = [output_angle, output_velocity, output_acceleration, output_torque]
+        self.output_pub.publish(output_msg)
+
+    def velocity_cb(self, msg):
+        
+        if self.mode != CONTROL_MODES["VELOCITY"]:
+            self.mode = CONTROL_MODES["VELOCITY"]
+            self.device.enter_velocity_control()
+            self.device.update()
+            print("vel")
+            
+        self.motor_data = msg.data
+
+    def duty_cb(self, msg):
+        
+        if self.mode != CONTROL_MODES["DUTY"]:
+            self.mode = CONTROL_MODES["DUTY"]
+            self.device.enter_duty_cycle_control()
+            self.device.update()
+            print("duty")
+            print(msg.data)
+            
+        self.motor_data = msg.data
+
+    def position_cb(self, msg):
+        
+        if self.mode != CONTROL_MODES["POSITION"]:
+            self.mode = CONTROL_MODES["POSITION"]
+            self.device.enter_position_control()
+            self.device.update()
+            print("position")
+            
+        self.motor_data = msg.data
+
+    def current_cb(self, msg):
+        
+        if self.mode != CONTROL_MODES["CURRENT"]:
+            self.mode = CONTROL_MODES["CURRENT"]
+            self.device.enter_current_control()
+            self.device.update()
+            print("current")
+            
+        self.motor_data = msg.data
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    with TMotorManager_servo_serial(port = "/dev/ttyUSB0") as dev:
+        motor_node = MotorNode(device = dev)
+        rclpy.spin(motor_node)
+
+    rclpy.shutdown()
+    
+if __name__ == "__main__":
+    main()
diff --git a/demos/servo_serial/demo_current_step_servo_serial.py b/demos/servo_serial/demo_current_step_servo_serial.py
index 9693c66..49cefc8 100644
--- a/demos/servo_serial/demo_current_step_servo_serial.py
+++ b/demos/servo_serial/demo_current_step_servo_serial.py
@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 current = 1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
diff --git a/demos/servo_serial/demo_duty_servo_serial.py b/demos/servo_serial/demo_duty_servo_serial.py
index 0f6a5a9..a6bb7fc 100644
--- a/demos/servo_serial/demo_duty_servo_serial.py
+++ b/demos/servo_serial/demo_duty_servo_serial.py
@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 duty = 0.1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
diff --git a/demos/servo_serial/demo_idle_servo_serial.py b/demos/servo_serial/demo_idle_servo_serial.py
index 9bf6d80..0a94d2b 100644
--- a/demos/servo_serial/demo_idle_servo_serial.py
+++ b/demos/servo_serial/demo_idle_servo_serial.py
@@ -1,13 +1,7 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
-baud=921600
-port='/dev/ttyUSB0'
-motor_params = Servo_Params_Serial['AK80-9']
-
-with TMotorManager_servo_serial(port=port, baud=baud, motor_params=motor_params) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
diff --git a/demos/servo_serial/check_motor_connection_servo_serial.py b/demos/servo_serial/demo_motor_connection_servo_serial.py
similarity index 50%
rename from demos/servo_serial/check_motor_connection_servo_serial.py
rename to demos/servo_serial/demo_motor_connection_servo_serial.py
index 72c5da9..dd4b64a 100644
--- a/demos/servo_serial/check_motor_connection_servo_serial.py
+++ b/demos/servo_serial/demo_motor_connection_servo_serial.py
@@ -1,10 +1,6 @@
-from TMotorCANControl.servo_serial import TMotorManager_servo_serial
+from TMotorCANControl.servo_serial import *
 
-# CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK10-9'
-ID = 0
-
-with TMotorManager_servo_serial(motor_type=Type, motor_ID=ID) as dev:
+with TMotorManager_servo_serial(port= '/dev/ttyUSB0', motor_params=Servo_Params_Serial["AK80-9"]) as dev:
     if dev.check_connection():
         print("\nmotor is successfully connected!\n")
     else:
diff --git a/demos/servo_serial/demo_position_step_servo_serial.py b/demos/servo_serial/demo_position_step_servo_serial.py
index 291ce32..6298ed0 100644
--- a/demos/servo_serial/demo_position_step_servo_serial.py
+++ b/demos/servo_serial/demo_position_step_servo_serial.py
@@ -1,11 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
+
 pos = 0
 
-with TMotorManager_servo_serial(port='/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         
         dev.set_zero_position()
diff --git a/demos/servo_serial/demo_position_tracking_servo_serial.py b/demos/servo_serial/demo_position_tracking_servo_serial.py
index 291ce32..6298ed0 100644
--- a/demos/servo_serial/demo_position_tracking_servo_serial.py
+++ b/demos/servo_serial/demo_position_tracking_servo_serial.py
@@ -1,11 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
+
 pos = 0
 
-with TMotorManager_servo_serial(port='/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         
         dev.set_zero_position()
diff --git a/demos/servo_serial/demo_velocity_servo_serial.py b/demos/servo_serial/demo_velocity_servo_serial.py
index 226131d..2a73a8a 100644
--- a/demos/servo_serial/demo_velocity_servo_serial.py
+++ b/demos/servo_serial/demo_velocity_servo_serial.py
@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
-vel = 1
+vel = 9
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.enter_velocity_control()
         
diff --git a/src/TMotorCANControl.egg-info/requires.txt b/src/TMotorCANControl.egg-info/requires.txt
index f81380d..9eeb2f9 100644
--- a/src/TMotorCANControl.egg-info/requires.txt
+++ b/src/TMotorCANControl.egg-info/requires.txt
@@ -1,4 +1,4 @@
-numpy
 NeuroLocoMiddleware
-python-can>=4.0.0
+numpy
 pyserial>=3.5
+python-can>=4.0.0
diff --git a/src/TMotorCANControl/__pycache__/CAN_Manager.cpython-39.pyc b/src/TMotorCANControl/__pycache__/CAN_Manager.cpython-39.pyc
deleted file mode 100644
index c1743f9..0000000
Binary files a/src/TMotorCANControl/__pycache__/CAN_Manager.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/CAN_Manager_mit.cpython-39.pyc b/src/TMotorCANControl/__pycache__/CAN_Manager_mit.cpython-39.pyc
deleted file mode 100644
index 2e40339..0000000
Binary files a/src/TMotorCANControl/__pycache__/CAN_Manager_mit.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-38.pyc b/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-38.pyc
deleted file mode 100644
index a24cd16..0000000
Binary files a/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-39.pyc b/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-39.pyc
deleted file mode 100644
index 58bfc07..0000000
Binary files a/src/TMotorCANControl/__pycache__/CAN_manager_servo.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/Serial_manager_servo.cpython-39.pyc b/src/TMotorCANControl/__pycache__/Serial_manager_servo.cpython-39.pyc
deleted file mode 100644
index 9f2647a..0000000
Binary files a/src/TMotorCANControl/__pycache__/Serial_manager_servo.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager.cpython-39.pyc b/src/TMotorCANControl/__pycache__/TMotorManager.cpython-39.pyc
deleted file mode 100644
index 8bee5b0..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager_mit_can.cpython-39.pyc b/src/TMotorCANControl/__pycache__/TMotorManager_mit_can.cpython-39.pyc
deleted file mode 100644
index 9798201..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager_mit_can.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager_servo.cpython-39.pyc b/src/TMotorCANControl/__pycache__/TMotorManager_servo.cpython-39.pyc
deleted file mode 100644
index 3f1f590..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager_servo.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager_servo_can.cpython-39.pyc b/src/TMotorCANControl/__pycache__/TMotorManager_servo_can.cpython-39.pyc
deleted file mode 100644
index cce026c..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager_servo_can.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-38.pyc b/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-38.pyc
deleted file mode 100644
index 9df917b..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-39.pyc b/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-39.pyc
deleted file mode 100644
index 0217e6a..0000000
Binary files a/src/TMotorCANControl/__pycache__/TMotorManager_servo_serial.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/__init__.cpython-38.pyc b/src/TMotorCANControl/__pycache__/__init__.cpython-38.pyc
deleted file mode 100644
index bd7519b..0000000
Binary files a/src/TMotorCANControl/__pycache__/__init__.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/__init__.cpython-39.pyc b/src/TMotorCANControl/__pycache__/__init__.cpython-39.pyc
deleted file mode 100644
index 345967d..0000000
Binary files a/src/TMotorCANControl/__pycache__/__init__.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/derive_torque_constants.cpython-39.pyc b/src/TMotorCANControl/__pycache__/derive_torque_constants.cpython-39.pyc
deleted file mode 100644
index 1e2f2aa..0000000
Binary files a/src/TMotorCANControl/__pycache__/derive_torque_constants.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/mit_can.cpython-38.pyc b/src/TMotorCANControl/__pycache__/mit_can.cpython-38.pyc
deleted file mode 100644
index 4cd380b..0000000
Binary files a/src/TMotorCANControl/__pycache__/mit_can.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/plot_adc_motor_log.cpython-39.pyc b/src/TMotorCANControl/__pycache__/plot_adc_motor_log.cpython-39.pyc
deleted file mode 100644
index 4c6392f..0000000
Binary files a/src/TMotorCANControl/__pycache__/plot_adc_motor_log.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/plot_log.cpython-39.pyc b/src/TMotorCANControl/__pycache__/plot_log.cpython-39.pyc
deleted file mode 100644
index b2ff15c..0000000
Binary files a/src/TMotorCANControl/__pycache__/plot_log.cpython-39.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/servo_can.cpython-38.pyc b/src/TMotorCANControl/__pycache__/servo_can.cpython-38.pyc
deleted file mode 100644
index a00ab98..0000000
Binary files a/src/TMotorCANControl/__pycache__/servo_can.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/__pycache__/servo_serial.cpython-38.pyc b/src/TMotorCANControl/__pycache__/servo_serial.cpython-38.pyc
deleted file mode 100644
index a0de647..0000000
Binary files a/src/TMotorCANControl/__pycache__/servo_serial.cpython-38.pyc and /dev/null differ
diff --git a/src/TMotorCANControl/servo_serial.py b/src/TMotorCANControl/servo_serial.py
index ad137ca..5123055 100644
--- a/src/TMotorCANControl/servo_serial.py
+++ b/src/TMotorCANControl/servo_serial.py
@@ -479,7 +479,7 @@ def data_received(self, data):
         Args:
             data: array of received data to parse
         """
-        # print(f"\n {len(data)}")
+    
         for d in data:
             if self.state == 0:
                 if d == 0x02:
@@ -512,7 +512,6 @@ def handle_packet(self, packet):
             packet: array of data to parse.
         """
         if self.motor is not None:
-            # print(packet)
             DL = packet[1]
             data = packet[2:2+DL]
             crc = buffer_get_int16(packet[2+DL:DL+4], 0)
@@ -528,14 +527,14 @@ class TMotorManager_servo_serial():
     used in the "context" of a with as block, in order to safely enter/exit
     control of the motor.
     """
-    def __init__(self, port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9'], max_mosfett_temp = 50,):
+    def __init__(self, port = '/dev/ttyUSB0', baud=921600, motor_params=Servo_Params_Serial['AK80-9'], max_mosfett_temp = 50,):
         """
         Initialize the motor manager. Note that this will not turn on the motor, 
         until __enter__ is called (automatically called in a with block)
 
         Args:
             port: the name of the serial port to connect to (ie, /dev/ttyUSB0, COM3, etc)
-            baud: the baud rate to use for connection. Should always be 961200 as far as I can tell.
+            baud: the baud rate to use for connection. Should always be 921600 as far as I can tell.
             motor_params: A parameter dictionary defining the motor parameters, as defined above.
             max_mosfett_temp: Temperature of the mosfett above which to throw an error, in Celsius
         """
@@ -589,11 +588,11 @@ def __enter__(self):
 
             # tell the motor to send back all parameters
             # TODO expand to allow user to only request some data, could be faster
-            self.set_motor_parameter_return_format_all()
+            self.comm_set_motor_parameter_return_format_all()
             self.send_command()
 
             # tell motor to send current position (for some reason current position is not in the other parameters)
-            self.begin_position_feedback()
+            self.comm_begin_position_feedback()
             self.send_command()
 
             # don't do anything else
@@ -601,7 +600,7 @@ def __enter__(self):
             self.send_command()
 
             if not self.check_connection():
-                print("device: {self.device_info_string()} not connected!")
+                print(f"device: {self.device_info_string()} not connected!")
             else:
                 print(f"device: {self.device_info_string()} successfully connected.")
 
@@ -621,7 +620,7 @@ def __exit__(self, etype, value, tb):
         self._reader_thread.stop() 
 
         # power down motor
-        self._ser.write(self.set_duty_cycle(0.0)) 
+        self._ser.write(self.comm_set_duty_cycle(0.0)) 
 
         # end serial connection
         self._ser.close() 
@@ -635,9 +634,9 @@ def check_connection(self):
         For now, just sends some parameter read commands and waits 0.2 seconds to
         see if we got a response.
         """
-        self._send_specific_command(self.get_motor_parameters())
-        self._send_specific_command(self.get_motor_parameters())
-        self._send_specific_command(self.get_motor_parameters())
+        self._send_specific_command(self.comm_get_motor_parameters())
+        self._send_specific_command(self.comm_get_motor_parameters())
+        self._send_specific_command(self.comm_get_motor_parameters())
         # slight delay to ensure connection!
         time.sleep(0.2)
         return self._updated_async
@@ -774,7 +773,7 @@ def update(self):
         self.send_command()
 
         # send the command to get parameters (message will be read in other thread)
-        self._send_specific_command(self.get_motor_parameters())
+        self._send_specific_command(self.comm_get_motor_parameters())
         
 
         # synchronize user-facing state with most recent async state
@@ -1018,7 +1017,9 @@ def comm_get_motor_parameters(self, set_command=True):
         """
         header = COMM_PACKET_ID.COMM_GET_VALUES
         data = [header]
-        cmd = bytearray(create_packet(data))
+        packet = create_packet(data)
+        cmd = bytearray(packet)
+        
         if set_command:
             self._command = cmd
         return cmd