diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
index adaa23f6..394a360c 100644
--- a/.github/workflows/test.yml
+++ b/.github/workflows/test.yml
@@ -2,6 +2,7 @@
 name: dist
 
 on:
+  workflow_dispatch: 
   pull_request:
   push:
     branches:
diff --git a/SwerveControllerCommand/constants.py b/SwerveControllerCommand/constants.py
new file mode 100644
index 00000000..3ea4be68
--- /dev/null
+++ b/SwerveControllerCommand/constants.py
@@ -0,0 +1,95 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from wpilib import TimedRobot
+from wpimath.geometry import Translation2d
+from wpimath.kinematics import SwerveDrive4Kinematics
+from wpimath.trajectory import TrapezoidProfile
+import math
+
+
+class DriveConstants:
+    kFrontLeftDriveMotorPort = 0
+    kRearLeftDriveMotorPort = 2
+    kFrontRightDriveMotorPort = 4
+    kRearRightDriveMotorPort = 6
+
+    kFrontLeftTurningMotorPort = 1
+    kRearLeftTurningMotorPort = 3
+    kFrontRightTurningMotorPort = 5
+    kRearRightTurningMotorPort = 7
+
+    kFrontLeftTurningEncoderPorts = [0, 1]
+    kRearLeftTurningEncoderPorts = [2, 3]
+    kFrontRightTurningEncoderPorts = [4, 5]
+    kRearRightTurningEncoderPorts = [6, 7]
+
+    kFrontLeftTurningEncoderReversed = False
+    kRearLeftTurningEncoderReversed = True
+    kFrontRightTurningEncoderReversed = False
+    kRearRightTurningEncoderReversed = True
+
+    kFrontLeftDriveEncoderPorts = [8, 9]
+    kRearLeftDriveEncoderPorts = [10, 11]
+    kFrontRightDriveEncoderPorts = [12, 13]
+    kRearRightDriveEncoderPorts = [14, 15]
+
+    kFrontLeftDriveEncoderReversed = False
+    kRearLeftDriveEncoderReversed = True
+    kFrontRightDriveEncoderReversed = False
+    kRearRightDriveEncoderReversed = True
+
+    kDrivePeriod = TimedRobot.kDefaultPeriod
+
+    kTrackWidth = 0.5
+    kWheelBase = 0.7
+    kDriveKinematics = SwerveDrive4Kinematics(
+        Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
+    )
+
+    kGyroReversed = False
+
+    ksVolts = 1
+    kvVoltSecondsPerMeter = 0.8
+    kaVoltSecondsSquaredPerMeter = 0.15
+
+    kMaxSpeedMetersPerSecond = 3
+
+
+class ModuleConstants:
+    kMaxModuleAngularSpeedRadiansPerSecond = 2 * math.pi
+    kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * math.pi
+
+    kEncoderCPR = 1024
+    kWheelDiameterMeters = 0.15
+    kDriveEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR
+
+    kTurningEncoderDistancePerPulse = (2 * math.pi) / kEncoderCPR
+
+    kPModuleTurningController = 1
+    kPModuleDriveController = 1
+
+
+class OIConstants:
+    kDriverControllerPort = 0
+
+
+class AutoConstants:
+    kMaxSpeedMetersPerSecond = 3
+    kMaxAccelerationMetersPerSecondSquared = 3
+    kMaxAngularSpeedRadiansPerSecond = math.pi
+    kMaxAngularSpeedRadiansPerSecondSquared = math.pi
+
+    kPXController = 1
+    kPYController = 1
+    kPThetaController = 1
+
+    kThetaControllerConstraints = TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
+    )
diff --git a/SwerveControllerCommand/robot.py b/SwerveControllerCommand/robot.py
new file mode 100644
index 00000000..3edb4e60
--- /dev/null
+++ b/SwerveControllerCommand/robot.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import TimedCommandRobot, CommandScheduler
+from robotcontainer import RobotContainer
+
+
+class Robot(TimedCommandRobot):
+    def robotInit(self):
+        """
+        Instantiate our RobotContainer. This will perform all our button bindings, and put our
+        autonomous chooser on the dashboard.
+        """
+        self.robotContainer = RobotContainer()
+
+    def robotPeriodic(self):
+        """
+        Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+        commands, running already-scheduled commands, removing finished or interrupted commands,
+        and running subsystem periodic() methods. This must be called from the robot's periodic
+        block in order for anything in the Command-based framework to work.
+        """
+
+    def disabledInit(self):
+        pass
+
+    def disabledPeriodic(self):
+        pass
+
+    def autonomousInit(self):
+        self.autonomousCommand = self.robotContainer.getAutonomousCommand()
+
+        """
+        schedule the autonomous command (example)
+        """
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self):
+        pass
+
+    def teleopInit(self):
+        """
+        This makes sure that the autonomous stops running when
+        teleop starts running. If you want the autonomous to
+        continue until interrupted by another command, remove
+        this line or comment it out.
+        """
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self):
+        pass
+
+    def testInit(self):
+        """
+        Cancels all running commands at the start of test mode.
+        """
+        CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self):
+        pass
diff --git a/SwerveControllerCommand/robotcontainer.py b/SwerveControllerCommand/robotcontainer.py
new file mode 100644
index 00000000..450b1031
--- /dev/null
+++ b/SwerveControllerCommand/robotcontainer.py
@@ -0,0 +1,116 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from wpilib import XboxController
+import commands2
+import constants
+from subsystems.drivesubsystems import DriveSubsystem
+from wpimath.trajectory import Trajectory, TrajectoryConfig, TrajectoryGenerator
+from wpimath.geometry import Translation2d, Pose2d, Rotation2d
+from wpimath.trajectory.constraint import DifferentialDriveVoltageConstraint
+from wpimath.controller import ProfiledPIDController, PIDController, HolonomicDriveController
+import math
+
+
+class RobotContainer:
+    def __init__(self):
+        """
+        The container for the robot. Contains subsystems, OI devices, and commands.
+        """
+        self.robotDrive = DriveSubsystem()
+
+        # Driver Controller
+        self.driverController = XboxController(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        """
+        Configure default commands
+        """
+        self.robotDrive.setDefaultCommand(
+            # The left stick controls translation of the robot.
+            # turning is controlled by the X axis of the right stick.
+            commands2.RunCommand(
+                lambda: self.robotDrive.drive(
+                    # Multiply by max speed to map the joystick unitless inputs to actual units.
+                    # This will map the [-1, 1] to [max speed backwards, max speed forwards],
+                    # converting them to actual units.
+                    self.driverController.getYChannel()
+                    * constants.DriveConstants.kMaxSpeedMetersPerSecond,
+                    self.driverController.getXChannel()
+                    * constants.DriveConstants.kMaxSpeedMetersPerSecond,
+                    self.driverController.getX(constants.Hand.kRight)
+                    * constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
+                    False,
+                ),
+                self.robotDrive,
+            )
+        )
+
+    def configureButtonBindings(self):
+        pass
+
+    def getAutonomousCommand(self):
+        """
+        Create config for trajectory
+        """
+
+
+        config = TrajectoryConfig(
+            constants.AutoConstants.kMaxSpeedMetersPerSecond,
+            constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared,
+        )
+        config.setKinematics(constants.DriveConstants.kDriveKinematics)
+
+        # An example trajectory to follow. All units in meters.
+
+        example_trajectory = TrajectoryGenerator.generateTrajectory(
+            # Start at the origin facing the +X direction
+            Pose2d(0, 0, Rotation2d(0)),
+            # Pass through these two interior waypoints, making an 's' curve path
+            [Translation2d(1, 1), Translation2d(2, -1)],
+            # End 3 meters straight ahead of where we started, facing forward
+            Pose2d(3, 0, Rotation2d(0)),
+            config
+        )
+
+        theta_controller = ProfiledPIDController(
+            constants.AutoConstants.kPThetaController,
+            0,
+            0,
+            constants.AutoConstants.kThetaControllerConstraints,
+        )
+        theta_controller.enableContinuousInput(-math.pi, math.pi)
+
+        swerve_controller_command = commands2.Swerve4ControllerCommand(
+            example_trajectory,
+            self.robotDrive.getPose,
+            # Functional interface to feed supplier
+            constants.DriveConstants.kDriveKinematics,
+            # Position controllers
+            HolonomicDriveController(
+                PIDController(constants.AutoConstants.kPXController, 0, 0),
+                PIDController(constants.AutoConstants.kPYController, 0, 0),
+                theta_controller
+            ),
+            Rotation2d(units.degrees_to_radians(0)),
+            self.robotDrive.setModuleStates,
+            self.robotDrive
+        )
+
+        """
+        Reset odometry to the initial pose of the trajectory, run path following
+        command, then stop at the end.
+        """
+        return commands2.Commands.sequence(
+            commands2.InstantCommand(
+                lambda: self.robotDrive.resetOdometry(
+                    example_trajectory.getInitialPose()
+                )
+            ),
+            swerve_controller_command,
+            commands2.InstantCommand(lambda: self.robotDrive.drive(0, 0, 0, False)),
+        )
\ No newline at end of file
diff --git a/SwerveControllerCommand/subsystems/drivesubsystems.py b/SwerveControllerCommand/subsystems/drivesubsystems.py
new file mode 100644
index 00000000..a6d25c6e
--- /dev/null
+++ b/SwerveControllerCommand/subsystems/drivesubsystems.py
@@ -0,0 +1,177 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from wpilib import ADXRS450_Gyro
+from swervemodule import SwerveModule
+from wpimath.kinematics import ChassisSpeeds
+from wpimath.kinematics import SwerveDrive4Kinematics, SwerveDrive4Odometry
+from wpimath.geometry import Pose2d, Rotation2d
+import constants
+
+
+class DriveSubsystem:
+    def __init__(self):
+        """
+        Robot swerve modules
+        """
+        self.frontLeft = SwerveModule(
+            constants.DriveConstants.kFrontLeftDriveMotorPort,
+            constants.DriveConstants.kFrontLeftTurningMotorPort,
+            constants.DriveConstants.kFrontLeftDriveEncoderPorts,
+            constants.DriveConstants.kFrontLeftTurningEncoderPorts,
+            constants.DriveConstants.kFrontLeftDriveEncoderReversed,
+            constants.DriveConstants.kFrontLeftTurningEncoderReversed,
+        )
+
+        self.rearLeft = SwerveModule(
+            constants.DriveConstants.kRearLeftDriveMotorPort,
+            constants.DriveConstants.kRearLeftTurningMotorPort,
+            constants.DriveConstants.kRearLeftDriveEncoderPorts,
+            constants.DriveConstants.kRearLeftTurningEncoderPorts,
+            constants.DriveConstants.kRearLeftDriveEncoderReversed,
+            constants.DriveConstants.kRearLeftTurningEncoderReversed,
+        )
+
+        self.frontRight = SwerveModule(
+            constants.DriveConstants.kFrontRightDriveMotorPort,
+            constants.DriveConstants.kFrontRightTurningMotorPort,
+            constants.DriveConstants.kFrontRightDriveEncoderPorts,
+            constants.DriveConstants.kFrontRightTurningEncoderPorts,
+            constants.DriveConstants.kFrontRightDriveEncoderReversed,
+            constants.DriveConstants.kFrontRightTurningEncoderReversed,
+        )
+
+        self.rearRight = SwerveModule(
+            constants.DriveConstants.kRearRightDriveMotorPort,
+            constants.DriveConstants.kRearRightTurningMotorPort,
+            constants.DriveConstants.kRearRightDriveEncoderPorts,
+            constants.DriveConstants.kRearRightTurningEncoderPorts,
+            constants.DriveConstants.kRearRightDriveEncoderReversed,
+            constants.DriveConstants.kRearRightTurningEncoderReversed,
+        )
+
+        # The gyro sensor
+
+        self.gyro = ADXRS450_Gyro()
+
+        # Odometry class for tracking robot pose
+
+        self.odometry = SwerveDrive4Odometry(
+            constants.DriveConstants.kDriveKinematics,
+            self.gyro.getRotation2d(),
+            [
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.rearLeft.getPosition(),
+                self.rearRight.getPosition(),
+            ],
+        )
+
+    def periodic(self):
+        """
+        Update the odometry in the periodic block
+        """
+        self.odometry.update(
+            self.gyro.getRotation2d(),
+            [
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.rearLeft.getPosition(),
+                self.rearRight.getPosition(),
+            ],
+        )
+
+    def getPose(self):
+        """
+        Returns the currently-estimated pose of the robot.
+        """
+        return self.odometry.getPoseMeters()
+
+    def resetOdometry(self, pose):
+        """
+        Resets the odometry to the specified pose.
+        """
+        self.odometry.resetPosition(
+            self.gyro.getRotation2d(),
+            [
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.rearLeft.getPosition(),
+                self.rearRight.getPosition(),
+            ],
+            pose,
+        )
+
+    def drive(self, xSpeed, ySpeed, rot, fieldRelative):
+        """
+        Method to drive the robot using joystick info.
+        """
+        swerveModuleStates = (
+            constants.DriveConstants.kDriveKinematics.toSwerveModuleStates(
+                ChassisSpeeds.discretize(
+                    (
+                        ChassisSpeeds.fromFieldRelativeSpeeds(
+                            xSpeed, ySpeed, rot, self.gyro.getRotation2d()
+                        )
+                        if fieldRelative
+                        else ChassisSpeeds(xSpeed, ySpeed, rot)
+                    ),
+                    constants.DriveConstants.kDrivePeriod,
+                )
+            )
+        )
+        SwerveDrive4Kinematics.desaturateWheelSpeeds(
+            swerveModuleStates, constants.DriveConstants.kMaxSpeedMetersPerSecond
+        )
+        self.frontLeft.setDesiredState(swerveModuleStates[0])
+        self.frontRight.setDesiredState(swerveModuleStates[1])
+        self.rearLeft.setDesiredState(swerveModuleStates[2])
+        self.rearRight.setDesiredState(swerveModuleStates[3])
+
+    def setModuleStates(self, desiredStates):
+        """
+        Sets the swerve ModuleStates.
+        """
+        SwerveDrive4Kinematics.desaturateWheelSpeeds(
+            desiredStates, constants.DriveConstants.kMaxSpeedMetersPerSecond
+        )
+        self.frontLeft.setDesiredState(desiredStates[0])
+        self.frontRight.setDesiredState(desiredStates[1])
+        self.rearLeft.setDesiredState(desiredStates[2])
+        self.rearRight.setDesiredState(desiredStates[3])
+
+
+    def resetEncoders(self):
+        """
+        Resets the drive encoders to currently read a position of 0.
+        """
+        self.frontLeft.resetEncoders()
+        self.rearLeft.resetEncoders()
+        self.frontRight.resetEncoders()
+        self.rearRight.resetEncoders()
+
+
+    def zeroHeading(self):
+        """
+        Zeroes the heading of the robot.
+        """
+        self.gyro.reset()
+
+
+    def getHeading(self):
+        """
+        Returns the heading of the robot.
+        """
+        return self.gyro.getRotation2d().getDegrees()
+
+
+    def getTurnRate(self):
+        """
+        Returns the turn rate of the robot.
+        """
+        return self.gyro.getRate() * (
+            -1.0 if constants.DriveConstants.kGyroReversed else 1.0
+        )
diff --git a/SwerveControllerCommand/subsystems/swervemodule.py b/SwerveControllerCommand/subsystems/swervemodule.py
new file mode 100644
index 00000000..714e717c
--- /dev/null
+++ b/SwerveControllerCommand/subsystems/swervemodule.py
@@ -0,0 +1,125 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from wpilib import Encoder, Spark
+from wpimath.geometry import Rotation2d
+from wpimath.kinematics import SwerveModulePosition, SwerveModuleState
+from wpimath.trajectory import TrapezoidProfile
+from wpimath.controller import PIDController, ProfiledPIDController
+import math
+import constants
+
+
+class SwerveModule:
+    """
+    Represents a swerve module.
+    """
+
+    def __init__(
+        self,
+        driveMotorChannel,
+        turningMotorChannel,
+        driveEncoderChannels,
+        turningEncoderChannels,
+        driveEncoderReversed,
+        turningEncoderReversed,
+    ):
+        self.driveMotor = Spark(driveMotorChannel)
+        self.turningMotor = Spark(turningMotorChannel)
+
+        self.driveEncoder = Encoder(*driveEncoderChannels)
+        self.turningEncoder = Encoder(*turningEncoderChannels)
+
+        self.drivePIDController = PIDController(
+            constants.ModuleConstants.kPModuleDriveController, 0, 0
+        )
+
+        self.turningPIDController = ProfiledPIDController(
+            constants.ModuleConstants.kPModuleTurningController,
+            0,
+            0,
+            TrapezoidProfile.Constraints(
+                constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
+                constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared,
+            ),
+        )
+
+        # Set the distance per pulse for the drive encoder.
+
+        self.driveEncoder.setDistancePerPulse(
+            constants.ModuleConstants.kDriveEncoderDistancePerPulse
+        )
+        self.driveEncoder.setReverseDirection(driveEncoderReversed)
+
+        # Set the distance in radians per pulse for the turning encoder.
+
+        self.turningEncoder.setDistancePerPulse(
+            constants.ModuleConstants.kTurningEncoderDistancePerPulse
+        )
+        self.turningEncoder.setReverseDirection(turningEncoderReversed)
+
+        # Limit the PID Controller's input range between -pi and pi and set the input
+        # to be continuous.
+
+        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
+
+    def getState(self):
+        """
+        Returns the state of the module
+        """
+        return SwerveModuleState(
+            self.driveEncoder.getRate(),
+            Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def getPosition(self):
+        """
+        Gets the position of the module based on the encoder
+        """
+
+        return SwerveModulePosition(
+            self.driveEncoder.getDistance(),
+            Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def setDesiredState(self, desiredState):
+        """
+        Using desiredState, calculate the drive and turn output
+        """
+
+        encoderRotation = Rotation2d(self.turningEncoder.getDistance())
+
+        # Optimize the reference state to avoid spinning further than 90 degrees
+
+        state = SwerveModuleState.optimize(desiredState, encoderRotation)
+
+        # Scale speed by cosine of angle error.
+
+        state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos()
+
+        # Calculate the drive output from the drive PID controller.
+
+        driveOutput = self.drivePIDController.calculate(
+            self.driveEncoder.getRate(), state.speedMetersPerSecond
+        )
+
+        # Calculate the turning motor output from the turning PID controller.
+
+        turnOutput = self.turningPIDController.calculate(
+            self.turningEncoder.getDistance(), state.angle.getRadians()
+        )
+
+        # Set motor outputs
+
+        self.driveMotor.set(driveOutput)
+        self.turningMotor.set(turnOutput)
+
+    def resetEncoders(self):
+        """
+        Resets the drive encoders
+        """
+        self.driveEncoder.reset()
+        self.turningEncoder.reset()
diff --git a/run_tests.sh b/run_tests.sh
index bb4f8e0f..4a4a61a6 100755
--- a/run_tests.sh
+++ b/run_tests.sh
@@ -49,6 +49,7 @@ BASE_TESTS="
   StateSpaceFlywheel
   StateSpaceFlywheelSysId
   SwerveBot
+  SwerveControllerCommand
   TankDrive
   TankDriveXboxController
   Timed/src