diff --git a/requirements.txt b/requirements.txt
index 4fbb830..23d62b2 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,3 +1,6 @@
 m2r2==0.3.3
 markdown-it-py==2.1.0
 sphinx-rtd-theme==1.1.1
+
+wpilib~=2023.4.3.0
+pytest~=7.3.1
\ No newline at end of file
diff --git a/robotpy_toolkit_7407/sensors/gyro/PigeonIMU.py b/robotpy_toolkit_7407/sensors/gyro/PigeonIMU.py
index a440f84..d28e015 100644
--- a/robotpy_toolkit_7407/sensors/gyro/PigeonIMU.py
+++ b/robotpy_toolkit_7407/sensors/gyro/PigeonIMU.py
@@ -1,4 +1,4 @@
-import ctre
+from ctre.sensors import Pigeon2
 import math
 
 from robotpy_toolkit_7407.utils.units import radians
@@ -15,7 +15,7 @@ def __init__(self, port):
         Args:
             port (int): CAN ID of the Pigeon gyro
         """
-        self._gyro = ctre.Pigeon2(port)
+        self._gyro = Pigeon2(port)
         self._gyro.configMountPose(0, 0, 0)
 
     def init(self, gyro_start_angle=0):
diff --git a/robotpy_toolkit_7407/sensors/gyro/__init__.py b/robotpy_toolkit_7407/sensors/gyro/__init__.py
index 0670634..41832c8 100644
--- a/robotpy_toolkit_7407/sensors/gyro/__init__.py
+++ b/robotpy_toolkit_7407/sensors/gyro/__init__.py
@@ -1,3 +1,4 @@
 from robotpy_toolkit_7407.sensors.gyro.ADIS16448 import GyroADIS16448
 from robotpy_toolkit_7407.sensors.gyro.PigeonIMU import PigeonIMUGyro_Wrapper
-from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
\ No newline at end of file
+from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
+from robotpy_toolkit_7407.sensors.gyro.swerve_gyro import SwerveGyro
\ No newline at end of file
diff --git a/robotpy_toolkit_7407/sensors/gyro/swerve_gyro.py b/robotpy_toolkit_7407/sensors/gyro/swerve_gyro.py
new file mode 100644
index 0000000..7f4c9c8
--- /dev/null
+++ b/robotpy_toolkit_7407/sensors/gyro/swerve_gyro.py
@@ -0,0 +1,8 @@
+from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
+class SwerveGyro(BaseGyro):
+    """
+    Extendable class for swerve gyro.
+    """
+
+    def __init__(self):
+        super().__init__()
diff --git a/robotpy_toolkit_7407/subsystem_templates/drivetrain/swerve_drivetrain.py b/robotpy_toolkit_7407/subsystem_templates/drivetrain/swerve_drivetrain.py
index 7556028..acf2095 100644
--- a/robotpy_toolkit_7407/subsystem_templates/drivetrain/swerve_drivetrain.py
+++ b/robotpy_toolkit_7407/subsystem_templates/drivetrain/swerve_drivetrain.py
@@ -116,7 +116,7 @@ def _resolve_angles(target_angle: radians, initial_angle: radians) -> tuple[floa
         :param initial_angle: Initial node sensor angle
         :return: (target_sensor_angle, flipped, flip_sensor_offset)
         """
-
+        # TODO This function only has one client function. Rewrite or integrate into _calculate_swerve_node
         # Actual angle difference in radians
         diff = bounded_angle_diff(initial_angle, target_angle)
 
@@ -131,13 +131,15 @@ def _resolve_angles(target_angle: radians, initial_angle: radians) -> tuple[floa
 
 class SwerveGyro(BaseGyro):
     """
+    LEGACY VERSION OF THIS CLASS. HERE FOR BACKWARDS COMPATIBILITY WITH
+    OLD CODE. PLEASE ONLY UPDATE robotpy_toolkit_7407.sensors.gyro.swerve_gyro
+
     Extendable class for swerve gyro.
     """
 
     def __init__(self):
         super().__init__()
 
-
 class SwerveDrivetrain(Subsystem):
     """
     Swerve Drivetrain Extendable class. Contains driving functions.
@@ -329,6 +331,23 @@ def reset_odometry(self, pose: Pose2d):
     @staticmethod
     def _calculate_swerve_node(node_x: meters, node_y: meters, dx: meters_per_second, dy: meters_per_second,
                                d_theta: radians_per_second) -> (meters_per_second, radians):
+        '''
+        This is a helper method to determine the direction and speed to set the nodes
+        ONLY WORKS WITH A SQUARE ROBOT!!!
+        This should really be a node method.
+        Args:
+            node_x: x Position of a node
+            node_y: y Position of a node
+            dx: change in x desired
+            dy: change in y desired
+            d_theta: change in theta desired
+
+        Returns:
+            a tuple of the magnitude and the direction
+
+        '''
+        # TODO move this function to SwerveNode
+        # TODO Rewrite function so that the robot doesn't need to be square
         tangent_x, tangent_y = -node_y, node_x
         tangent_m = math.sqrt(tangent_x ** 2 + tangent_y ** 2)
         tangent_x /= tangent_m
diff --git a/robotpy_toolkit_7407/tests/double_solenoid_test.py b/robotpy_toolkit_7407/tests/double_solenoid_test.py
new file mode 100644
index 0000000..0d5c1ca
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/double_solenoid_test.py
@@ -0,0 +1,52 @@
+from unittest.mock import MagicMock
+from robotpy_toolkit_7407.pneumatics.pistons.double_solenoid import DoubleSolenoidPiston
+import wpilib
+import pytest
+@pytest.fixture
+def solenoid() -> DoubleSolenoidPiston:
+    # Create a Solenoid, but it has mock
+    # classes for its dependencies
+    solenoid = DoubleSolenoidPiston(1, 2, 3 )
+    solenoid.solenoid = MagicMock()
+
+    return solenoid
+def test_extend(solenoid: DoubleSolenoidPiston ):
+    # setup
+    extend=solenoid.solenoid.set
+
+    # action
+    solenoid.extend()
+
+    # assert
+    extend.assert_called_with(wpilib.DoubleSolenoid.Value.kForward)
+
+def test_retract(solenoid: DoubleSolenoidPiston ):
+    # setup
+    retract=solenoid.solenoid.set
+
+    # action
+    solenoid.retract()
+
+    # assert
+    retract.assert_called_with(wpilib.DoubleSolenoid.Value.kReverse)
+
+def test_toggle(solenoid: DoubleSolenoidPiston ):
+    # setup
+    toggle=solenoid.solenoid.toggle
+
+    # action
+    solenoid.toggle()
+
+    # assert
+    toggle.assert_called_once()
+
+def test_get(solenoid: DoubleSolenoidPiston ):
+    # setup
+    get=solenoid.solenoid.get
+
+    # action
+    value=solenoid.get_value()
+
+    # assert
+    get.assert_called_once()
+
diff --git a/robotpy_toolkit_7407/tests/joystick_test.py b/robotpy_toolkit_7407/tests/joystick_test.py
new file mode 100644
index 0000000..2594f36
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/joystick_test.py
@@ -0,0 +1,37 @@
+import pytest
+import wpilib
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.oi.joysticks import Joysticks, JoystickAxis
+
+
+def test_joysticks() -> None:
+    # setup
+    # action
+    jystks = Joysticks()
+
+    # assert
+    assert jystks.joysticks[0].getPort() == 0
+    assert jystks.joysticks[1].getPort() == 1
+
+
+@pytest.mark.parametrize(("shift", "joystick", "axis"), [
+    (3, 0, 1),
+    (2, 1, 2),
+    (0, 1, 3),
+    (1, 0, 4)
+])
+def test_joystickaxis(shift, joystick, axis, monkeypatch: MonkeyPatch) -> None:
+    # setup
+    def mock_getRawAxis(self, id):
+        return id + shift
+
+    monkeypatch.setattr(wpilib.Joystick, "getRawAxis", mock_getRawAxis)
+    # action
+    jystk = JoystickAxis(controller_id=joystick, axis_id=axis)
+    value = jystk.value
+
+    # assert
+    assert value == axis + shift
+    assert joystick == jystk.controller_id
+    assert axis == jystk.axis_id
diff --git a/robotpy_toolkit_7407/tests/subsystem_test.py b/robotpy_toolkit_7407/tests/subsystem_test.py
new file mode 100644
index 0000000..d4bb18a
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/subsystem_test.py
@@ -0,0 +1,10 @@
+from robotpy_toolkit_7407.subsystem import Subsystem
+import commands2
+def subsystem_test()->None:
+    # Setup
+
+    system = Subsystem()
+    system.init()
+    assert type(system) == commands2.SubsystemBase
+
+
diff --git a/robotpy_toolkit_7407/tests/subsystemcommand_test.py b/robotpy_toolkit_7407/tests/subsystemcommand_test.py
new file mode 100644
index 0000000..471bff6
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/subsystemcommand_test.py
@@ -0,0 +1,20 @@
+from robotpy_toolkit_7407.command import SubsystemCommand
+from robotpy_toolkit_7407.subsystem import Subsystem
+
+
+def test_subsystemcommand()->None:
+    '''
+    A test to make sure that a SubsystemCommand adds the necessary
+    required subsystem.
+
+    Asserts: That the Command has the required subsystem
+
+    '''
+    # Setup
+    my_subsystem=Subsystem()
+
+    # Action
+    my_command = SubsystemCommand(my_subsystem)
+
+    # Assert
+    assert my_subsystem in my_command.getRequirements()
\ No newline at end of file
diff --git a/robotpy_toolkit_7407/tests/swerve_drivetrain_test.py b/robotpy_toolkit_7407/tests/swerve_drivetrain_test.py
new file mode 100644
index 0000000..ff6ccff
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/swerve_drivetrain_test.py
@@ -0,0 +1,87 @@
+import pytest
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.subsystem_templates.drivetrain.swerve_drivetrain import *
+#from robotpy_toolkit_7407.utils.units import s, m, deg, rad, hour, mile, rev, meters, meters_per_second, \
+#    radians_per_second, radians
+
+# def test_swerveNode_set()->None:
+#     #TODO Finish finding the right assert
+#     # Setup
+#     node=SwerveNode()
+#
+#     # Action
+#     node.set(vel=10, angle_radians=1.2)
+#
+#     # Assert
+#     assert True
+@pytest.mark.parametrize("initial_angle, target_angle, answer", [
+    (math.pi/4 , math.pi/3, (math.pi/3, False, 0)),
+    (3, 6, (3-math.pi+3, True, math.pi)),
+     (3, -3, (3.2831853071, False, 0))
+])
+def test_resolve_angles(target_angle, initial_angle, answer)->None:
+    # Setup
+    node=SwerveNode()
+
+    # Action
+
+    result = SwerveNode._resolve_angles(target_angle=target_angle, initial_angle=initial_angle)
+
+    # Assert
+    assert result[2]==answer[2]
+    assert result[1]==answer[1]
+    assert result[0]==pytest.approx(answer[0], abs=.00001)
+
+
+
+def test_get_node_state(monkeypatch: MonkeyPatch)->None:
+    # Setup
+    node=SwerveNode()
+
+    def mock_get_motor_velocity(self):
+        return 8
+    def mock_get_turn_motor_angle(self):
+        return math.pi/4
+    monkeypatch.setattr(SwerveNode, "get_motor_velocity", mock_get_motor_velocity)
+    monkeypatch.setattr(SwerveNode, "get_turn_motor_angle", mock_get_turn_motor_angle)
+
+    # Action
+    state=node.get_node_state()
+    # Assert
+    assert type(state)==SwerveModuleState
+    assert state.speed==8
+    assert state.angle.radians()==pytest.approx(math.pi/4, abs=.0001)
+
+def test_get_node_positions(monkeypatch: MonkeyPatch)->None:
+    # Setup
+    node=SwerveNode()
+
+    def mock_get_drive_motor_traveled_distance(self):
+        return 8
+    def mock_get_turn_motor_angle(self):
+        return math.pi/4
+    monkeypatch.setattr(SwerveNode, "get_drive_motor_traveled_distance", mock_get_drive_motor_traveled_distance)
+    monkeypatch.setattr(SwerveNode, "get_turn_motor_angle", mock_get_turn_motor_angle)
+
+    # Action
+    position=node.get_node_position()
+    # Assert
+    assert type(position)==SwerveModulePosition
+    assert position.distance==8
+    assert position.angle.radians()==pytest.approx(math.pi/4, abs=.0001)
+
+# TODO Parameterize and make more cases
+def test_set_angle(monkeypatch: MonkeyPatch)->None:
+    #setup
+    node=SwerveNode()
+    node.motor_sensor_offset=1
+    def mock_set_motor_angle(self, pos):
+        node.target=pos
+
+    monkeypatch.setattr(SwerveNode, "set_motor_angle", mock_set_motor_angle)
+    #action
+    node._set_angle(-3, 3) # returns (3.2831853071, False, 0)
+
+    #assert
+    assert node.target==pytest.approx(2.2831853071)
\ No newline at end of file
diff --git a/robotpy_toolkit_7407/tests/swervegyro_test.py b/robotpy_toolkit_7407/tests/swervegyro_test.py
new file mode 100644
index 0000000..f1f0fc5
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/swervegyro_test.py
@@ -0,0 +1,88 @@
+import math
+from unittest.mock import MagicMock
+
+import pytest
+from ctre.sensors import Pigeon2
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.sensors.gyro import SwerveGyro, BaseGyro, PigeonIMUGyro_Wrapper
+
+
+@pytest.fixture
+def pigeon() -> PigeonIMUGyro_Wrapper:
+    gyro = PigeonIMUGyro_Wrapper(3)
+    gyro._gyro = MagicMock()
+    return gyro
+
+
+def test_swerve_gyro() -> None:
+    # setup
+
+    # action
+    gyro = SwerveGyro()
+    # assert
+    assert isinstance(gyro, BaseGyro)
+
+
+def test_pigeon_gyro_wrapper_init(pigeon) -> None:
+    # setup
+    # gyro = PigeonIMUGyro_Wrapper(1)
+    syaw = pigeon._gyro.setYaw
+    # action
+    pigeon.init(10)
+    # assert
+    assert syaw.called_once_with(10 / (2 * math.pi) * 360)
+
+
+def test_pigeon_get_robot_heading(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_getYaw(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getYaw", mock_getYaw)
+
+    # action
+    value = gyro.get_robot_heading()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_pigeon_robot_pitch(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_get_Pitch(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getPitch", mock_get_Pitch)
+
+    # action
+    value = gyro.get_robot_pitch()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_pigeon_robot_roll(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_get_Roll(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getRoll", mock_get_Roll)
+
+    # action
+    value = gyro.get_robot_roll()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_reset_angle(pigeon) -> None:
+    # setup
+    setyaw = pigeon._gyro.setYaw
+    # action
+    pigeon.reset_angle(math.pi / 6)
+    # assert
+    assert setyaw.called_once_with(30)
diff --git a/robotpy_toolkit_7407/tests/test_limitswitches.py b/robotpy_toolkit_7407/tests/test_limitswitches.py
new file mode 100644
index 0000000..690d8db
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/test_limitswitches.py
@@ -0,0 +1,65 @@
+import wpilib
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.sensors.limit_switches import *
+
+
+def test_limit_switch_init() -> None:
+    # setup
+    # action
+    lsw = LimitSwitch(1)
+    # assert
+    assert lsw.limit_switch.getChannel() == 1
+
+    # action
+    lsw.reverse = False
+    # assert
+    assert lsw.reverse == False
+
+
+def test_get_unreversed(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    lsw = LimitSwitch(3, False)
+
+    # lsw.limit_switch=MagicMock()
+    def mock_get(self) -> bool:
+        return True
+
+    monkeypatch.setattr(wpilib.DigitalInput, "get", mock_get)
+
+    value = lsw.get_value()
+
+    # assert
+    assert value == True
+
+
+def test_get_reversed(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    lsw = LimitSwitch(3, True)
+
+    # lsw.limit_switch=MagicMock()
+    def mock_get(self) -> bool:
+        return True
+
+    monkeypatch.setattr(wpilib.DigitalInput, "get", mock_get)
+
+    value = lsw.get_value()
+
+    # assert
+    assert value == False
+
+
+def test_PhotoElectricSwitch() -> None:
+    # setup
+    # action
+    pes = PhotoElectricSwitch(3)
+
+    assert isinstance(pes, LimitSwitch)
+
+
+def test_MagneticLimitSwitch() -> None:
+    # setup
+    # action
+    mes = MagneticLimitSwitch(3)
+
+    assert isinstance(mes, LimitSwitch)
diff --git a/robotpy_toolkit_7407/tests/utilsmath_test.py b/robotpy_toolkit_7407/tests/utilsmath_test.py
new file mode 100644
index 0000000..8a024cf
--- /dev/null
+++ b/robotpy_toolkit_7407/tests/utilsmath_test.py
@@ -0,0 +1,41 @@
+import pytest
+import math
+
+from robotpy_toolkit_7407.utils.math import bounded_angle_diff, rotate_vector, clamp
+
+@pytest.mark.parametrize("theta_from, theta_to, answer",[
+    (3,1,-2),
+    (4,-1,1.2831853),
+    (-1.1, 4.2,-0.983185),
+    (5, 9, -2.2831853),
+    (-10, 1, -1.566370614),
+    (0, -2*math.pi, 0),
+    (4*math.pi, 0, 0),
+    (3*math.pi/2, 7*math.pi/4, math.pi/4)
+])
+def test_bounded_angle_diff(theta_from, theta_to, answer)->None:
+    angle=bounded_angle_diff(theta_from,theta_to)
+    assert angle==pytest.approx(answer, abs=1e-06)
+
+@pytest.mark.parametrize("x, y, theta, answer",[
+    (1,1,math.pi/4, (0, math.sqrt(2))),
+    (1,0,math.pi, (-1,0)),
+    (-1,1, math.pi/3, (math.sqrt(2)*math.cos(13*math.pi/12), math.sqrt(2)*math.sin(13*math.pi/12))),
+    (3,-2, 13*math.pi/6, (math.sqrt(13)*math.cos(math.atan2(-2, 3)+math.pi/6), math.sqrt(13)*math.sin(math.atan2(-2, 3)+math.pi/6))),
+    (-5.2,-2.3, 4.7, (math.sqrt(5.2**2+2.3**2)*math.cos(math.atan2(-2.3, -5.2)+4.7), math.sqrt(5.2**2+2.3**2)*math.sin(math.atan2(-2.3, -5.2)+4.7)))
+    ])
+def test_rotate_vector(x, y, theta, answer)->None:
+    vector=rotate_vector(x,y, theta)
+    assert vector[0]==pytest.approx(answer[0], abs=1e-06)
+    assert vector[1]==pytest.approx(answer[1], abs=1e-06)
+
+@pytest.mark.parametrize("val, _min, _max, answer", [
+    (14, 1, 20, 14),
+    (-2, 3, 12, 3),
+    (-2, -12, -3, -3),
+    (-2, -3, 12, -2),
+    (18, 3, 12, 12)
+    ])
+def test_clamp(val: float, _min: float, _max: float, answer):
+    num=clamp(val, _min, _max)
+    assert num == pytest.approx(answer, abs=1e-06)
diff --git a/robotpy_toolkit_7407/utils/data_log.py b/robotpy_toolkit_7407/utils/data_log.py
index d71dade..817c130 100644
--- a/robotpy_toolkit_7407/utils/data_log.py
+++ b/robotpy_toolkit_7407/utils/data_log.py
@@ -2,25 +2,28 @@
 import inspect
 import os
 
-import robotpy_toolkit_7407.utils.logger as lg
-
 
 class Logger:
     """
     Custom logger utility for logging to console and a custom log file.
     """
+
     def __init__(
-        self,
-        debug: bool = False,
-        use_file: bool = True,
-        filename: str = f"custom_logs/custom_logging_{datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}.log",
+            self,
+            debug: bool = False,
+            filename: str = f"custom_logs/custom_logging_{datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}.log",
+            to_file: bool = True
     ):
+
         self.filename: str = filename
         self.logfile = None
         self.debug_on = debug
-        self.file_on = use_file
+        self.file_created = False
+        self.to_file = to_file
 
         self.start_time = datetime.datetime.now()
+        if not os.path.exists("./custom_logs"):
+            os.mkdir(os.path.join(os.getcwd(), "custom_logs"))
 
     def log(self, system: str, message: str):
         """
@@ -36,19 +39,25 @@ def log(self, system: str, message: str):
         file_name = os.path.basename(frame.f_code.co_filename)
         line_no = str(frame.f_lineno)
 
-        message = f"[{str(datetime.datetime.now() - self.start_time) + ']'} [{file_name + ':' + line_no + ']' : <19} [{system + ']'  : <15} ~ {message  : <20}\n"
+        message = f"[{str(datetime.datetime.now() - self.start_time) + ']'} [{file_name + ':' + line_no + ']' : <19} \
+        [{system + ']'  : <15} ~ {message  : <20}\n"
 
-        if self.file_on:
+        if self.to_file:
             try:
-                self.logfile = open(self.filename, "a")
+                if self.file_created:
+                    self.logfile = open(self.filename, "a")
+                else:
+                    self.logfile = open(self.filename, "x")
+                    self.file_created = True
                 self.logfile.write(
                     message
                 )
                 self.logfile.close()
-            except Exception:
-                ...
+            except FileNotFoundError:
+                print("File Not Found")
 
         # lg.info(message, system, frame)
+        # print(self.filename)
         print(message)
 
     def debug(self, system: str, message: str):