diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml
new file mode 100644
index 0000000..d9b72a7
--- /dev/null
+++ b/.github/workflows/pre-commit.yml
@@ -0,0 +1,24 @@
+name: pre-commit
+
+on: pull_request
+
+jobs:
+  black-format:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+        with:
+          ref: ${{ github.ref }}
+      - uses: actions/setup-python@v3
+      - name: Install uv
+        run: pip install uv
+      - name: Run black via uv
+        run: uvx black .
+      - name: Commit and push changes
+        run: |
+          git config --global user.name "github-actions[bot]"
+          git config --global user.email "github-actions[bot]@users.noreply.github.com"
+          git add .
+          git diff --cached --quiet || git commit -m "Apply black formatting"
+          git pull --rebase origin ${{ github.head_ref }}
+          git push origin HEAD:${{ github.head_ref }}
diff --git a/launch/auto.launch.py b/launch/auto.launch.py
index f925fca..5ef518c 100644
--- a/launch/auto.launch.py
+++ b/launch/auto.launch.py
@@ -5,35 +5,31 @@
 from ament_index_python import get_package_share_directory
 import os.path
 
+
 def generate_launch_description():
-    #Auto control node
-    start_auto_node = Node(
-        package='control_pkg',
-        executable='auto',
-        name='auto'
-    )
-    
-    #Axle manager for Hyflex node
-    start_axle_manager_node = Node(
-        package='axle_manager',
-        executable='axle_manager',
-        name='axle_manager',
-        arguments=['--params-file '+os.path.join(get_package_share_directory('axle_manager'),'config','params.yaml')]
-    )
+    # Auto control node
+    start_auto_node = Node(package="control_pkg", executable="auto", name="auto")
 
-    #VN100 Sensor IMU Node
-    start_imu_node = Node(
-        package='sensors_pkg',
-        executable='imu',
-        name='imu'
+    # Axle manager for Hyflex node
+    start_axle_manager_node = Node(
+        package="axle_manager",
+        executable="axle_manager",
+        name="axle_manager",
+        arguments=[
+            "--params-file "
+            + os.path.join(
+                get_package_share_directory("axle_manager"), "config", "params.yaml"
+            )
+        ],
     )
 
+    # VN100 Sensor IMU Node
+    start_imu_node = Node(package="sensors_pkg", executable="imu", name="imu")
 
-
-    #Declare launch description and populate
+    # Declare launch description and populate
     ld = LaunchDescription()
 
-    #declare launch actions
+    # declare launch actions
     ld.add_action(start_axle_manager_node)
     ld.add_action(start_auto_node)
 
diff --git a/launch/fqr.launch.py b/launch/fqr.launch.py
index 1ea79a8..8c28a0a 100644
--- a/launch/fqr.launch.py
+++ b/launch/fqr.launch.py
@@ -5,35 +5,31 @@
 from ament_index_python import get_package_share_directory
 import os.path
 
+
 def generate_launch_description():
-    #Auto control node
-    start_fqr_node = Node(
-        package='control_pkg',
-        executable='fqr',
-        name='fqr'
-    )
-    
-    #Axle manager for Hyflex node
-    start_axle_manager_node = Node(
-        package='axle_manager',
-        executable='axle_manager',
-        name='axle_manager',
-        arguments=['--params-file '+os.path.join(get_package_share_directory('axle_manager'),'config','params.yaml')]
-    )
+    # Auto control node
+    start_fqr_node = Node(package="control_pkg", executable="fqr", name="fqr")
 
-    #VN100 Sensor IMU Node
-    start_imu_node = Node(
-        package='sensors_pkg',
-        executable='imu',
-        name='imu'
+    # Axle manager for Hyflex node
+    start_axle_manager_node = Node(
+        package="axle_manager",
+        executable="axle_manager",
+        name="axle_manager",
+        arguments=[
+            "--params-file "
+            + os.path.join(
+                get_package_share_directory("axle_manager"), "config", "params.yaml"
+            )
+        ],
     )
 
+    # VN100 Sensor IMU Node
+    start_imu_node = Node(package="sensors_pkg", executable="imu", name="imu")
 
-
-    #Declare launch description and populate
+    # Declare launch description and populate
     ld = LaunchDescription()
 
-    #declare launch actions
+    # declare launch actions
     ld.add_action(start_axle_manager_node)
     ld.add_action(start_fqr_node)
 
diff --git a/launch/jetson.launch.py b/launch/jetson.launch.py
index f7cada5..5bc8358 100644
--- a/launch/jetson.launch.py
+++ b/launch/jetson.launch.py
@@ -1,19 +1,23 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='jetson_pkg',
-            executable='apriltag',
-            name='apriltag',
-            parameters=[
-                {'cap':'rtsp://admin:hyflex@192.168.1.131:80/cam/realmonitor?channel=1&subtype=0'},
-                {'fx':1071.1362274102335},
-                {'fy':1102.1406887400624},
-                {'cx':953.030188084331},
-                {'cy':468.0382502048589}
 
-            ]
-        )
-    ])
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            Node(
+                package="jetson_pkg",
+                executable="apriltag",
+                name="apriltag",
+                parameters=[
+                    {
+                        "cap": "rtsp://admin:hyflex@192.168.1.131:80/cam/realmonitor?channel=1&subtype=0"
+                    },
+                    {"fx": 1071.1362274102335},
+                    {"fy": 1102.1406887400624},
+                    {"cx": 953.030188084331},
+                    {"cy": 468.0382502048589},
+                ],
+            )
+        ]
+    )
diff --git a/launch/lidar.launch.py b/launch/lidar.launch.py
index 824a8ce..400d578 100644
--- a/launch/lidar.launch.py
+++ b/launch/lidar.launch.py
@@ -3,30 +3,29 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
-    sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
-    lms_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_lms_1xx.launch')
+    sick_scan_pkg_prefix = get_package_share_directory("sick_scan_xd")
+    lms_launch_file_path = os.path.join(
+        sick_scan_pkg_prefix, "launch/sick_lms_1xx.launch"
+    )
     laser_scan_node = Node(
-        package='sick_scan_xd',
-        executable='sick_generic_caller',
-        output='screen',
+        package="sick_scan_xd",
+        executable="sick_generic_caller",
+        output="screen",
         arguments=[
             lms_launch_file_path,
-            'hostname:=192.168.1.134',
-            'tf_base_frame_id:=lidar_laser',
-            'scan_freq:=25',
-            'ang_res:=0.5',
-            ]
+            "hostname:=192.168.1.134",
+            "tf_base_frame_id:=lidar_laser",
+            "scan_freq:=25",
+            "ang_res:=0.5",
+        ],
     )
-    
+
     obs_node = Node(
-        package='sensors_pkg',
-        executable='obs',
-        name='obs',
+        package="sensors_pkg",
+        executable="obs",
+        name="obs",
     )
-    
-    return LaunchDescription([
-        laser_scan_node,
-        obs_node
-    ])
 
+    return LaunchDescription([laser_scan_node, obs_node])
diff --git a/launch/sick_lms_1xx.launch.py b/launch/sick_lms_1xx.launch.py
index e09695d..de9d34e 100644
--- a/launch/sick_lms_1xx.launch.py
+++ b/launch/sick_lms_1xx.launch.py
@@ -5,35 +5,41 @@
 from launch_ros.actions import Node
 from launch.actions import DeclareLaunchArgument
 
+
 def generate_launch_description():
 
     ld = LaunchDescription()
-    sick_scan_pkg_prefix = get_package_share_directory('sick_scan')
-    launchfile = os.path.basename(__file__)[:-3] # convert "<lidar_name>.launch.py" to "<lidar_name>.launch"
-    launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_lms_1xx.launch')
-    node_arguments=[launch_file_path]
-    
+    sick_scan_pkg_prefix = get_package_share_directory("sick_scan")
+    launchfile = os.path.basename(__file__)[
+        :-3
+    ]  # convert "<lidar_name>.launch.py" to "<lidar_name>.launch"
+    launch_file_path = os.path.join(
+        sick_scan_pkg_prefix, "launch/" + launchfile
+    )  # 'launch/sick_lms_1xx.launch')
+    node_arguments = [launch_file_path]
+
     # append optional commandline arguments in name:=value syntax
     for arg in sys.argv:
         if len(arg.split(":=")) == 2:
             node_arguments.append(arg)
-    
-    ROS_DISTRO = os.environ.get('ROS_DISTRO') # i.e. 'eloquent', 'foxy', etc.
-    if ROS_DISTRO[0] <= "e": # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
+
+    ROS_DISTRO = os.environ.get("ROS_DISTRO")  # i.e. 'eloquent', 'foxy', etc.
+    if (
+        ROS_DISTRO[0] <= "e"
+    ):  # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
         node = Node(
-            package='sick_scan',
-            node_executable='sick_generic_caller',
-            output='screen',
-            arguments=node_arguments
+            package="sick_scan",
+            node_executable="sick_generic_caller",
+            output="screen",
+            arguments=node_arguments,
         )
-    else: # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
+    else:  # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
         node = Node(
-            package='sick_scan',
-            executable='sick_generic_caller',
-            output='screen',
-            arguments=node_arguments
+            package="sick_scan",
+            executable="sick_generic_caller",
+            output="screen",
+            arguments=node_arguments,
         )
-    
+
     ld.add_action(node)
     return ld
-
diff --git a/launch/simulation/hyflex_sim.launch.py b/launch/simulation/hyflex_sim.launch.py
index 6a47afd..cf9aa0e 100644
--- a/launch/simulation/hyflex_sim.launch.py
+++ b/launch/simulation/hyflex_sim.launch.py
@@ -7,27 +7,29 @@
 
 
 def generate_launch_description():
-    package_dir = get_package_share_directory('simulation')
-    robot_description_path = os.path.join(package_dir, 'resource', 'hyflex.urdf')
+    package_dir = get_package_share_directory("simulation")
+    robot_description_path = os.path.join(package_dir, "resource", "hyflex.urdf")
 
     webots = WebotsLauncher(
-        world=os.path.join(package_dir, 'worlds', 'hyflex_world.wbt')
+        world=os.path.join(package_dir, "worlds", "hyflex_world.wbt")
     )
 
     hyflex_driver = WebotsController(
-        robot_name='hyflex',
+        robot_name="hyflex",
         parameters=[
-            {'robot_description': robot_description_path},
-        ]
+            {"robot_description": robot_description_path},
+        ],
     )
 
-    return LaunchDescription([
-        webots,
-        hyflex_driver,
-        launch.actions.RegisterEventHandler(
-            event_handler=launch.event_handlers.OnProcessExit(
-                target_action=webots,
-                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
-            )
-        )
-    ])
\ No newline at end of file
+    return LaunchDescription(
+        [
+            webots,
+            hyflex_driver,
+            launch.actions.RegisterEventHandler(
+                event_handler=launch.event_handlers.OnProcessExit(
+                    target_action=webots,
+                    on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
+                )
+            ),
+        ]
+    )
diff --git a/launch/teleop.launch.py b/launch/teleop.launch.py
index ce55d3e..d62e5e4 100644
--- a/launch/teleop.launch.py
+++ b/launch/teleop.launch.py
@@ -5,57 +5,55 @@
 from ament_index_python import get_package_share_directory
 import os.path
 
+
 def generate_launch_description():
-    default_joy_address = '/dev/input/js0'
+    default_joy_address = "/dev/input/js0"
     joy_name = LaunchConfiguration("joy_name")
 
-    #Joystick address option
+    # Joystick address option
     declare_joy_address = DeclareLaunchArgument(
-        name='joy_name',
+        name="joy_name",
         default_value=default_joy_address,
-        description="Full path to Joystick Device"
+        description="Full path to Joystick Device",
     )
 
-    #Base Joystick node
+    # Base Joystick node
     start_joy_node = Node(
-        package='joy_linux',
-        executable='joy_linux_node',
-        parameters=[{'dev_name':joy_name,'deadzone':0.0}]
+        package="joy_linux",
+        executable="joy_linux_node",
+        parameters=[{"dev_name": joy_name, "deadzone": 0.0}],
     )
 
-    #Joystick conversion for Hyflex node
+    # Joystick conversion for Hyflex node
     start_joy_conv_node = Node(
-        package='joy_conv',
-        executable='joy_conv',
-        name='joy_conv'
+        package="joy_conv", executable="joy_conv", name="joy_conv"
     )
 
-    #Axle manager for Hyflex node
+    # Axle manager for Hyflex node
     start_axle_manager_node = Node(
-        package='axle_manager',
-        executable='axle_manager',
-        name='axle_manager',
-        arguments=['--params-file '+os.path.join(get_package_share_directory('axle_manager'),'config','params.yaml')]
-    )
-
-    #VN100 Sensor IMU Node
-    start_imu_node = Node(
-        package='sensors_pkg',
-        executable='imu',
-        name='imu'
+        package="axle_manager",
+        executable="axle_manager",
+        name="axle_manager",
+        arguments=[
+            "--params-file "
+            + os.path.join(
+                get_package_share_directory("axle_manager"), "config", "params.yaml"
+            )
+        ],
     )
 
+    # VN100 Sensor IMU Node
+    start_imu_node = Node(package="sensors_pkg", executable="imu", name="imu")
 
-
-    #Declare launch description and populate
+    # Declare launch description and populate
     ld = LaunchDescription()
 
-    #declare launch options (if any)
+    # declare launch options (if any)
     ld.add_action(declare_joy_address)
 
-    #declare launch actions
+    # declare launch actions
     ld.add_action(start_axle_manager_node)
     ld.add_action(start_joy_conv_node)
     ld.add_action(start_joy_node)
 
-    return ld
\ No newline at end of file
+    return ld
diff --git a/src/axle_manager/axle_manager/hdc2460.py b/src/axle_manager/axle_manager/hdc2460.py
index 474aba4..45181c8 100644
--- a/src/axle_manager/axle_manager/hdc2460.py
+++ b/src/axle_manager/axle_manager/hdc2460.py
@@ -5,10 +5,15 @@
 from numpy import float32
 from utilities.tools import Tools
 
-class Hdc2460():
 
-    def __init__(self, serialPort:str,bitRate:int,leftChannel:int,rightChannel:int):
-        self.port = serial.Serial(serialPort, bitRate, timeout=2, xonxoff=False, rtscts=False, dsrdtr=False)
+class Hdc2460:
+
+    def __init__(
+        self, serialPort: str, bitRate: int, leftChannel: int, rightChannel: int
+    ):
+        self.port = serial.Serial(
+            serialPort, bitRate, timeout=2, xonxoff=False, rtscts=False, dsrdtr=False
+        )
         self.maxSpeed = 1000
         self.leftChannel = leftChannel
         self.rightChannel = rightChannel
@@ -16,54 +21,53 @@ def __init__(self, serialPort:str,bitRate:int,leftChannel:int,rightChannel:int):
     def close(self):
         self.port.close()
 
-    def  isConnected(self) -> bool:
+    def isConnected(self) -> bool:
         return self.port.is_open
 
-    def move(self, left_speed:float32, right_speed:float32):
+    def move(self, left_speed: float32, right_speed: float32):
         """Tells both motors to move at set speed. Assumes speed input from -1 to 1."""
-        left_speed = Tools.clamp(left_speed,-2,2)
-        right_speed = Tools.clamp(right_speed,-2,2)
-        
-        self.setMotor(self.leftChannel,left_speed)
-        self.setMotor(self.rightChannel,right_speed)
+        left_speed = Tools.clamp(left_speed, -2, 2)
+        right_speed = Tools.clamp(right_speed, -2, 2)
+
+        self.setMotor(self.leftChannel, left_speed)
+        self.setMotor(self.rightChannel, right_speed)
 
-    def actuate(self, val:int, extendCh:int, retractCh:int):
+    def actuate(self, val: int, extendCh: int, retractCh: int):
         """Controls an actuator using 2 digital output channels"""
         val = Tools.clamp(val, -1, 1)
         if val > 0:
-            self.setDigitalOut(extendCh,1)
-            self.setDigitalOut(retractCh,0)
+            self.setDigitalOut(extendCh, 1)
+            self.setDigitalOut(retractCh, 0)
         elif val < 0:
-            self.setDigitalOut(extendCh,0)
-            self.setDigitalOut(retractCh,1)
+            self.setDigitalOut(extendCh, 0)
+            self.setDigitalOut(retractCh, 1)
         else:
-            self.setDigitalOut(extendCh,0)
-            self.setDigitalOut(retractCh,0)
+            self.setDigitalOut(extendCh, 0)
+            self.setDigitalOut(retractCh, 0)
 
-    def configure(self, maxSpeed:int,accelRate:int, brakeRate:int):
+    def configure(self, maxSpeed: int, accelRate: int, brakeRate: int):
         """Configures the HDC2460"""
         self.setAccelration(accelRate)
         self.setDeccelration(brakeRate)
         self.maxSpeed = maxSpeed
 
-
-#region Roboteq Driver
+    # region Roboteq Driver
     def sendCommand(self, cmd):
         """Sends the serial data to both the HDC2460"""
         self.port.write((cmd + "\r").encode())
 
-    def setMotor(self,ch:int,speed:float32):
+    def setMotor(self, ch: int, speed: float32):
         """Tells the HDC2460 to move at set speed"""
-        speed = round(speed*500)
-        #clamp speed, format, and send it of to the roboteq
-        #speed = Tools.clamp(speed, -self.maxSpeed, self.maxSpeed)
+        speed = round(speed * 500)
+        # clamp speed, format, and send it of to the roboteq
+        # speed = Tools.clamp(speed, -self.maxSpeed, self.maxSpeed)
 
-        #Roboteq speed input is from -1000 to 1000. Just in case max speed > 1000.
+        # Roboteq speed input is from -1000 to 1000. Just in case max speed > 1000.
         speed = Tools.clamp(speed, -1000, 1000)
-        #send command to
+        # send command to
         self.sendCommand(f"!G {ch} {speed}")
 
-    def stopMotor(self, ch:int):
+    def stopMotor(self, ch: int):
         """Stops the motor for the specified channel."""
         self.sendCommand(f"!MS {ch}")
 
@@ -73,49 +77,49 @@ def stopAllMotors(self):
         self.stopMotor(self.rightChannel)
 
     def resetDigitalOut(self):
-        """"Will turn off all digital outputs."""
-        self.sendCommand(f"!DS 0")  
+        """ "Will turn off all digital outputs."""
+        self.sendCommand(f"!DS 0")
 
-    def setDigitalOut(self,bit:int,val:int):
-        """"Will set digital output selected by the bit number."""
+    def setDigitalOut(self, bit: int, val: int):
+        """ "Will set digital output selected by the bit number."""
         val = Tools.clamp(val, 0, 1)
         val = 1 if val == 1 else 0
         self.sendCommand(f"!D{val} {bit}")
 
-    def setDeccelration(self, rate:int):
+    def setDeccelration(self, rate: int):
         """Set the rate of speed change during decceleration for a motor channel."""
         rate = Tools.clamp(rate, 0, 500000)
-        self.sendCommand(f"!DC {self.leftChannel} {rate}")  
-        self.sendCommand(f"!DC {self.rightChannel} {rate}")  
+        self.sendCommand(f"!DC {self.leftChannel} {rate}")
+        self.sendCommand(f"!DC {self.rightChannel} {rate}")
 
-    def setAccelration(self, rate:int):
+    def setAccelration(self, rate: int):
         """Set the rate of speed change during acceleration for a motor channel."""
         rate = Tools.clamp(rate, 0, 500000)
-        self.sendCommand(f"!AC {self.leftChannel} {rate}")  
-        self.sendCommand(f"!AC {self.rightChannel} {rate}")  
+        self.sendCommand(f"!AC {self.leftChannel} {rate}")
+        self.sendCommand(f"!AC {self.rightChannel} {rate}")
 
-    def readAnalogInput(self,channel:int)->float:
+    def readAnalogInput(self, channel: int) -> float:
         """Reads the raw analog value"""
         self.port.reset_input_buffer()
         self.sendCommand(f"?AI {channel}")
-        line = ''
-        while (not re.search(r'AI=-?\d+\D', line)):
+        line = ""
+        while not re.search(r"AI=-?\d+\D", line):
             line += str(self.port.read(self.port.in_waiting))[2:-1]
-        result = float(re.search(r'AI=-?\d+', line).group()[3:])
+        result = float(re.search(r"AI=-?\d+", line).group()[3:])
         return result
-    
-    def readRPM(self,channel:int)->int:
-        """Reports the actual speed measured by the encoders as the actual RPM value. To report RPM accurately, the correct Pulses per Revolution (PPR) must be stored in the encoder configuration """
+
+    def readRPM(self, channel: int) -> int:
+        """Reports the actual speed measured by the encoders as the actual RPM value. To report RPM accurately, the correct Pulses per Revolution (PPR) must be stored in the encoder configuration"""
         self.port.reset_input_buffer()
         self.sendCommand("?S {channel}")
-        line = ''
-        while (not re.search(r'AI=-?\d+\D', line)):
+        line = ""
+        while not re.search(r"AI=-?\d+\D", line):
             line += str(self.port.read(self.port.in_waiting))[2:-1]
-        result = int(re.search(r'S=-?\d+', line).group()[2:])
+        result = int(re.search(r"S=-?\d+", line).group()[2:])
         return result
-    
+
     def readRPMs(self):
         left = self.readRPM(self.leftChannel)
         right = self.readRPM(self.rightChannel)
-        return left,right
-        #endregion
+        return left, right
+        # endregion
diff --git a/src/axle_manager/axle_manager/hdc2460_node.py b/src/axle_manager/axle_manager/hdc2460_node.py
index 4f3f081..7167d67 100644
--- a/src/axle_manager/axle_manager/hdc2460_node.py
+++ b/src/axle_manager/axle_manager/hdc2460_node.py
@@ -9,110 +9,120 @@
 from axle_manager.hdc2460 import Hdc2460
 from utilities.tools import Tools
 
+
 class Hdc2460Node(Node):
     def __init__(self):
         super().__init__("hdc2460")
-        self.set_parameters_atomically(self.declare_parameters(
-            namespace='',
-            parameters=[
-                ('facSerialPort',"/dev/ttyS1"),
-                ('bacSerialPort',"/dev/ttyS0"),
-                ('serialBitRate',115200),
-                ('leftChannel',1),
-                ('rightChannel',2),
-                ('maxSpeed',500),
-                ('accelRate',31860),
-                ('brakeRate',10620),
-                ('pivotDevice',"FAC"),
-                ('pivotExtendChannel',1),
-                ('pivotRetractChannel',2),
-                ('pivotSensorChannel',1),
-                ('plowDevice',"FAC"),
-                ('plowLeftChannel',3),
-                ('plowRightChannel',4),
-                ('plowUpChannel',5),
-                ('plowDownChannel',6)
-            ]
-        ))
-        #Start subscriptions
-        self.speedSubscription = self.create_subscription(Twist, '/cmd_vel', self.speed, 10)
+        self.set_parameters_atomically(
+            self.declare_parameters(
+                namespace="",
+                parameters=[
+                    ("facSerialPort", "/dev/ttyS1"),
+                    ("bacSerialPort", "/dev/ttyS0"),
+                    ("serialBitRate", 115200),
+                    ("leftChannel", 1),
+                    ("rightChannel", 2),
+                    ("maxSpeed", 500),
+                    ("accelRate", 31860),
+                    ("brakeRate", 10620),
+                    ("pivotDevice", "FAC"),
+                    ("pivotExtendChannel", 1),
+                    ("pivotRetractChannel", 2),
+                    ("pivotSensorChannel", 1),
+                    ("plowDevice", "FAC"),
+                    ("plowLeftChannel", 3),
+                    ("plowRightChannel", 4),
+                    ("plowUpChannel", 5),
+                    ("plowDownChannel", 6),
+                ],
+            )
+        )
+        # Start subscriptions
+        self.speedSubscription = self.create_subscription(
+            Twist, "/cmd_vel", self.speed, 10
+        )
         self.speedSubscription  # prevent unused variable warning
-        self.pivotSubscription = self.create_subscription(Int8, '/vehicle/pivot', self.pivot, 10)
+        self.pivotSubscription = self.create_subscription(
+            Int8, "/vehicle/pivot", self.pivot, 10
+        )
         self.pivotSubscription  # prevent unused variable warning
-        self.plowSubscription = self.create_subscription(Twist, '/vehicle/plow', self.plow, 10)
+        self.plowSubscription = self.create_subscription(
+            Twist, "/vehicle/plow", self.plow, 10
+        )
         self.plowSubscription  # prevent unused variable warning
-        self.pivotpublisher = self.create_publisher(Float32, '/sensor/pivot', 10)
-        #self.flEnc = self.create_publisher(Int32, '/sensor/wheel/frontleft', 10)
-        #self.frEnc = self.create_publisher(Int32, '/sensor/wheel/frontright', 10)
-        #self.rlEnc = self.create_publisher(Int32, '/sensor/wheel/rearleft', 10)
-        #self.rrEnc = self.create_publisher(Int32, '/sensor/wheel/rearright', 10)
-        
-        #Get all parameters
-        facSerialPort = str(self.get_parameter('facSerialPort').value)
-        bacSerialPort = str(self.get_parameter('bacSerialPort').value)
-        bitRate = int(self.get_parameter('serialBitRate').value)
-        leftChannel = int(self.get_parameter('leftChannel').value)
-        rightChannel = int(self.get_parameter('rightChannel').value)
-        maxSpeed = int(self.get_parameter('maxSpeed').value)
-        accelRate = int(self.get_parameter('accelRate').value)
-        brakeRate = int(self.get_parameter('brakeRate').value)
-        self.pivotDevice = str(self.get_parameter('pivotDevice').value)
-        self.pivotLeft = int(self.get_parameter('pivotExtendChannel').value)
-        self.pivotRight = int(self.get_parameter('pivotRetractChannel').value)
-        self.plowDevice = str(self.get_parameter('plowDevice').value)
-        self.plowLeft = int(self.get_parameter('plowLeftChannel').value)
-        self.plowRight = int(self.get_parameter('plowRightChannel').value)
-        self.plowUp = int(self.get_parameter('plowUpChannel').value)
-        self.plowDown = int(self.get_parameter('plowDownChannel').value)
-        self.pivotSensor = int(self.get_parameter('pivotSensorChannel').value)
-
-        self.fac = Hdc2460(facSerialPort,bitRate,leftChannel,rightChannel)
-        self.bac = Hdc2460(bacSerialPort,bitRate,leftChannel,rightChannel)
+        self.pivotpublisher = self.create_publisher(Float32, "/sensor/pivot", 10)
+        # self.flEnc = self.create_publisher(Int32, '/sensor/wheel/frontleft', 10)
+        # self.frEnc = self.create_publisher(Int32, '/sensor/wheel/frontright', 10)
+        # self.rlEnc = self.create_publisher(Int32, '/sensor/wheel/rearleft', 10)
+        # self.rrEnc = self.create_publisher(Int32, '/sensor/wheel/rearright', 10)
+
+        # Get all parameters
+        facSerialPort = str(self.get_parameter("facSerialPort").value)
+        bacSerialPort = str(self.get_parameter("bacSerialPort").value)
+        bitRate = int(self.get_parameter("serialBitRate").value)
+        leftChannel = int(self.get_parameter("leftChannel").value)
+        rightChannel = int(self.get_parameter("rightChannel").value)
+        maxSpeed = int(self.get_parameter("maxSpeed").value)
+        accelRate = int(self.get_parameter("accelRate").value)
+        brakeRate = int(self.get_parameter("brakeRate").value)
+        self.pivotDevice = str(self.get_parameter("pivotDevice").value)
+        self.pivotLeft = int(self.get_parameter("pivotExtendChannel").value)
+        self.pivotRight = int(self.get_parameter("pivotRetractChannel").value)
+        self.plowDevice = str(self.get_parameter("plowDevice").value)
+        self.plowLeft = int(self.get_parameter("plowLeftChannel").value)
+        self.plowRight = int(self.get_parameter("plowRightChannel").value)
+        self.plowUp = int(self.get_parameter("plowUpChannel").value)
+        self.plowDown = int(self.get_parameter("plowDownChannel").value)
+        self.pivotSensor = int(self.get_parameter("pivotSensorChannel").value)
+
+        self.fac = Hdc2460(facSerialPort, bitRate, leftChannel, rightChannel)
+        self.bac = Hdc2460(bacSerialPort, bitRate, leftChannel, rightChannel)
 
         if self.fac.isConnected and self.bac.isConnected:
             self.get_logger().info("HDC2460s connected.")
-            self.fac.configure(maxSpeed,accelRate,brakeRate)
+            self.fac.configure(maxSpeed, accelRate, brakeRate)
             self.get_logger().debug("FAC configured.")
-            self.bac.configure(maxSpeed,accelRate,brakeRate)
+            self.bac.configure(maxSpeed, accelRate, brakeRate)
             self.get_logger().debug("BAC configured.")
             self.get_logger().info("HDC2460s initialized.")
         else:
             self.get_logger().error("Unable to initialize Hdc2460s")
-            #shutdown since we couldn't startup ports
+            # shutdown since we couldn't startup ports
             self.destroy_node()
             rclpy.shutdown()
-       
+
         self.reading_from_sensors = False
         self.turn_angle = 0
-        self.timer = self.create_timer(0.1,self.sensorsPub)
+        self.timer = self.create_timer(0.1, self.sensorsPub)
 
-    def speed(self, msg:Twist):
+    def speed(self, msg: Twist):
         speed = Tools.clamp(msg.linear.x, -1, 1)
-        
+
         percentage = 0.01 * self.turn_angle + 1
         offset = (speed * percentage) - speed
-        
-        left_speed = (speed + offset)
-        right_speed = (speed - offset)
+
+        left_speed = speed + offset
+        right_speed = speed - offset
         # Send linear and angular velocities to serial
-        self.fac.move(left_speed,right_speed)
-        self.bac.move(left_speed,right_speed)
-        self.get_logger().debug("Roboteq: Left={0:.2f} Right={0:.2f}".format(left_speed,right_speed))
+        self.fac.move(left_speed, right_speed)
+        self.bac.move(left_speed, right_speed)
+        self.get_logger().debug(
+            "Roboteq: Left={0:.2f} Right={0:.2f}".format(left_speed, right_speed)
+        )
 
-    def pivot(self, msg:Int8):
+    def pivot(self, msg: Int8):
         cmd = int(msg.data)
         chL = int(self.pivotLeft)
         chR = int(self.pivotRight)
         if self.pivotDevice.casefold() == "FAC".casefold():
-            self.fac.actuate(cmd,chL,chR)
+            self.fac.actuate(cmd, chL, chR)
         elif self.pivotDevice.casefold() == "BAC".casefold():
-            self.bac.actuate(cmd,chL,chR)
+            self.bac.actuate(cmd, chL, chR)
         else:
             self.get_logger().warning("No pivot device configured.")
-        self.get_logger().debug("Roboteq: Val={} Ch1={} Ch2={}".format(cmd,chL,chR))
-        
+        self.get_logger().debug("Roboteq: Val={} Ch1={} Ch2={}".format(cmd, chL, chR))
 
-    def plow(self, msg:Twist):
+    def plow(self, msg: Twist):
         cmd = Vector3()
         cmd.x = msg.angular.x
         cmd.y = msg.angular.y
@@ -122,15 +132,19 @@ def plow(self, msg:Twist):
         chU = int(self.plowLeft)
         chD = int(self.plowLeft)
         if self.pivotDevice.casefold() == "FAC".casefold():
-            self.fac.actuate(cmd.x,chR,chL)
-            self.fac.actuate(cmd.y,chU,chD)
+            self.fac.actuate(cmd.x, chR, chL)
+            self.fac.actuate(cmd.y, chU, chD)
         elif self.pivotDevice.casefold() == "BAC".casefold():
-            self.bac.actuate(cmd.x,chR,chL)
-            self.bac.actuate(cmd.y,chU,chD)
+            self.bac.actuate(cmd.x, chR, chL)
+            self.bac.actuate(cmd.y, chU, chD)
         else:
             self.get_logger().warning("No plow device configured.")
-        self.get_logger().debug("Roboteq: Val={0:.4f} Ch1={0} Ch2={0}".format(cmd.x,chR,chL))
-        self.get_logger().debug("Roboteq: Val={0:.4f} Ch1={0} Ch2={0}".format(cmd.y,chU,chD))
+        self.get_logger().debug(
+            "Roboteq: Val={0:.4f} Ch1={0} Ch2={0}".format(cmd.x, chR, chL)
+        )
+        self.get_logger().debug(
+            "Roboteq: Val={0:.4f} Ch1={0} Ch2={0}".format(cmd.y, chU, chD)
+        )
 
     def publish_sensor_data(self):
         self.reading_from_sensors = True
@@ -141,7 +155,9 @@ def publish_sensor_data(self):
             pivot.data = self.bac.readAnalogInput(self.pivotSensor)
         else:
             self.get_logger().warning("No pivot device configured.")
-        self.get_logger().debug("Roboteq: AI={0:.2f} Ch={1}".format(pivot.data,self.pivotSensor))
+        self.get_logger().debug(
+            "Roboteq: AI={0:.2f} Ch={1}".format(pivot.data, self.pivotSensor)
+        )
         self.pivotpublisher.publish(pivot)
         self.turn_angle = Tools.potentiometer_to_degrees(pivot.data)
         self.reading_from_sensors = False
@@ -157,15 +173,17 @@ def destroy_node(self):
         self.bac.close
         self.get_logger().info("HDC2460s closed.")
 
+
 def main(args=None):
     rclpy.init(args=args)
 
     node = Hdc2460Node()
     rclpy.spin(node)
-    
+
     # Destroy the node explicitly
     node.destroy_node()
     rclpy.shutdown()
-    
+
+
 if __name__ == "__main__":
     main()
diff --git a/src/axle_manager/setup.py b/src/axle_manager/setup.py
index 5715b86..fb5463a 100644
--- a/src/axle_manager/setup.py
+++ b/src/axle_manager/setup.py
@@ -1,28 +1,25 @@
 from setuptools import find_packages, setup
 from glob import glob
 
-package_name = 'axle_manager'
+package_name = "axle_manager"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        ('share/'+package_name,glob('config/*params.[yaml]*'))
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name, glob("config/*params.[yaml]*")),
     ],
-    install_requires=['setuptools','serial'],
+    install_requires=["setuptools", "serial"],
     zip_safe=True,
-    maintainer='nate',
-    maintainer_email='20501643+naterbots@users.noreply.github.com',
-    description='Driver for RoboteQ HDC2460',
-    license='MIT',
-    tests_require=['pytest'],
+    maintainer="nate",
+    maintainer_email="20501643+naterbots@users.noreply.github.com",
+    description="Driver for RoboteQ HDC2460",
+    license="MIT",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-             'axle_manager = axle_manager.hdc2460_node:main'
-        ],
+        "console_scripts": ["axle_manager = axle_manager.hdc2460_node:main"],
     },
 )
diff --git a/src/axle_manager/test/test_copyright.py b/src/axle_manager/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/axle_manager/test/test_copyright.py
+++ b/src/axle_manager/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/axle_manager/test/test_flake8.py b/src/axle_manager/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/axle_manager/test/test_flake8.py
+++ b/src/axle_manager/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/axle_manager/test/test_pep257.py b/src/axle_manager/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/axle_manager/test/test_pep257.py
+++ b/src/axle_manager/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/control_pkg/control_pkg/auto.py b/src/control_pkg/control_pkg/auto.py
index bee5afb..0e7588f 100644
--- a/src/control_pkg/control_pkg/auto.py
+++ b/src/control_pkg/control_pkg/auto.py
@@ -9,30 +9,33 @@
 from control_pkg.wait_commands import WaitCommand, WaitUntilCommand
 from control_pkg.turn_command import TurnToDegreesCommand
 
+
 class Auto(Node):
     def __init__(self):
-        super().__init__('auto')
-        
+        super().__init__("auto")
+
         # Set up ROS stuff
         self.pivot_position = None
         self.position = None
-        
+
         self.pivot_position_updated = False
         self.position_updated = False
-        
-        self.pivot_publisher = self.create_publisher(Int8, '/vehicle/pivot', 10)
-        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
-        
-        self.create_subscription(Twist, '/apriltag', self.update_position, 10)
-        self.create_subscription(Float32, '/sensor/pivot', self.update_pivot_position, 10)
-        
+
+        self.pivot_publisher = self.create_publisher(Int8, "/vehicle/pivot", 10)
+        self.speed_publisher = self.create_publisher(Twist, "/cmd_vel", 10)
+
+        self.create_subscription(Twist, "/apriltag", self.update_position, 10)
+        self.create_subscription(
+            Float32, "/sensor/pivot", self.update_pivot_position, 10
+        )
+
         # Start runner
         self.runner = Runner()
         self.create_timer(0.01, self.runner.run)
-        
+
         # Start auto command
         self.runner.start_command(self.get_auto_command())
-        
+
     def get_auto_command(self) -> Command:
         """
         Generates and returns the command to start when auto is started
@@ -45,72 +48,84 @@ def get_auto_command(self) -> Command:
         left_waypoint.linear.x = -2.5
         left_waypoint.linear.z = 2
         left_waypoint.angular.y = 180
-        
+
         # Waypoint after right turn out
         right_waypoint = Twist()
         right_waypoint.linear.x = 2.5
         right_waypoint.linear.z = 2
         right_waypoint.angular.y = 0
-        
+
         # Factory functions for removing redundancy
-        wait = lambda time_seconds : WaitCommand(time_seconds)
-        turn_to_degrees = lambda degrees : TurnToDegreesCommand(degrees, self.get_pivot_position, self.drive_pivot)
-        drive_to_waypoint = lambda waypoint : DriveToWaypointCommand(waypoint, self.get_position, self.get_pivot_position, self.drive_pivot, self.drive)
-        drive_distance = lambda speed, distance : DriveDistanceCommand(speed, distance, self.drive)
-        wait_until = lambda condition : WaitUntilCommand(condition)
+        wait = lambda time_seconds: WaitCommand(time_seconds)
+        turn_to_degrees = lambda degrees: TurnToDegreesCommand(
+            degrees, self.get_pivot_position, self.drive_pivot
+        )
+        drive_to_waypoint = lambda waypoint: DriveToWaypointCommand(
+            waypoint,
+            self.get_position,
+            self.get_pivot_position,
+            self.drive_pivot,
+            self.drive,
+        )
+        drive_distance = lambda speed, distance: DriveDistanceCommand(
+            speed, distance, self.drive
+        )
+        wait_until = lambda condition: WaitUntilCommand(condition)
 
         # Creating and returning command
-        return wait_until(lambda : self.position_updated and self.pivot_position_updated)\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_to_waypoint(left_waypoint))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(1, 2))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(-22))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(1, 3.14 * 1.90475 / 4))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 3.14 * 1.90471 / 4))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 1.5))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(-18.624))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 2.25 * 3.14 / 2))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 1.5))\
-                .and_then(wait(2))\
-                .and_then(drive_to_waypoint(right_waypoint))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(1, 2))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(-22))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(1, 3.14 * 1.90475 / 4))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 3.14 * 1.90475 / 4))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 1.5))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(18.624))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 2.25 * 3.14 / 2))\
-                .and_then(wait(2))\
-                .and_then(turn_to_degrees(0))\
-                .and_then(wait(2))\
-                .and_then(drive_distance(-1, 1.5))
-    
+        return (
+            wait_until(lambda: self.position_updated and self.pivot_position_updated)
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_to_waypoint(left_waypoint))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(1, 2))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(-22))
+            .and_then(wait(2))
+            .and_then(drive_distance(1, 3.14 * 1.90475 / 4))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 3.14 * 1.90471 / 4))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 1.5))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(-18.624))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 2.25 * 3.14 / 2))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 1.5))
+            .and_then(wait(2))
+            .and_then(drive_to_waypoint(right_waypoint))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(1, 2))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(-22))
+            .and_then(wait(2))
+            .and_then(drive_distance(1, 3.14 * 1.90475 / 4))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 3.14 * 1.90475 / 4))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 1.5))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(18.624))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 2.25 * 3.14 / 2))
+            .and_then(wait(2))
+            .and_then(turn_to_degrees(0))
+            .and_then(wait(2))
+            .and_then(drive_distance(-1, 1.5))
+        )
+
     def drive_pivot(self, speed) -> None:
         """
         Drives the pivot left or right or stops the pivot
@@ -121,7 +136,7 @@ def drive_pivot(self, speed) -> None:
         msg = Int8()
         msg.data = speed
         self.pivot_publisher.publish(msg)
-        
+
     def update_pivot_position(self, msg: Float32) -> None:
         """
         Updates the pivot position when a new message is heard
@@ -131,7 +146,7 @@ def update_pivot_position(self, msg: Float32) -> None:
         """
         self.pivot_position_updated = True
         self.pivot_position = msg.data
-        
+
     def get_pivot_position(self) -> float:
         """
         Gets the current pivot position
@@ -140,7 +155,7 @@ def get_pivot_position(self) -> float:
             float: Current pivot position
         """
         return self.pivot_position
-    
+
     def update_position(self, msg: Twist) -> None:
         """
         Updates the position of the vehicle when a new message is heard
@@ -150,7 +165,7 @@ def update_position(self, msg: Twist) -> None:
         """
         self.position_updated = True
         self.position = msg
-        
+
     def get_position(self) -> float:
         """
         Gets the current position of the vehicle
@@ -159,7 +174,7 @@ def get_position(self) -> float:
             float: Current position of the vehicle
         """
         return self.position
-        
+
     def drive(self, speed: float) -> None:
         """
         Drives the vehicle at the given speed
@@ -170,15 +185,17 @@ def drive(self, speed: float) -> None:
         msg = Twist()
         msg.linear.x = speed
         self.speed_publisher.publish(msg)
-        
+
+
 def main():
     rclpy.init()
-    
+
     node = Auto()
     rclpy.spin(node)
-    
+
     node.destroy_node()
     rclpy.shutdown()
-    
-if __name__ == '__main__':
+
+
+if __name__ == "__main__":
     main()
diff --git a/src/control_pkg/control_pkg/commands.py b/src/control_pkg/control_pkg/commands.py
index f3b19ca..5c3345f 100644
--- a/src/control_pkg/control_pkg/commands.py
+++ b/src/control_pkg/control_pkg/commands.py
@@ -2,21 +2,22 @@ class Command:
     """
     The basic building block of the command-based framework
     """
+
     def __init__(self):
         pass
-    
+
     def initialize(self) -> None:
         """
         Runs once when the command starts
         """
         pass
-    
+
     def execute(self) -> None:
         """
         Runs periodically while the command is active
         """
         pass
-    
+
     def is_finished(self) -> bool:
         """
         Returns whether or not the command is completed. Once this returns True, the command will be stopped
@@ -25,14 +26,14 @@ def is_finished(self) -> bool:
             bool: If the command is finished executing
         """
         return False
-    
+
     def end(self) -> None:
         """
         Runs once when the command is finished executing
         """
         pass
-    
-    def along_with(self, other_command: 'Command') -> 'Command':
+
+    def along_with(self, other_command: "Command") -> "Command":
         """
         Runs another command at the same time as this command.
         The resulting command will be finished once this command and the other command
@@ -47,8 +48,8 @@ def along_with(self, other_command: 'Command') -> 'Command':
         group: ParallelCommandGroup = ParallelCommandGroup()
         group.add_commands(self, other_command)
         return group
-    
-    def and_then(self, next_command: 'Command') -> 'Command':
+
+    def and_then(self, next_command: "Command") -> "Command":
         """
         Runs another command after this command is finished. The resulting command
         will be finished once the next command is finished
@@ -62,28 +63,32 @@ def and_then(self, next_command: 'Command') -> 'Command':
         group: SequentialCommandGroup = SequentialCommandGroup()
         group.add_commands(self, next_command)
         return group
-    
+
+
 class SequentialCommandGroup(Command):
     """
     Command group that runs commands one after another. It is finished once all sub-commands are finished
     """
+
     def __init__(self):
         super().__init__()
         self.commands: list[Command] = []
-        
+
     def add_command(self, command: Command) -> None:
         self.commands.append(command)
-        
+
     def add_commands(self, *commands: Command) -> None:
         for command in commands:
             self.add_command(command)
-            
+
     def initialize(self) -> None:
         if len(self.commands) > 0:
             self.commands[0].initialize()
-        
+
     def execute(self) -> None:
-        current_command: Command | None = self.commands[0] if len(self.commands) > 0 else None
+        current_command: Command | None = (
+            self.commands[0] if len(self.commands) > 0 else None
+        )
         while current_command is not None and current_command.is_finished():
             current_command.end()
             self.commands.remove(current_command)
@@ -95,30 +100,32 @@ def execute(self) -> None:
 
     def is_finished(self) -> bool:
         return len(self.commands) == 0
-    
+
     def end(self) -> None:
         if len(self.commands) > 0:
             self.commands[0].end()
 
+
 class ParallelCommandGroup(Command):
     """
     Command group that runs commands one after another. It is finished once all sub-commands are finished
     """
+
     def __init__(self):
         super().__init__()
         self.commands: list[Command] = []
-        
+
     def add_command(self, command: Command) -> None:
         self.commands.append(command)
-        
+
     def add_commands(self, *commands: Command) -> None:
         for command in commands:
             self.add_command(command)
-            
+
     def initialize(self) -> None:
         for command in self.commands:
             command.initialize()
-            
+
     def execute(self) -> None:
         for command in self.commands:
             if command.is_finished():
@@ -126,21 +133,23 @@ def execute(self) -> None:
                 self.commands.remove(command)
                 continue
             command.execute()
-            
+
     def is_finished(self) -> bool:
-        return all(map(lambda command : command.is_finished(), self.commands))
-    
+        return all(map(lambda command: command.is_finished(), self.commands))
+
     def end(self) -> None:
         for command in self.commands:
             command.end()
-    
+
+
 class Runner:
     """
     Backbone of the command based framework. Manages scheduling and running of commands
     """
+
     def __init__(self):
         self.commands: list[Command] = []
-        
+
     def start_command(self, command: Command) -> None:
         """
         Starts the execution of a command
@@ -150,7 +159,7 @@ def start_command(self, command: Command) -> None:
         """
         self.commands.append(command)
         command.initialize()
-        
+
     def run(self) -> None:
         """
         Runs all active commands and stops finished commands
diff --git a/src/control_pkg/control_pkg/drive_commands.py b/src/control_pkg/control_pkg/drive_commands.py
index 213dfc2..5168c13 100644
--- a/src/control_pkg/control_pkg/drive_commands.py
+++ b/src/control_pkg/control_pkg/drive_commands.py
@@ -8,92 +8,128 @@
 from time import time
 from math import sqrt
 
+
 class DriveTimeCommand(Command):
     """
     Command that drives the vehicle at the given speed for the given time
     """
+
     def __init__(self, speed: float, drive_time: float, drive: Callable[[float], None]):
         super().__init__()
         self.speed = float(speed)
         self.drive_time = drive_time
         self.drive = drive
-        
+
     def initialize(self):
         self.end_time = time() + self.drive_time
-        
+
     def execute(self):
         self.drive(self.speed)
-        
+
     def is_finished(self) -> bool:
         return time() >= self.end_time
-    
+
     def end(self):
         self.drive(float(0))
 
+
 # measured in m/s
 top_speed = 1
 # measured in m/s/s
 acceleration = 3
 deceleration = 1
-        
+
+
 class DriveDistanceCommand(DriveTimeCommand):
     """
     Command that drives the vehicle the given distance at the given speed
     """
+
     def __init__(self, speed: float, distance: float, drive: Callable[[float], None]):
-        super().__init__(speed=speed, drive_time=self.calculate_time(speed, distance), drive=drive)
-        
+        super().__init__(
+            speed=speed, drive_time=self.calculate_time(speed, distance), drive=drive
+        )
+
     def calculate_time(self, speed, distance) -> float:
         real_speed = abs(speed) * top_speed
-        
+
         acceleration_time = real_speed / acceleration
         deceleration_time = real_speed / deceleration
-        
+
         if distance < real_speed * (acceleration_time + deceleration_time) / 2:
-            return sqrt((2 * distance * acceleration * deceleration) / (acceleration + deceleration)) / acceleration
-        
-        hold_time = ((2 * distance / real_speed) - acceleration_time - deceleration_time) / 2
+            return (
+                sqrt(
+                    (2 * distance * acceleration * deceleration)
+                    / (acceleration + deceleration)
+                )
+                / acceleration
+            )
+
+        hold_time = (
+            (2 * distance / real_speed) - acceleration_time - deceleration_time
+        ) / 2
         return acceleration_time + hold_time
 
+
 max_turn = 18.25
 
+
 class DriveToWaypointCommand(SequentialCommandGroup):
     """
     Command that drives the vehicle to the given waypoint
     """
-    def __init__(self, waypoint: Twist, get_position: Callable[[], Twist], get_pivot_position: Callable[[], float], drive_pivot: Callable[[int], None], drive: Callable[[float], None]):
+
+    def __init__(
+        self,
+        waypoint: Twist,
+        get_position: Callable[[], Twist],
+        get_pivot_position: Callable[[], float],
+        drive_pivot: Callable[[int], None],
+        drive: Callable[[float], None],
+    ):
         super().__init__()
         self.waypoint = waypoint
         self.get_position = get_position
         self.get_pivot_position = get_pivot_position
         self.drive_pivot = drive_pivot
         self.drive = drive
-        
+
     def initialize(self):
         my_position = self.get_position()
-        path = turn_path(start_point=(my_position.linear.x, my_position.linear.z), start_direction=my_position.angular.y, end_point=(self.waypoint.linear.x, self.waypoint.linear.z), end_direction=self.waypoint.angular.y)
-    
+        path = turn_path(
+            start_point=(my_position.linear.x, my_position.linear.z),
+            start_direction=my_position.angular.y,
+            end_point=(self.waypoint.linear.x, self.waypoint.linear.z),
+            end_direction=self.waypoint.angular.y,
+        )
+
         path_segment_1 = SequentialCommandGroup()
         path_segment_1.add_commands(
-            TurnToDegreesCommand(path[0] * 18.624, self.get_pivot_position, self.drive_pivot),
+            TurnToDegreesCommand(
+                path[0] * 18.624, self.get_pivot_position, self.drive_pivot
+            ),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[1], path[2], self.drive)
+            DriveDistanceCommand(top_speed * path[1], path[2], self.drive),
         )
-        
+
         path_segment_2 = SequentialCommandGroup()
         path_segment_2.add_commands(
-            TurnToDegreesCommand(path[3] * 18.624, self.get_pivot_position, self.drive_pivot),
+            TurnToDegreesCommand(
+                path[3] * 18.624, self.get_pivot_position, self.drive_pivot
+            ),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[4], path[5], self.drive)
+            DriveDistanceCommand(top_speed * path[4], path[5], self.drive),
         )
-        
+
         path_segment_3 = SequentialCommandGroup()
         path_segment_3.add_commands(
-            TurnToDegreesCommand(path[6] * 18.624, self.get_pivot_position, self.drive_pivot),
+            TurnToDegreesCommand(
+                path[6] * 18.624, self.get_pivot_position, self.drive_pivot
+            ),
             WaitCommand(2),
-            DriveDistanceCommand(top_speed * path[7], path[8], self.drive)
+            DriveDistanceCommand(top_speed * path[7], path[8], self.drive),
         )
-        
+
         needed_segments: list[SequentialCommandGroup] = []
         if path[2] > 0.2:
             needed_segments.append(path_segment_1)
@@ -101,11 +137,11 @@ def initialize(self):
             needed_segments.append(path_segment_2)
         if path[8] > 0.2:
             needed_segments.append(path_segment_3)
-            
+
         for i in range(len(needed_segments)):
             if i != len(needed_segments) - 1:
                 needed_segments[i].add_command(WaitCommand(2))
             self.add_command(needed_segments[i])
-            
+
         if len(self.commands) > 0:
             self.commands[0].initialize()
diff --git a/src/control_pkg/control_pkg/fqr.py b/src/control_pkg/control_pkg/fqr.py
index d12b765..6ddec30 100644
--- a/src/control_pkg/control_pkg/fqr.py
+++ b/src/control_pkg/control_pkg/fqr.py
@@ -3,28 +3,31 @@
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
 
+
 class FQR(Node):
-    
+
     def __init__(self):
-        super().__init__('fqr')
-        
-        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
-        
-        self.create_timer(.01, self.publish_speed)
-        
+        super().__init__("fqr")
+
+        self.speed_publisher = self.create_publisher(Twist, "/cmd_vel", 10)
+
+        self.create_timer(0.01, self.publish_speed)
+
     def publish_speed(self):
         msg = Twist()
         msg.linear.x = 1.0
         self.speed_publisher.publish(msg)
-        
+
+
 def main():
     rclpy.init()
-    
+
     node = FQR()
     rclpy.spin(node)
-    
+
     node.destroy_node()
     rclpy.shutdown()
-    
-if __name__ == '__main__':
-    main()
\ No newline at end of file
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/control_pkg/control_pkg/path_planning.py b/src/control_pkg/control_pkg/path_planning.py
index 55c3c87..f3dbb3c 100644
--- a/src/control_pkg/control_pkg/path_planning.py
+++ b/src/control_pkg/control_pkg/path_planning.py
@@ -2,100 +2,174 @@
 
 pi = math.pi
 turn_radius = 2.25
-radians = (pi/180)
+radians = pi / 180
+
 
 def direction(start_point, end_point):
     change = (end_point[0] - start_point[0], end_point[1] - start_point[1])
-    
+
     if change[0] == 0:
         return math.pi / 2 if change[1] > 0 else 3 * math.pi / 2
-    if change [1] == 0:
+    if change[1] == 0:
         return math.pi if change[0] < 0 else 0
-    return math.pi + math.atan(change[1] / change[0]) if change[0] < 0 else math.atan(change[1] / change[0])
+    return (
+        math.pi + math.atan(change[1] / change[0])
+        if change[0] < 0
+        else math.atan(change[1] / change[0])
+    )
+
 
 # finds center point of a circle for the robot to drive around and the direction it should take
 def pivot_tuple(point_a, point_b, direction):
-    #defining the circle
+    # defining the circle
 
     # find first tuple
     rad_a = math.radians(direction + 90)
-    tuple_a = (point_b[0] + turn_radius * math.cos(rad_a), point_b[1] + turn_radius * math.sin(rad_a))
-    
+    tuple_a = (
+        point_b[0] + turn_radius * math.cos(rad_a),
+        point_b[1] + turn_radius * math.sin(rad_a),
+    )
+
     # find second tuple
     rad_b = math.radians(direction - 90)
-    tuple_b = (point_b[0] + turn_radius * math.cos(rad_b), point_b[1] + turn_radius * math.sin(rad_b))
-    
+    tuple_b = (
+        point_b[0] + turn_radius * math.cos(rad_b),
+        point_b[1] + turn_radius * math.sin(rad_b),
+    )
+
     # determine which tuple should be used and which direction
     if math.dist(point_a, tuple_a) <= math.dist(point_a, tuple_b):
-        return tuple_a, -1 #turn left one of these could possibly be inverted need to test
+        return (
+            tuple_a,
+            -1,
+        )  # turn left one of these could possibly be inverted need to test
     else:
-        return tuple_b, 1 # turn right
+        return tuple_b, 1  # turn right
+
 
 # determines the direction the robot should move in
 def final_direction(tuple, point, direction):
     rad = math.radians(direction)
-    tuple_a = (tuple[0] + turn_radius * math.cos(rad), tuple[1] + turn_radius * math.sin(rad))
-    tuple_b = (tuple[0] - turn_radius * math.cos(rad), tuple[1] - turn_radius * math.sin(rad))
+    tuple_a = (
+        tuple[0] + turn_radius * math.cos(rad),
+        tuple[1] + turn_radius * math.sin(rad),
+    )
+    tuple_b = (
+        tuple[0] - turn_radius * math.cos(rad),
+        tuple[1] - turn_radius * math.sin(rad),
+    )
 
     if math.dist(point, tuple_a) <= math.dist(point, tuple_b):
-        return 1 #forward
+        return 1  # forward
     else:
-        return -1 #backward
+        return -1  # backward
+
 
 def turn_path(start_point, start_direction, end_point, end_direction):
-    #defining the end circle
-    end_tuplea = (math.cos(math.radians(end_direction + 90)), math.sin(math.radians(end_direction + 90)))
-    end_tuplea = (end_point[0] + turn_radius * end_tuplea[0],end_point[1] + turn_radius * end_tuplea[1])
-    
-    
-    end_tupleb = (math.cos(math.radians(end_direction - 90)), math.sin(math.radians(end_direction - 90)))
-    end_tupleb = (end_point[0] + turn_radius * end_tupleb[0], end_point[1] + turn_radius * end_tupleb[1])
-    
-
-    if (math.dist(start_point, end_tuplea) <= math.dist(start_point, end_tupleb)):
+    # defining the end circle
+    end_tuplea = (
+        math.cos(math.radians(end_direction + 90)),
+        math.sin(math.radians(end_direction + 90)),
+    )
+    end_tuplea = (
+        end_point[0] + turn_radius * end_tuplea[0],
+        end_point[1] + turn_radius * end_tuplea[1],
+    )
+
+    end_tupleb = (
+        math.cos(math.radians(end_direction - 90)),
+        math.sin(math.radians(end_direction - 90)),
+    )
+    end_tupleb = (
+        end_point[0] + turn_radius * end_tupleb[0],
+        end_point[1] + turn_radius * end_tupleb[1],
+    )
+
+    if math.dist(start_point, end_tuplea) <= math.dist(start_point, end_tupleb):
         end_tuple = end_tuplea
-        end_turn = -1 #turn left one of these could possibly be inverted need to test (flipped >)
+        end_turn = (
+            -1
+        )  # turn left one of these could possibly be inverted need to test (flipped >)
     else:
         end_tuple = end_tupleb
-        end_turn = 1 #turn right
+        end_turn = 1  # turn right
 
-    #define the start circle
-    start_tuplea = (math.cos(math.radians(start_direction + 90)), math.sin(math.radians(start_direction + 90)))
-    start_tuplea = (start_point[0] + turn_radius * start_tuplea[0], start_point[1] + turn_radius * start_tuplea[1])
+    # define the start circle
+    start_tuplea = (
+        math.cos(math.radians(start_direction + 90)),
+        math.sin(math.radians(start_direction + 90)),
+    )
+    start_tuplea = (
+        start_point[0] + turn_radius * start_tuplea[0],
+        start_point[1] + turn_radius * start_tuplea[1],
+    )
 
-    start_tupleb = (math.cos(math.radians(start_direction - 90)), math.sin(math.radians(start_direction-90)))
-    start_tupleb = (start_point[0] + turn_radius * start_tupleb[0], start_point[1] + turn_radius * start_tupleb[1])
-    
+    start_tupleb = (
+        math.cos(math.radians(start_direction - 90)),
+        math.sin(math.radians(start_direction - 90)),
+    )
+    start_tupleb = (
+        start_point[0] + turn_radius * start_tupleb[0],
+        start_point[1] + turn_radius * start_tupleb[1],
+    )
 
-    if (math.dist(start_tuplea, end_tuple) <= math.dist(start_tupleb, end_tuple)):
+    if math.dist(start_tuplea, end_tuple) <= math.dist(start_tupleb, end_tuple):
         start_tuple = start_tuplea
-        start_turn = -1 #turn left
+        start_turn = -1  # turn left
     else:
         start_tuple = start_tupleb
-        start_turn = 1 #turn right
+        start_turn = 1  # turn right
 
     # --- UNTESTED CODE TO REDUCE THE STUFF BEFORE ---
     # end_tuple, end_turn = pivot_tuple(start_point, end_point, end_direction)
     # start_tuple, start_turn = pivot_tuple(end_tuple, start_point, start_direction) # not sure if it is end_tuple or end_point
-    
+
     # if we are using both a or both b
-    #m = (end_tuple[1]-start_tuple[1]) / (end_tuple[0]-start_tuple[0]) #slope 
-    #b = start_tuple[1] - m * start_tuple[0]
+    # m = (end_tuple[1]-start_tuple[1]) / (end_tuple[0]-start_tuple[0]) #slope
+    # b = start_tuple[1] - m * start_tuple[0]
 
     if start_turn == end_turn:
-    # if ((start_tuple == start_tuplea and end_tuple == end_tuplea) or (start_tuple == start_tupleb and end_tuple == end_tupleb)):
-            
-        tan_point_starta = (start_tuple[0] + turn_radius * (math.cos(direction(start_tuple, end_tuple) - pi / 2)), start_tuple[1] + turn_radius * (math.sin(direction(start_tuple,end_tuple) - pi / 2))) #counterclockwise
-        
-        tan_point_startb = (start_tuple[0] + turn_radius * (math.cos(direction(start_tuple, end_tuple) + pi / 2)) , start_tuple[1] + turn_radius * (math.sin(direction(start_tuple,end_tuple) + pi / 2))) #clockwise
-            
-            
-        tan_point_endb = (end_tuple[0] + turn_radius * (math.cos(direction(end_tuple, start_tuple) - pi / 2)), end_tuple[1] + turn_radius * (math.sin(direction(end_tuple, start_tuple) - pi / 2))) #counterclockwise
-            
-        tan_point_enda = (end_tuple[0] + turn_radius * (math.cos(direction(end_tuple, start_tuple) + pi / 2)), end_tuple[1] + turn_radius * (math.sin(direction(end_tuple, start_tuple) + pi / 2))) #clockwise
-
-        #c = abs(direction(start_tuple,tan_point_starta)-direction(start_tuple,start_point)+ math.radians(start_direction) - direction(tan_point_starta,tan_point_enda))%(2*math.pi)
-        if abs(direction(start_tuple, tan_point_starta) - direction(start_tuple,start_point) + math.radians(start_direction) - direction(tan_point_starta, tan_point_enda) )% (2 * math.pi) <= 0.0001:
+        # if ((start_tuple == start_tuplea and end_tuple == end_tuplea) or (start_tuple == start_tupleb and end_tuple == end_tupleb)):
+
+        tan_point_starta = (
+            start_tuple[0]
+            + turn_radius * (math.cos(direction(start_tuple, end_tuple) - pi / 2)),
+            start_tuple[1]
+            + turn_radius * (math.sin(direction(start_tuple, end_tuple) - pi / 2)),
+        )  # counterclockwise
+
+        tan_point_startb = (
+            start_tuple[0]
+            + turn_radius * (math.cos(direction(start_tuple, end_tuple) + pi / 2)),
+            start_tuple[1]
+            + turn_radius * (math.sin(direction(start_tuple, end_tuple) + pi / 2)),
+        )  # clockwise
+
+        tan_point_endb = (
+            end_tuple[0]
+            + turn_radius * (math.cos(direction(end_tuple, start_tuple) - pi / 2)),
+            end_tuple[1]
+            + turn_radius * (math.sin(direction(end_tuple, start_tuple) - pi / 2)),
+        )  # counterclockwise
+
+        tan_point_enda = (
+            end_tuple[0]
+            + turn_radius * (math.cos(direction(end_tuple, start_tuple) + pi / 2)),
+            end_tuple[1]
+            + turn_radius * (math.sin(direction(end_tuple, start_tuple) + pi / 2)),
+        )  # clockwise
+
+        # c = abs(direction(start_tuple,tan_point_starta)-direction(start_tuple,start_point)+ math.radians(start_direction) - direction(tan_point_starta,tan_point_enda))%(2*math.pi)
+        if (
+            abs(
+                direction(start_tuple, tan_point_starta)
+                - direction(start_tuple, start_point)
+                + math.radians(start_direction)
+                - direction(tan_point_starta, tan_point_enda)
+            )
+            % (2 * math.pi)
+            <= 0.0001
+        ):
             tan_point_start = tan_point_starta
             tan_point_end = tan_point_enda
         else:
@@ -104,97 +178,230 @@ def turn_path(start_point, start_direction, end_point, end_direction):
     else:
         hypotenuse = math.dist(start_tuple, end_tuple)
         short = 2 * turn_radius
-        theta = math.atan2(end_tuple[1] - start_tuple[1], end_tuple[0] - start_tuple[0]) + math.asin(short / hypotenuse) - pi / 2
-        tan_point_starta = (start_tuple[0] + turn_radius * math.cos(theta), start_tuple[1] + turn_radius * math.sin(theta))
-        tan_point_enda = (end_tuple[0] + turn_radius * math.cos(theta+pi), end_tuple[1] + turn_radius * math.sin(theta + pi))
-        if abs((direction(start_tuple, tan_point_starta) - direction(start_tuple, start_point) + math.radians(start_direction) - direction(tan_point_starta, tan_point_enda))) % (2 * math.pi) <= 0.0001:
+        theta = (
+            math.atan2(end_tuple[1] - start_tuple[1], end_tuple[0] - start_tuple[0])
+            + math.asin(short / hypotenuse)
+            - pi / 2
+        )
+        tan_point_starta = (
+            start_tuple[0] + turn_radius * math.cos(theta),
+            start_tuple[1] + turn_radius * math.sin(theta),
+        )
+        tan_point_enda = (
+            end_tuple[0] + turn_radius * math.cos(theta + pi),
+            end_tuple[1] + turn_radius * math.sin(theta + pi),
+        )
+        if (
+            abs(
+                (
+                    direction(start_tuple, tan_point_starta)
+                    - direction(start_tuple, start_point)
+                    + math.radians(start_direction)
+                    - direction(tan_point_starta, tan_point_enda)
+                )
+            )
+            % (2 * math.pi)
+            <= 0.0001
+        ):
             tan_point_start = tan_point_starta
             tan_point_end = tan_point_enda
-        else: 
-                
-            if end_tuple[0]-start_tuple[0] == 0:
+        else:
+
+            if end_tuple[0] - start_tuple[0] == 0:
                 inv_slope = 0
                 b2 = tan_point_starta[1] - inv_slope * tan_point_starta[0]
-                b3 = tan_point_enda[1] - inv_slope * tan_point_enda[0] 
+                b3 = tan_point_enda[1] - inv_slope * tan_point_enda[0]
                 tan_point_startb = 2 * start_tuple[0] - tan_point_starta[0], b2
                 tan_point_endb = 2 * start_tuple[0] - tan_point_enda[0], b3
-            else: 
-                slope = (end_tuple[1] - start_tuple[1]) / (end_tuple[0] - start_tuple[0])
+            else:
+                slope = (end_tuple[1] - start_tuple[1]) / (
+                    end_tuple[0] - start_tuple[0]
+                )
                 b1 = start_tuple[1] - slope * start_tuple[0]
-                
+
                 if slope != 0:
                     inv_slope = -1 / slope
                     b2 = tan_point_starta[1] - inv_slope * tan_point_starta[0]
-                    b3 = tan_point_enda[1] - inv_slope * tan_point_enda[0] 
-                    intersectionstart = ((b2 - b1) / (slope - inv_slope),0)
-                    intersectionend = ((b3 - b1) / (slope - inv_slope),0)
-                    intersectionstart= (intersectionstart[0],intersectionstart[0] * slope + b1)
-                    intersectionend = (intersectionend[0], intersectionend[0] * slope + b1) 
-                    tan_point_startb = (2 * intersectionstart[0]- tan_point_starta[0], 2 * intersectionstart[1] - tan_point_starta[1])
-                    tan_point_endb = (2 * intersectionend[0] - tan_point_enda[0],2 * intersectionend[1]- tan_point_enda[1])
+                    b3 = tan_point_enda[1] - inv_slope * tan_point_enda[0]
+                    intersectionstart = ((b2 - b1) / (slope - inv_slope), 0)
+                    intersectionend = ((b3 - b1) / (slope - inv_slope), 0)
+                    intersectionstart = (
+                        intersectionstart[0],
+                        intersectionstart[0] * slope + b1,
+                    )
+                    intersectionend = (
+                        intersectionend[0],
+                        intersectionend[0] * slope + b1,
+                    )
+                    tan_point_startb = (
+                        2 * intersectionstart[0] - tan_point_starta[0],
+                        2 * intersectionstart[1] - tan_point_starta[1],
+                    )
+                    tan_point_endb = (
+                        2 * intersectionend[0] - tan_point_enda[0],
+                        2 * intersectionend[1] - tan_point_enda[1],
+                    )
                 else:
-                    tan_point_startb = (tan_point_starta[0],start_tuple[1]*2-tan_point_starta[1])
-                    tan_point_endb = (tan_point_enda[0],end_tuple[1]*2-tan_point_enda[1])
+                    tan_point_startb = (
+                        tan_point_starta[0],
+                        start_tuple[1] * 2 - tan_point_starta[1],
+                    )
+                    tan_point_endb = (
+                        tan_point_enda[0],
+                        end_tuple[1] * 2 - tan_point_enda[1],
+                    )
 
             tan_point_start = tan_point_startb
             tan_point_end = tan_point_endb
-  
-    
-    placeholder_tuple1= (start_tuple[0]+turn_radius*math.cos(math.radians(start_direction)),start_tuple[1]+turn_radius*math.sin(math.radians(start_direction)))
-    placeholder_tuple2 = (start_tuple[0]-turn_radius*math.cos(math.radians(start_direction)),start_tuple[1]-turn_radius*math.sin(math.radians(start_direction)))
-
-    if (math.dist(tan_point_start, placeholder_tuple1)<= math.dist(tan_point_start, placeholder_tuple2)):
-        direction_movement_start = 1 #forward
-    
+
+    placeholder_tuple1 = (
+        start_tuple[0] + turn_radius * math.cos(math.radians(start_direction)),
+        start_tuple[1] + turn_radius * math.sin(math.radians(start_direction)),
+    )
+    placeholder_tuple2 = (
+        start_tuple[0] - turn_radius * math.cos(math.radians(start_direction)),
+        start_tuple[1] - turn_radius * math.sin(math.radians(start_direction)),
+    )
+
+    if math.dist(tan_point_start, placeholder_tuple1) <= math.dist(
+        tan_point_start, placeholder_tuple2
+    ):
+        direction_movement_start = 1  # forward
+
     else:
-        direction_movement_start = -1 #backward
+        direction_movement_start = -1  # backward
+
+    if math.dist(
+        tan_point_end,
+        (
+            end_tuple[0] - turn_radius * math.cos(math.radians(end_direction)),
+            end_tuple[1] - turn_radius * math.sin(math.radians(end_direction)),
+        ),
+    ) <= math.dist(
+        tan_point_end,
+        (
+            end_tuple[0] + turn_radius * math.cos(math.radians(end_direction)),
+            end_tuple[1] + turn_radius * math.sin(math.radians(end_direction)),
+        ),
+    ):
+        direction_movement_end = 1  # forward
 
-    if math.dist(tan_point_end, (end_tuple[0] - turn_radius * math.cos(math.radians(end_direction)), end_tuple[1] - turn_radius * math.sin(math.radians(end_direction)))) <= math.dist(tan_point_end, (end_tuple[0] + turn_radius * math.cos(math.radians(end_direction)), end_tuple[1] + turn_radius * math.sin(math.radians(end_direction)))):
-        direction_movement_end = 1 #forward
-    
     else:
-        direction_movement_end = -1 #backward
-    
+        direction_movement_end = -1  # backward
+
     # --- UNTESTED CODE TO REDUCE THE STUFF BEFORE ---
     # Find the initial direction the robot needs to move in
     # direction_movement_start = final_direction(start_tuple, tan_point_start, start_direction)
     # direction_movement_end = final_direction(end_tuple, tan_point_end, end_direction)
     distance1, distance3 = 0, 0
-    
+
     if start_turn == -1:
-        distance1 = round(turn_radius * (round((direction(start_tuple, tan_point_start) - direction(start_tuple, start_point)), 3) % (2*pi)), 3)
-        if (distance1 > turn_radius * pi):
+        distance1 = round(
+            turn_radius
+            * (
+                round(
+                    (
+                        direction(start_tuple, tan_point_start)
+                        - direction(start_tuple, start_point)
+                    ),
+                    3,
+                )
+                % (2 * pi)
+            ),
+            3,
+        )
+        if distance1 > turn_radius * pi:
             distance1 = 2 * turn_radius * pi - distance1
             direction_movement_start = direction_movement_start * -1
     else:
-        distance1 = round(turn_radius * (round((direction(start_tuple, start_point) - direction(start_tuple, tan_point_start)), 3) % (2*pi)), 3)
-        if (distance1 > turn_radius * pi):
+        distance1 = round(
+            turn_radius
+            * (
+                round(
+                    (
+                        direction(start_tuple, start_point)
+                        - direction(start_tuple, tan_point_start)
+                    ),
+                    3,
+                )
+                % (2 * pi)
+            ),
+            3,
+        )
+        if distance1 > turn_radius * pi:
             distance1 = 2 * turn_radius * pi - distance1
             direction_movement_start = direction_movement_start * -1
-    
-    distance2 = round(math.dist(tan_point_start, tan_point_end),3)
-    
+
+    distance2 = round(math.dist(tan_point_start, tan_point_end), 3)
+
     if end_turn == -1:
-        distance3 = round(turn_radius * (round((direction(end_tuple, end_point) - direction(end_tuple, tan_point_end)), 3) % (2*pi)), 3)
-        if (distance3 > turn_radius * pi):
-            distance3 = round(2 * turn_radius * pi - distance3 , 3)
+        distance3 = round(
+            turn_radius
+            * (
+                round(
+                    (
+                        direction(end_tuple, end_point)
+                        - direction(end_tuple, tan_point_end)
+                    ),
+                    3,
+                )
+                % (2 * pi)
+            ),
+            3,
+        )
+        if distance3 > turn_radius * pi:
+            distance3 = round(2 * turn_radius * pi - distance3, 3)
             direction_movement_end = direction_movement_end * -1
     else:
-        distance3 = round(turn_radius * (round((direction(end_tuple, tan_point_end) - direction(end_tuple, end_point)), 3) % (2*pi)), 3)
-        if (distance3 > turn_radius * pi):
-            distance3 = round(2 * turn_radius * pi - distance3 , 3)
+        distance3 = round(
+            turn_radius
+            * (
+                round(
+                    (
+                        direction(end_tuple, tan_point_end)
+                        - direction(end_tuple, end_point)
+                    ),
+                    3,
+                )
+                % (2 * pi)
+            ),
+            3,
+        )
+        if distance3 > turn_radius * pi:
+            distance3 = round(2 * turn_radius * pi - distance3, 3)
             direction_movement_end = direction_movement_end * -1
-            
-    #return (start_turn, direction_movement_start, tan_point_start, tan_point_end, direction_movement_end, end_turn, end_point)
-    return (start_turn, direction_movement_start, distance1, 0, 1, distance2, end_turn, direction_movement_end, distance3)
-# outputs starting turn direction, point which robot starts going straight, point robot stops going straight, ending turn direction, destination
-#right = 1 left = -1
 
+    # return (start_turn, direction_movement_start, tan_point_start, tan_point_end, direction_movement_end, end_turn, end_point)
+    return (
+        start_turn,
+        direction_movement_start,
+        distance1,
+        0,
+        1,
+        distance2,
+        end_turn,
+        direction_movement_end,
+        distance3,
+    )
+
+
+# outputs starting turn direction, point which robot starts going straight, point robot stops going straight, ending turn direction, destination
+# right = 1 left = -1
 
 
 if __name__ == "__main__":
-   # start_turn,direction_movement_start,tan_point_start,tan_point_end,direction_movement_end,end_turn,end_point = turn_path((0,6),0,(8,6),180)
-    start_turn,direction_movement_start,distance1,mid_turn,direction_movement_mid,distance2,end_turn,direction_movement_end,distance3 = turn_path((0.14,-2.5),90,(-2.5,2),180)
+    # start_turn,direction_movement_start,tan_point_start,tan_point_end,direction_movement_end,end_turn,end_point = turn_path((0,6),0,(8,6),180)
+    (
+        start_turn,
+        direction_movement_start,
+        distance1,
+        mid_turn,
+        direction_movement_mid,
+        distance2,
+        end_turn,
+        direction_movement_end,
+        distance3,
+    ) = turn_path((0.14, -2.5), 90, (-2.5, 2), 180)
 
     # Verbose
     print(f"Start turn: {start_turn}")
diff --git a/src/control_pkg/control_pkg/turn_command.py b/src/control_pkg/control_pkg/turn_command.py
index 969dec8..e45911d 100644
--- a/src/control_pkg/control_pkg/turn_command.py
+++ b/src/control_pkg/control_pkg/turn_command.py
@@ -4,27 +4,38 @@
 
 degree_deadband = 0.2
 
+
 class TurnToDegreesCommand(Command):
     """
     Command that turns the pivot to the given degrees
     """
-    def __init__(self, degrees: float, get_pivot_degrees: Callable[[], float], drive_pivot: Callable[[int], None]):
+
+    def __init__(
+        self,
+        degrees: float,
+        get_pivot_degrees: Callable[[], float],
+        drive_pivot: Callable[[int], None],
+    ):
         super().__init__()
         self.target_degrees = degrees
-        self.get_pivot_degrees = lambda : Tools.potentiometer_to_degrees(get_pivot_degrees())
+        self.get_pivot_degrees = lambda: Tools.potentiometer_to_degrees(
+            get_pivot_degrees()
+        )
         self.drive_pivot = drive_pivot
-        
+
     def in_deadband(self) -> bool:
         return abs(self.get_pivot_degrees() - self.target_degrees) <= degree_deadband
-        
+
     def execute(self):
         if self.in_deadband():
             self.drive_pivot(0)
         else:
-            self.drive_pivot(-1 if self.get_pivot_degrees() > self.target_degrees else 1)
-            
+            self.drive_pivot(
+                -1 if self.get_pivot_degrees() > self.target_degrees else 1
+            )
+
     def is_finished(self) -> bool:
         return self.in_deadband()
-    
+
     def end(self):
         self.drive_pivot(0)
diff --git a/src/control_pkg/control_pkg/wait_commands.py b/src/control_pkg/control_pkg/wait_commands.py
index 93564e8..9fda294 100644
--- a/src/control_pkg/control_pkg/wait_commands.py
+++ b/src/control_pkg/control_pkg/wait_commands.py
@@ -2,27 +2,31 @@
 from time import time
 from typing import Callable
 
+
 class WaitCommand(Command):
     """
     Command that does nothing for the given number of seconds
     """
+
     def __init__(self, wait_time: float):
         super().__init__()
         self.wait_time = wait_time
-        
+
     def initialize(self):
         self.end_time = time() + self.wait_time
-        
+
     def is_finished(self) -> bool:
         return time() >= self.end_time
-    
+
+
 class WaitUntilCommand(Command):
     """
     Command that waits until the given condition is true
     """
+
     def __init__(self, condition: Callable[[], bool]):
         super().__init__()
         self.condition = condition
-        
+
     def is_finished(self) -> bool:
         return self.condition()
diff --git a/src/control_pkg/setup.py b/src/control_pkg/setup.py
index bf6d7ac..c1a568e 100644
--- a/src/control_pkg/setup.py
+++ b/src/control_pkg/setup.py
@@ -1,29 +1,28 @@
 from setuptools import find_packages, setup
 from glob import glob
 
-package_name = 'control_pkg'
+package_name = "control_pkg"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        ('share/'+package_name,glob('launch*launch.[pxy][yaml]*'))
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name, glob("launch*launch.[pxy][yaml]*")),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='brandon',
-    maintainer_email='brandon@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="brandon",
+    maintainer_email="brandon@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'auto = control_pkg.auto:main',
-            'fqr = control_pkg.fqr:main'
+        "console_scripts": [
+            "auto = control_pkg.auto:main",
+            "fqr = control_pkg.fqr:main",
         ],
     },
 )
diff --git a/src/control_pkg/test/test_copyright.py b/src/control_pkg/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/control_pkg/test/test_copyright.py
+++ b/src/control_pkg/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/control_pkg/test/test_flake8.py b/src/control_pkg/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/control_pkg/test/test_flake8.py
+++ b/src/control_pkg/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/control_pkg/test/test_pep257.py b/src/control_pkg/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/control_pkg/test/test_pep257.py
+++ b/src/control_pkg/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/jetson_pkg/jetson_pkg/apriltag_interpretation.py b/src/jetson_pkg/jetson_pkg/apriltag_interpretation.py
index 3f04df8..9ea1eab 100644
--- a/src/jetson_pkg/jetson_pkg/apriltag_interpretation.py
+++ b/src/jetson_pkg/jetson_pkg/apriltag_interpretation.py
@@ -1,15 +1,17 @@
 import math
 
-def apriltag_interpretation(xa,ya,thetaa,xc,zc,thetac):
-    thetar=math.radians(thetaa) + math.pi + math.radians(thetac) + math.atan(xc/zc)
-   
+
+def apriltag_interpretation(xa, ya, thetaa, xc, zc, thetac):
+    thetar = math.radians(thetaa) + math.pi + math.radians(thetac) + math.atan(xc / zc)
+
     dist = math.sqrt(xc**2 + zc**2)
-    xr = xa + math.cos(math.radians(thetaa) + math.radians(thetac))* dist
-    yr = ya + math.sin(math.radians(thetaa) + math.radians(thetac))* dist
-    return (xr,yr,(thetar*(180/math.pi)) % 360)
+    xr = xa + math.cos(math.radians(thetaa) + math.radians(thetac)) * dist
+    yr = ya + math.sin(math.radians(thetaa) + math.radians(thetac)) * dist
+    return (xr, yr, (thetar * (180 / math.pi)) % 360)
+
 
 if __name__ == "__main__":
-    xr,yr,thetar = april_tag_interpretation(10,0,270,1,1.732,30)
+    xr, yr, thetar = april_tag_interpretation(10, 0, 270, 1, 1.732, 30)
 
     # Verbose
     print(f"xr: {xr}")
diff --git a/src/jetson_pkg/jetson_pkg/jetson_apriltag.py b/src/jetson_pkg/jetson_pkg/jetson_apriltag.py
index 3f7d601..4282148 100644
--- a/src/jetson_pkg/jetson_pkg/jetson_apriltag.py
+++ b/src/jetson_pkg/jetson_pkg/jetson_apriltag.py
@@ -10,31 +10,36 @@
 import cv2
 import apriltag
 
+
 class ApriltagPublisher(Node):
     def __init__(self):
-        super().__init__('apriltag_publisher')
-        self.publisher_ = self.create_publisher(Twist, '/apriltag', 10)
+        super().__init__("apriltag_publisher")
+        self.publisher_ = self.create_publisher(Twist, "/apriltag", 10)
         timer_period = 0.5
         self.latest_frame = None
 
         # Declare parameters with default values
-        self.declare_parameter('cap', '')
-        self.declare_parameter('fx', 1.0)
-        self.declare_parameter('fy', 1.0)
-        self.declare_parameter('cx', 0.0)
-        self.declare_parameter('cy', 0.0)
+        self.declare_parameter("cap", "")
+        self.declare_parameter("fx", 1.0)
+        self.declare_parameter("fy", 1.0)
+        self.declare_parameter("cx", 0.0)
+        self.declare_parameter("cy", 0.0)
 
         # Assign parameters
-        cap_value = self.get_parameter('cap').value
+        cap_value = self.get_parameter("cap").value
         self.cap = cv2.VideoCapture(cap_value) if cap_value else None
-        self.fx = self.get_parameter('fx').value
-        self.fy = self.get_parameter('fy').value
-        self.cx = self.get_parameter('cx').value
-        self.cy = self.get_parameter('cy').value
+        self.fx = self.get_parameter("fx").value
+        self.fy = self.get_parameter("fy").value
+        self.cx = self.get_parameter("cx").value
+        self.cy = self.get_parameter("cy").value
 
         if self.cap is None or not self.cap.isOpened():
-            self.get_logger().error("Failed to open video capture device. Please check the connection string.")
-            raise ValueError("Failed to open video capture device. Please check the connection string.")
+            self.get_logger().error(
+                "Failed to open video capture device. Please check the connection string."
+            )
+            raise ValueError(
+                "Failed to open video capture device. Please check the connection string."
+            )
 
         self.detector = apriltag.Detector()  # Initialize the detector
         threading.Thread(target=self.keep_up_thread, daemon=True).start()
@@ -47,7 +52,9 @@ def keep_up_thread(self):
                 if success:
                     self.latest_frame = frame
                 else:
-                    self.get_logger().warning("Failed to read frame from video capture device.")
+                    self.get_logger().warning(
+                        "Failed to read frame from video capture device."
+                    )
 
     def timer_callback(self):
         if self.latest_frame is None:
@@ -58,27 +65,33 @@ def timer_callback(self):
             gray = cv2.cvtColor(self.latest_frame, cv2.COLOR_RGB2GRAY)
             detections = self.detector.detect(gray)
             if len(detections) > 0:
-                pose, _, _ = self.detector.detection_pose(detections[0], [self.fx, self.fy, self.cx, self.cy], 0.3254375)
+                pose, _, _ = self.detector.detection_pose(
+                    detections[0], [self.fx, self.fy, self.cx, self.cy], 0.3254375
+                )
                 relative_x = pose[0][3] + -0.017
                 relative_z = pose[2][3] + 0.83
                 relative_rotation = np.arcsin(-pose[2][0]) * (180 / math.pi)
-                xr, zr, thetar = apriltag_interpretation(0, 3.71, 270, relative_x, relative_z, relative_rotation)
+                xr, zr, thetar = apriltag_interpretation(
+                    0, 3.71, 270, relative_x, relative_z, relative_rotation
+                )
                 msg = Twist()
                 msg.linear.x = xr
                 msg.linear.z = zr
                 msg.angular.y = thetar
-                self.get_logger().info(f'{msg}')
+                self.get_logger().info(f"{msg}")
                 self.publisher_.publish(msg)
         except cv2.error as e:
             self.get_logger().error(f"OpenCV error: {e}")
-        
+
+
 def main(args=None):
     rclpy.init(args=args)
     apriltag_publisher = ApriltagPublisher()
     rclpy.spin(apriltag_publisher)
-    
+
     apriltag_publisher.destroy_node()
     rclpy.shutdown()
-    
-if __name__ == '__main__':
+
+
+if __name__ == "__main__":
     main()
diff --git a/src/jetson_pkg/setup.py b/src/jetson_pkg/setup.py
index 249411c..10dc8ea 100644
--- a/src/jetson_pkg/setup.py
+++ b/src/jetson_pkg/setup.py
@@ -1,26 +1,23 @@
 from setuptools import find_packages, setup
 
-package_name = 'jetson_pkg'
+package_name = "jetson_pkg"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='brandon',
-    maintainer_email='brandon@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="brandon",
+    maintainer_email="brandon@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'apriltag = jetson_pkg.jetson_apriltag:main'
-        ],
+        "console_scripts": ["apriltag = jetson_pkg.jetson_apriltag:main"],
     },
 )
diff --git a/src/jetson_pkg/test/test_copyright.py b/src/jetson_pkg/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/jetson_pkg/test/test_copyright.py
+++ b/src/jetson_pkg/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/jetson_pkg/test/test_flake8.py b/src/jetson_pkg/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/jetson_pkg/test/test_flake8.py
+++ b/src/jetson_pkg/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/jetson_pkg/test/test_pep257.py b/src/jetson_pkg/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/jetson_pkg/test/test_pep257.py
+++ b/src/jetson_pkg/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/joy_conv/joy_conv/joy_conv.py b/src/joy_conv/joy_conv/joy_conv.py
index 00f2fcd..779267a 100644
--- a/src/joy_conv/joy_conv/joy_conv.py
+++ b/src/joy_conv/joy_conv/joy_conv.py
@@ -20,30 +20,32 @@
 
 
 class JoyConv(Node):
-    
+
     def __init__(self):
-        super().__init__('joy_conv')
-        
+        super().__init__("joy_conv")
+
         self.pivot_to_middle = False
         self.current_pivot_pos = 0
         self.degree_deadband = 0.5
-        
-        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
-        self.pivot_publisher = self.create_publisher(Int8, '/vehicle/pivot', 10)
-        self.plow_publisher = self.create_publisher(Twist, '/vehicle/plow', 10)
 
-        self.pivot_sub = self.create_subscription(Float32, '/sensor/pivot', self.update_pivot, 10)
-        self.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
+        self.speed_publisher = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.pivot_publisher = self.create_publisher(Int8, "/vehicle/pivot", 10)
+        self.plow_publisher = self.create_publisher(Twist, "/vehicle/plow", 10)
+
+        self.pivot_sub = self.create_subscription(
+            Float32, "/sensor/pivot", self.update_pivot, 10
+        )
+        self.joy_sub = self.create_subscription(Joy, "joy", self.joy_callback, 10)
 
     def update_pivot(self, msg: Float32):
         self.current_pivot_pos = Tools.potentiometer_to_degrees(msg.data)
 
-    def joy_callback(self, msg:Joy):
+    def joy_callback(self, msg: Joy):
         self.pivot_publisher.publish(self.calculate_pivot(msg))
         self.speed_publisher.publish(self.calculate_speed(msg))
         self.plow_publisher.publish(self.calculate_plow(msg))
 
-    def calculate_pivot(self, joy_msg:Joy) -> Int8:
+    def calculate_pivot(self, joy_msg: Joy) -> Int8:
         msg = Int8()
         right = int(joy_msg.buttons[XBOX_RIGHT_PALM])
         left = int(joy_msg.buttons[XBOX_LEFT_PALM])
@@ -61,32 +63,37 @@ def calculate_pivot(self, joy_msg:Joy) -> Int8:
         else:
             msg.data = right - left
         return msg
-    
+
     def in_deadband(self):
         lower_deadband = -self.degree_deadband
         upper_deadband = self.degree_deadband
-        return self.current_pivot_pos >= lower_deadband and self.current_pivot_pos <= upper_deadband
+        return (
+            self.current_pivot_pos >= lower_deadband
+            and self.current_pivot_pos <= upper_deadband
+        )
 
-    def calculate_speed(self, joy_msg:Joy) -> Twist:
+    def calculate_speed(self, joy_msg: Joy) -> Twist:
         msg = Twist()
         msg.linear.x = joy_msg.axes[XBOX_RIGHT_Y] * abs(joy_msg.axes[XBOX_RIGHT_Y])
         return msg
 
-    def calculate_plow(self, joy_msg:Joy) -> Twist:
+    def calculate_plow(self, joy_msg: Joy) -> Twist:
         msg = Twist()
         msg.angular.x = joy_msg.axes[XBOX_DPAD_LEFTRIGHT]
         msg.angular.y = joy_msg.axes[XBOX_DPAD_UPDOWN]
         return msg
 
+
 def main():
     rclpy.init()
 
     node = JoyConv()
     rclpy.spin(node)
-    
+
     # Destroy the node explicitly
     node.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/src/joy_conv/setup.py b/src/joy_conv/setup.py
index 5b586a0..6ed305c 100644
--- a/src/joy_conv/setup.py
+++ b/src/joy_conv/setup.py
@@ -1,26 +1,23 @@
 from setuptools import find_packages, setup
 
-package_name = 'joy_conv'
+package_name = "joy_conv"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='brandon',
-    maintainer_email='brandon@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="brandon",
+    maintainer_email="brandon@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'joy_conv = joy_conv.joy_conv:main'
-        ],
+        "console_scripts": ["joy_conv = joy_conv.joy_conv:main"],
     },
 )
diff --git a/src/joy_conv/test/test_copyright.py b/src/joy_conv/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/joy_conv/test/test_copyright.py
+++ b/src/joy_conv/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/joy_conv/test/test_flake8.py b/src/joy_conv/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/joy_conv/test/test_flake8.py
+++ b/src/joy_conv/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/joy_conv/test/test_pep257.py b/src/joy_conv/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/joy_conv/test/test_pep257.py
+++ b/src/joy_conv/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/sensors_pkg/setup.py b/src/sensors_pkg/setup.py
index b4606e3..f110398 100644
--- a/src/sensors_pkg/setup.py
+++ b/src/sensors_pkg/setup.py
@@ -1,25 +1,23 @@
 from setuptools import find_packages, setup
 
-package_name = 'sensors_pkg'
+package_name = "sensors_pkg"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='brandon',
-    maintainer_email='brandon@todo.todo',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="brandon",
+    maintainer_email="brandon@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-        ],
+        "console_scripts": [],
     },
 )
diff --git a/src/sensors_pkg/test/test_copyright.py b/src/sensors_pkg/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/sensors_pkg/test/test_copyright.py
+++ b/src/sensors_pkg/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/sensors_pkg/test/test_flake8.py b/src/sensors_pkg/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/sensors_pkg/test/test_flake8.py
+++ b/src/sensors_pkg/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/sensors_pkg/test/test_pep257.py b/src/sensors_pkg/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/sensors_pkg/test/test_pep257.py
+++ b/src/sensors_pkg/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/simulation/setup.py b/src/simulation/setup.py
index b2009ca..a4482da 100644
--- a/src/simulation/setup.py
+++ b/src/simulation/setup.py
@@ -1,32 +1,59 @@
 from setuptools import find_packages, setup
 
-package_name = 'simulation'
+package_name = "simulation"
 data_files = []
-data_files.append(('share/ament_index/resource_index/packages', ['resource/' + package_name]))
-data_files.append(('share/' + package_name + '/worlds', ['worlds/hyflex_world.wbt']))
-data_files.append(('share/' + package_name + '/resource', ['resource/hyflex.urdf']))
-data_files.append(('share/' + package_name + '/resource/meshes/hyflex', ['resource/meshes/hyflex/back_collision.stl']))
-data_files.append(('share/' + package_name + '/resource/meshes/hyflex', ['resource/meshes/hyflex/back.dae']))
-data_files.append(('share/' + package_name + '/resource/meshes/hyflex', ['resource/meshes/hyflex/base_link_collision.stl']))
-data_files.append(('share/' + package_name + '/resource/meshes/hyflex', ['resource/meshes/hyflex/base_link.dae']))
-data_files.append(('share/' + package_name + '/resource/meshes/hyflex', ['resource/meshes/hyflex/wheel_collision.stl']))
-data_files.append(('share/' + package_name, ['package.xml']))
+data_files.append(
+    ("share/ament_index/resource_index/packages", ["resource/" + package_name])
+)
+data_files.append(("share/" + package_name + "/worlds", ["worlds/hyflex_world.wbt"]))
+data_files.append(("share/" + package_name + "/resource", ["resource/hyflex.urdf"]))
+data_files.append(
+    (
+        "share/" + package_name + "/resource/meshes/hyflex",
+        ["resource/meshes/hyflex/back_collision.stl"],
+    )
+)
+data_files.append(
+    (
+        "share/" + package_name + "/resource/meshes/hyflex",
+        ["resource/meshes/hyflex/back.dae"],
+    )
+)
+data_files.append(
+    (
+        "share/" + package_name + "/resource/meshes/hyflex",
+        ["resource/meshes/hyflex/base_link_collision.stl"],
+    )
+)
+data_files.append(
+    (
+        "share/" + package_name + "/resource/meshes/hyflex",
+        ["resource/meshes/hyflex/base_link.dae"],
+    )
+)
+data_files.append(
+    (
+        "share/" + package_name + "/resource/meshes/hyflex",
+        ["resource/meshes/hyflex/wheel_collision.stl"],
+    )
+)
+data_files.append(("share/" + package_name, ["package.xml"]))
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=data_files,
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='user',
-    maintainer_email='user.name@mail.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
+    maintainer="user",
+    maintainer_email="user.name@mail.com",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-            'hyflex_driver = hyflex.hyflex_driver:main',
+        "console_scripts": [
+            "hyflex_driver = hyflex.hyflex_driver:main",
         ],
     },
-)
\ No newline at end of file
+)
diff --git a/src/simulation/simulation/hyflex_driver.py b/src/simulation/simulation/hyflex_driver.py
index c63643a..aee868e 100644
--- a/src/simulation/simulation/hyflex_driver.py
+++ b/src/simulation/simulation/hyflex_driver.py
@@ -4,39 +4,40 @@
 HALF_DISTANCE_BETWEEN_WHEELS = 0.045
 WHEEL_RADIUS = 0.025
 
+
 class HyflexDriver:
     def init(self, webots_node, properties):
         self.__robot = webots_node.robot
 
         # Initialize motors
-        self.__front_left_motor = self.__robot.getDevice('front_left_motor')
-        self.__front_right_motor = self.__robot.getDevice('front_right_motor')
-        self.__back_left_motor = self.__robot.getDevice('back_left_motor')
-        self.__back_right_motor = self.__robot.getDevice('back_right_motor')
-        self.__pivot_motor = self.__robot.getDevice('pivot_motor')
+        self.__front_left_motor = self.__robot.getDevice("front_left_motor")
+        self.__front_right_motor = self.__robot.getDevice("front_right_motor")
+        self.__back_left_motor = self.__robot.getDevice("back_left_motor")
+        self.__back_right_motor = self.__robot.getDevice("back_right_motor")
+        self.__pivot_motor = self.__robot.getDevice("pivot_motor")
 
         # Set motors to velocity control mode
-        self.__front_left_motor.setPosition(float('inf'))
+        self.__front_left_motor.setPosition(float("inf"))
         self.__front_left_motor.setVelocity(0)
 
-        self.__front_right_motor.setPosition(float('inf'))
+        self.__front_right_motor.setPosition(float("inf"))
         self.__front_right_motor.setVelocity(0)
 
-        self.__back_left_motor.setPosition(float('inf'))
+        self.__back_left_motor.setPosition(float("inf"))
         self.__back_left_motor.setVelocity(0)
 
-        self.__back_right_motor.setPosition(float('inf'))
+        self.__back_right_motor.setPosition(float("inf"))
         self.__back_right_motor.setVelocity(0)
 
-        self.__pivot_motor.setPosition(float('inf'))
+        self.__pivot_motor.setPosition(float("inf"))
         self.__pivot_motor.setVelocity(0)
 
         # Initialize target twist
         self.__target_twist = Twist()
 
         rclpy.init(args=None)
-        self.__node = rclpy.create_node('hyflex_driver')
-        self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)
+        self.__node = rclpy.create_node("hyflex_driver")
+        self.__node.create_subscription(Twist, "cmd_vel", self.__cmd_vel_callback, 1)
 
     def __cmd_vel_callback(self, twist: Twist):
         self.__target_twist = twist
@@ -52,4 +53,4 @@ def step(self):
         self.__back_left_motor.setVelocity(forward_speed)
         self.__back_right_motor.setVelocity(forward_speed)
 
-        self.__pivot_motor.setVelocity(angular_speed)
\ No newline at end of file
+        self.__pivot_motor.setVelocity(angular_speed)
diff --git a/src/simulation/test/test_copyright.py b/src/simulation/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/simulation/test/test_copyright.py
+++ b/src/simulation/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/simulation/test/test_flake8.py b/src/simulation/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/simulation/test/test_flake8.py
+++ b/src/simulation/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/simulation/test/test_pep257.py b/src/simulation/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/simulation/test/test_pep257.py
+++ b/src/simulation/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/utilities/setup.py b/src/utilities/setup.py
index 1073cfe..3b49208 100644
--- a/src/utilities/setup.py
+++ b/src/utilities/setup.py
@@ -1,25 +1,23 @@
 from setuptools import find_packages, setup
 
-package_name = 'utilities'
+package_name = "utilities"
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='nate',
-    maintainer_email='20501643+naterbots@users.noreply.github.com',
-    description='The utils package',
-    license='MIT',
-    tests_require=['pytest'],
+    maintainer="nate",
+    maintainer_email="20501643+naterbots@users.noreply.github.com",
+    description="The utils package",
+    license="MIT",
+    tests_require=["pytest"],
     entry_points={
-        'console_scripts': [
-        ],
+        "console_scripts": [],
     },
 )
diff --git a/src/utilities/test/test_copyright.py b/src/utilities/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/utilities/test/test_copyright.py
+++ b/src/utilities/test/test_copyright.py
@@ -17,9 +17,11 @@
 
 
 # Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
diff --git a/src/utilities/test/test_flake8.py b/src/utilities/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/utilities/test/test_flake8.py
+++ b/src/utilities/test/test_flake8.py
@@ -20,6 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/src/utilities/test/test_pep257.py b/src/utilities/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/utilities/test/test_pep257.py
+++ b/src/utilities/test/test_pep257.py
@@ -19,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/src/utilities/utilities/tools.py b/src/utilities/utilities/tools.py
index a9af4ca..9aeaf43 100644
--- a/src/utilities/utilities/tools.py
+++ b/src/utilities/utilities/tools.py
@@ -1,21 +1,21 @@
-class Tools():    
+class Tools:
+
+    def clamp(num: float, min_value: float, max_value: float) -> float:
+        """Method which takes three parameters where n is the number we
+        would like to clip and the range to be used for clipping the number."""
 
-    def clamp(num:float, min_value:float, max_value:float)->float:
-        """ Method which takes three parameters where n is the number we 
-            would like to clip and the range to be used for clipping the number."""
-        
         if num < min_value:
             return min_value
         elif num > max_value:
             return max_value
         else:
             return num
-        
-    def deadband(value:float, size:float)->float:
+
+    def deadband(value: float, size: float) -> float:
         return value if abs(value) >= size else 0
 
-    def potentiometer_to_degrees(pot_value: float)->float:
+    def potentiometer_to_degrees(pot_value: float) -> float:
         return -0.089350632 * pot_value + 87.49582692
 
-    def degrees_to_potentiometer(degree_value: float)->float:
+    def degrees_to_potentiometer(degree_value: float) -> float:
         return round((degree_value - 87.49582692) / -0.089350632, 0)