diff --git a/build.gradle b/build.gradle
index affa84d..b78c032 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/glass.json b/glass.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/glass.json
@@ -0,0 +1 @@
+{}
diff --git a/lib/src/rmb/controller/LogitechGamepad.h b/lib/src/rmb/controller/LogitechGamepad.h
index 6359f3e..5041f72 100644
--- a/lib/src/rmb/controller/LogitechGamepad.h
+++ b/lib/src/rmb/controller/LogitechGamepad.h
@@ -1,14 +1,17 @@
 #pragma once
 
+#include "frc/event/BooleanEvent.h"
+#include "frc/event/EventLoop.h"
 #include "wpi/MathExtras.h"
-#include <frc2/command/button/CommandGenericHID.h>
+#include "frc/GenericHID.h"
+// #include <frc2/command/button/CommandGenericHID.h>
 #include <frc2/command/button/Trigger.h>
 
 #include <iostream>
 
 namespace rmb {
 
-class LogitechGamepad : public frc2::CommandGenericHID {
+class LogitechGamepad : public frc::GenericHID {
 public:
   struct Axes {
     constexpr static int leftX = 0;
@@ -38,7 +41,7 @@ class LogitechGamepad : public frc2::CommandGenericHID {
 
   LogitechGamepad(int channel, double deadZone = 0.0,
                   bool squareOutputs = false)
-      : frc2::CommandGenericHID(channel), deadZone(deadZone),
+      : frc::GenericHID(channel), deadZone(deadZone),
         squareOutputs(squareOutputs) {}
 
   double GetLeftX() const {
@@ -52,11 +55,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
 
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
-  frc2::Trigger LeftXLessThan(double threshold) const {
-    return AxisLessThan(Axes::leftX, threshold);
+  frc::BooleanEvent LeftXLessThan(double threshold) const {
+    return AxisLessThan(Axes::leftX, threshold, loop);
   }
-  frc2::Trigger LeftXGreaterThan(double threshold) const {
-    return AxisGreaterThan(Axes::leftX, threshold);
+  frc::BooleanEvent LeftXGreaterThan(double threshold) const {
+    return AxisGreaterThan(Axes::leftX, threshold, loop);
   }
 
   double GetLeftY() const {
@@ -71,11 +74,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
 
-  frc2::Trigger LeftYLessThan(double threshold) const {
-    return AxisLessThan(Axes::leftY, threshold);
+  frc::BooleanEvent LeftYLessThan(double threshold) const {
+    return AxisLessThan(Axes::leftY, threshold, loop);
   }
-  frc2::Trigger LeftYgreaterThan(double threshold) const {
-    return AxisGreaterThan(Axes::leftY, threshold);
+  frc::BooleanEvent LeftYgreaterThan(double threshold) const {
+    return AxisGreaterThan(Axes::leftY, threshold, loop);
   }
 
   bool GetLeftStickButton() const { return GetRawButton(Buttons::leftStick); }
@@ -85,7 +88,7 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetLeftStickButtonReleased() {
     return GetRawButtonPressed(Buttons::leftStick);
   }
-  frc2::Trigger LeftStickButton() const { return Button(Buttons::leftStick); }
+  frc::BooleanEvent LeftStickButton() const { return Button(Buttons::leftStick, loop); }
 
   double GetRightX() const {
     double raw = -GetRawAxis(Axes::rightX);
@@ -99,11 +102,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
 
-  frc2::Trigger RightXLessThan(double threshold) const {
-    return AxisLessThan(Axes::rightX, threshold);
+  frc::BooleanEvent RightXLessThan(double threshold) const {
+    return AxisLessThan(Axes::rightX, threshold, loop);
   }
-  frc2::Trigger RightXgreaterThan(double threshold) const {
-    return AxisGreaterThan(Axes::rightX, threshold);
+  frc::BooleanEvent RightXgreaterThan(double threshold) const {
+    return AxisGreaterThan(Axes::rightX, threshold, loop);
   }
 
   double GetRightY() const {
@@ -118,11 +121,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
 
-  frc2::Trigger RightYLessThan(double threshold) const {
-    return AxisLessThan(Axes::rightY, threshold);
+  frc::BooleanEvent RightYLessThan(double threshold) const {
+    return AxisLessThan(Axes::rightY, threshold,loop);
   }
-  frc2::Trigger RightYGearterThan(double threshold) const {
-    return AxisGreaterThan(Axes::rightY, threshold);
+  frc::BooleanEvent RightYGearterThan(double threshold) const {
+    return AxisGreaterThan(Axes::rightY, threshold, loop);
   }
 
   bool GetRightStickButton() const { return GetRawButton(Buttons::rightStick); }
@@ -132,7 +135,7 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetRightStickButtonReleased() {
     return GetRawButtonPressed(Buttons::rightStick);
   }
-  frc2::Trigger RightStickButton() const { return Button(Buttons::rightStick); }
+  frc::BooleanEvent RightStickButton() const { return Button(Buttons::rightStick,loop); }
 
   double GetLeftTrigger() const {
     double raw = GetRawAxis(Axes::leftTrigger);
@@ -145,11 +148,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
 
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
-  frc2::Trigger LeftTriggerLessThan(double threshold) const {
-    return AxisLessThan(Axes::leftTrigger, threshold);
+  frc::BooleanEvent LeftTriggerLessThan(double threshold) const {
+    return AxisLessThan(Axes::leftTrigger, threshold,loop);
   }
-  frc2::Trigger LeftTriggergreaterThan(double threshold) const {
-    return AxisGreaterThan(Axes::leftTrigger, threshold);
+  frc::BooleanEvent LeftTriggergreaterThan(double threshold) const {
+    return AxisGreaterThan(Axes::leftTrigger, threshold,loop);
   }
 
   double GetRightTrigger() const {
@@ -163,11 +166,11 @@ class LogitechGamepad : public frc2::CommandGenericHID {
 
     return squareOutputs ? std::copysign(raw * raw, raw) : raw;
   }
-  frc2::Trigger RightTriggerLessThan(double threshold) const {
-    return AxisLessThan(Axes::rightTrigger, threshold);
+  frc::BooleanEvent RightTriggerLessThan(double threshold) const {
+    return AxisLessThan(Axes::rightTrigger, threshold,loop);
   }
-  frc2::Trigger RightTriggerGreaterThan(double threshold) const {
-    return AxisGreaterThan(Axes::rightTrigger, threshold);
+  frc::BooleanEvent RightTriggerGreaterThan(double threshold) const {
+    return AxisGreaterThan(Axes::rightTrigger, threshold,loop);
   }
 
   bool GetLeftBumper() const { return GetRawButton(Buttons::leftBumper); }
@@ -177,7 +180,7 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetLeftBumperReleased() {
     return GetRawButtonPressed(Buttons::leftBumper);
   }
-  frc2::Trigger LeftBumper() const { return Button(Buttons::leftBumper); }
+  frc::BooleanEvent LeftBumper() const { return Button(Buttons::leftBumper, loop); }
 
   bool GetRightBumper() const { return GetRawButton(Buttons::rightBumper); }
   bool GetRightBumperPressed() {
@@ -186,27 +189,27 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetRightBumperReleased() {
     return GetRawButtonPressed(Buttons::rightBumper);
   }
-  frc2::Trigger RightBumper() const { return Button(Buttons::rightBumper); }
+  frc::BooleanEvent RightBumper() const { return Button(Buttons::rightBumper,loop); }
 
   bool GetX() const { return GetRawButton(Buttons::X); }
   bool GetXPressed() { return GetRawButtonPressed(Buttons::X); }
   bool GetXReleased() { return GetRawButtonReleased(Buttons::X); }
-  frc2::Trigger X() const { return Button(Buttons::X); }
+  frc::BooleanEvent X() const { return Button(Buttons::X,loop); }
 
   bool GetY() const { return GetRawButton(Buttons::Y); }
   bool GetYPressed() { return GetRawButtonPressed(Buttons::Y); }
   bool GetYReleased() { return GetRawButtonReleased(Buttons::Y); }
-  frc2::Trigger Y() const { return Button(Buttons::Y); }
+  frc::BooleanEvent Y() const { return Button(Buttons::Y,loop); }
 
   bool GetA() const { return GetRawButton(Buttons::A); }
   bool GetAPressed() { return GetRawButtonPressed(Buttons::A); }
   bool GetAReleased() { return GetRawButtonReleased(Buttons::A); }
-  frc2::Trigger A() const { return Button(Buttons::A); }
+  frc::BooleanEvent A() const { return Button(Buttons::A,loop); }
 
   bool GetB() const { return GetRawButton(Buttons::B); }
   bool GetBPressed() { return GetRawButtonPressed(Buttons::B); }
   bool GetBReleased() { return GetRawButtonReleased(Buttons::B); }
-  frc2::Trigger B() const { return Button(Buttons::B); }
+  frc::BooleanEvent B() const { return Button(Buttons::B,loop); }
 
   bool GetBackButton() const { return GetRawButton(Buttons::backButton); }
   bool GetBackButtonPressed() {
@@ -215,7 +218,7 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetBackButtonReleased() {
     return GetRawButtonReleased(Buttons::backButton);
   }
-  frc2::Trigger BackButton() const { return Button(Buttons::backButton); }
+  frc::BooleanEvent BackButton() const { return Button(Buttons::backButton,loop); }
 
   bool GetStartButton() const { return GetRawButton(Buttons::startButton); }
   bool GetStartButtonPressed() {
@@ -224,9 +227,10 @@ class LogitechGamepad : public frc2::CommandGenericHID {
   bool GetStartButtonReleased() {
     return GetRawButtonReleased(Buttons::startButton);
   }
-  frc2::Trigger StartButton() const { return Button(Buttons::startButton); }
+  frc::BooleanEvent StartButton() const { return Button(Buttons::startButton,loop); }
 
 private:
+  frc::EventLoop *loop; 
   double deadZone;
   bool squareOutputs;
 };
diff --git a/lib/src/rmb/motorcontrol/sparkmax/SparkMaxPositionController.cpp b/lib/src/rmb/motorcontrol/sparkmax/SparkMaxPositionController.cpp
index acbd06f..052b01e 100644
--- a/lib/src/rmb/motorcontrol/sparkmax/SparkMaxPositionController.cpp
+++ b/lib/src/rmb/motorcontrol/sparkmax/SparkMaxPositionController.cpp
@@ -228,7 +228,7 @@ units::radians_per_second_t SparkMaxPositionController::getVelocity() const {
     rev::spark::SparkRelativeEncoder *rel =
         static_cast<rev::spark::SparkRelativeEncoder *>(&sparkMax.GetEncoder());
     return units::revolutions_per_minute_t(rel->GetVelocity() / gearRatio);
-    encoder.get();
+    //encoder.get();
   }
   case EncoderType::Alternate: {
     rev::spark::SparkMaxAlternateEncoder *alt =
diff --git a/src/main/cpp/Constants.h b/src/main/cpp/Constants.h
index 76c62a2..a926620 100644
--- a/src/main/cpp/Constants.h
+++ b/src/main/cpp/Constants.h
@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #pragma once
+#include <studica/AHRS.h>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -17,5 +18,6 @@
 namespace OperatorConstants {
 
 inline constexpr int driverControllerPort = 0;
+const studica::AHRS::NavXComType gyroPort = studica::AHRS::kMXP_SPI; 
 
 }  // namespace OperatorConstants
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 0fe53c4..753fedb 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -51,6 +51,10 @@ void Robot::TeleopInit() {
   if (m_autonomousCommand) {
     m_autonomousCommand->Cancel();
   }
+
+  frc2::CommandScheduler::GetInstance().CancelAll();
+  container.setTeleopDefaults();
+  
 }
 
 /**
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
index 930d35e..09e94d2 100644
--- a/src/main/cpp/RobotContainer.cpp
+++ b/src/main/cpp/RobotContainer.cpp
@@ -3,12 +3,13 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
+#include "main/cpp/subsystems/drive/DriveSubsystem.h"
 
 #include <frc2/command/button/Trigger.h>
 #include<frc2/command/CommandPtr.h>
 
 
-RobotContainer::RobotContainer() {
+RobotContainer::RobotContainer():driveSubsystem(gyro){
   // Initialize all of your commands and subsystems here
 
   // Configure the button bindings
@@ -23,3 +24,7 @@ void RobotContainer::ConfigureBindings() {
 frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   
 }
+
+void RobotContainer::setTeleopDefaults(){
+  driveSubsystem.SetDefaultCommand(driveSubsystem.driveTeleopCommand(gamepad));
+}
diff --git a/src/main/cpp/RobotContainer.h b/src/main/cpp/RobotContainer.h
index edd5747..0dc9643 100644
--- a/src/main/cpp/RobotContainer.h
+++ b/src/main/cpp/RobotContainer.h
@@ -8,7 +8,11 @@
 #include <frc2/command/button/CommandXboxController.h>
 
 #include "Constants.h"
+#include "main/cpp/subsystems/drive/DriveSubsystem.h"
+#include "rmb/controller/LogitechGamepad.h"
+#include "rmb/sensors/AHRS/AHRSGyro.h"
 #include "subsystems/ExampleSubsystem.h"
+#include <memory>
 #include <rev/SparkMax.h>
 //#include <frc/Joystick.h>
 
@@ -25,12 +29,14 @@ class RobotContainer {
 
   frc2::CommandPtr GetAutonomousCommand();
   frc2::CommandPtr runMotorCommand(); 
-
+  void setTeleopDefaults(); 
 
  private:
   // Replace with CommandPS4Controller or CommandJoystick if needed
-  frc2::CommandXboxController m_driverController{
+  rmb::LogitechGamepad gamepad{
       OperatorConstants::driverControllerPort};
+    std::shared_ptr<rmb::NavXGyro::Gyro> gyro = std::make_shared<rmb::NavXGyro>(OperatorConstants::gyroPort);
+    DriveSubsystem driveSubsystem; 
 
   // The robot's subsystems are defined here...
   ExampleSubsystem m_subsystem;
diff --git a/src/main/cpp/subsystems/drive/DriveConstants.h b/src/main/cpp/subsystems/drive/DriveConstants.h
index aac24e5..b6817c4 100644
--- a/src/main/cpp/subsystems/drive/DriveConstants.h
+++ b/src/main/cpp/subsystems/drive/DriveConstants.h
@@ -11,40 +11,44 @@
 namespace constants {
 namespace drive {
 
-using rmb::TalonFXPositionControllerHelper::CANCoderConfig;
+
 
 // PID Configs:
 const rmb::TalonFXPositionControllerHelper::PIDConfig positionPIDConfig{
-    .p = 0.0, .i=0.0, .d=0.0, .ff=0.0};
+    .p = 8.0, .i=0.0003, .d=0.0, .ff=1.0};
 const rmb::TalonFXVelocityControllerHelper::PIDConfig velocityPIDConfig{
-    .p=0.0, .i=0.0, .d=0.0, .ff=0.0, .kV=0.0};
+    .p= 4.0, .i=0.0, .d=0.0, .ff=0.0, .kV=0.0};
+const rmb::TalonFXVelocityControllerHelper::PIDConfig velocityPIDConfigA{
+    .p= 4.0, .i=0.0, .d=0.0, .ff=0.0, .kV=0.080};
+
+using rmb::TalonFXPositionControllerHelper::CANCoderConfig;
 
 // Magnet Module Offsets, Wheel Circumference, Robot Diameter, and Max Module
 // Speed
 
-const units::turn_t moduleMagnetOffset1 = 0.0_tr;
-const units::turn_t moduleMagnetOffset2 = 0.0_tr;
-const units::turn_t moduleMagnetOffset3 = 0.0_tr;
-const units::turn_t moduleMagnetOffset4 = 0.0_tr;
+const units::turn_t moduleMagnetOffset1(-0.163330078+0.5); 
+const units::turn_t moduleMagnetOffset2(0.028564453); 
+const units::turn_t moduleMagnetOffset3(-0.13671875);
+const units::turn_t moduleMagnetOffset4(0.30737304);
 
 const units::meter_t wheelCircumference = 4_in * std::numbers::pi;
-const units::meter_t robotDimX = 2.5_ft;
-const units::meter_t robotDimY = 2.5_ft;
+const units::meter_t robotDimX = 1.5_ft;
+const units::meter_t robotDimY = 1.5_ft;
 const units::meters_per_second_t maxModuleSpeed = 1_mps;
 
 // module 1
 const rmb::TalonFXVelocityController::CreateInfo velocityControllerCreateInfo1{
-    .config{.id = 11, .inverted = false, .brake = true},
+    .config{.id = 10, .inverted = false, .brake = true},
     .pidConfig = velocityPIDConfig,
     .profileConfig = {.maxVelocity = 100.0_tps,
                       .minVelocity = -100_tps,
                       .maxAcceleration = 10000_tr_per_s_sq},
-    .feedbackConfig = {.sensorToMechanismRatio = 6.82f},
+    .feedbackConfig = {.sensorToMechanismRatio = 6.12f},
     .openLoopConfig{.minOutput = -1.0, .maxOutput = 1.0, .rampRate = 0.0_s},
     .currentLimits{},
     .canCoderConfig = std::nullopt};
 
-const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo1{
+const rmb::TalonFXPositionController::CreateInfo positionControllerCreateInfo1{
     .config{.id = 12, .inverted = false, .brake = true},
     .pidConfig = positionPIDConfig,
     .range{.minPosition =
@@ -58,22 +62,22 @@ const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo1{
         },
     .openLoopConfig = {},
     .currentLimits = {},
-    .canCoderConfig{
-        .id = 10, .useIntegrated = false, .magnetOffset = moduleMagnetOffset1}};
+    .canCoderConfig = CANCoderConfig{
+        .id = 11, .useIntegrated = false, .magnetOffset = moduleMagnetOffset1}};
 
 // module 2
 const rmb::TalonFXVelocityController::CreateInfo velocityControllerCreateInfo2{
-    .config{.id = 21, .inverted = false, .brake = true},
-    .pidConfig = velocityPIDConfig,
+    .config{.id = 20, .inverted = false, .brake = true},
+    .pidConfig = velocityPIDConfigA,
     .profileConfig = {.maxVelocity = 100_tps,
                       .minVelocity = -100_tps,
                       .maxAcceleration = 10000_tr_per_s_sq},
-    .feedbackConfig = {.sensorToMechanismRatio = 6.82f},
+    .feedbackConfig = {.sensorToMechanismRatio = 6.12f},
     .openLoopConfig{.minOutput = -1.0, .maxOutput = 1.0, .rampRate = 0.0_s},
     .currentLimits{},
     .canCoderConfig = std::nullopt};
 
-const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo2{
+const rmb::TalonFXPositionController::CreateInfo positionControllerCreateInfo2{
     .config{.id = 22, .inverted = false, .brake = true},
     .pidConfig = positionPIDConfig,
     .range{.minPosition =
@@ -87,24 +91,24 @@ const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo2{
         },
     .openLoopConfig = {},
     .currentLimits = {},
-    .canCoderConfig{
-        .id = 20, .useIntegrated = false, .magnetOffset = moduleMagnetOffset2},
+    .canCoderConfig= CANCoderConfig{
+        .id = 21, .useIntegrated = false, .magnetOffset = moduleMagnetOffset2},
 };
 
 // module 3
 const rmb::TalonFXVelocityController::CreateInfo velocityControllerCreateInfo3{
-    .config{.id = 31, .inverted = false, .brake = true},
-    .pidConfig = velocityPIDConfig,
+    .config{.id = 30, .inverted = false, .brake = true},
+    .pidConfig = velocityPIDConfigA,
     .profileConfig = {.maxVelocity = 100.0_tps,
                       .minVelocity = -100_tps,
                       .maxAcceleration = 10000_tr_per_s_sq},
-    .feedbackConfig = {.sensorToMechanismRatio = 6.82f},
+    .feedbackConfig = {.sensorToMechanismRatio = 6.12f},
     .openLoopConfig{.minOutput = -1.0, .maxOutput = 1.0, .rampRate = 0.0_s},
     .currentLimits{},
     .canCoderConfig = std::nullopt,
 };
 
-const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo3{
+const rmb::TalonFXPositionController::CreateInfo positionControllerCreateInfo3{
     .config{.id = 32, .inverted = false, .brake = true},
     .pidConfig = positionPIDConfig,
     .range{.minPosition =
@@ -118,23 +122,23 @@ const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo3{
         },
     .openLoopConfig = {},
     .currentLimits = {},
-    .canCoderConfig{
-        .id = 30, .useIntegrated = false, .magnetOffset = moduleMagnetOffset3},
+    .canCoderConfig = CANCoderConfig{
+        .id = 31, .useIntegrated = false, .magnetOffset = moduleMagnetOffset3},
 };
 
 // module 4
 const rmb::TalonFXVelocityController::CreateInfo velocityControllerCreateInfo4{
-    .config{.id = 41, .inverted = false, .brake = true},
+    .config{.id = 40, .inverted = false, .brake = true},
     .pidConfig = velocityPIDConfig,
     .profileConfig = {.maxVelocity = 100.0_tps,
                       .minVelocity = -100_tps,
                       .maxAcceleration = 10000_tr_per_s_sq},
-    .feedbackConfig = {.sensorToMechanismRatio = 6.82f},
+    .feedbackConfig = {.sensorToMechanismRatio = 6.12f},
     .openLoopConfig{.minOutput = -1.0, .maxOutput = 1.0, .rampRate = 0.0_s},
     .currentLimits{},
     .canCoderConfig = std::nullopt};
 
-const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo4{
+const rmb::TalonFXPositionController::CreateInfo positionControllerCreateInfo4{
     .config{.id = 42, .inverted = false, .brake = true},
     .pidConfig = positionPIDConfig,
     .range{.minPosition =
@@ -148,8 +152,8 @@ const rmb::TalonFXPositionController::CreateInfo positionConstrollerCreateInfo4{
         },
     .openLoopConfig = {},
     .currentLimits = {},
-    .canCoderConfig{
-        .id = 40, .useIntegrated = false, .magnetOffset = moduleMagnetOffset4},
+    .canCoderConfig = CANCoderConfig{
+        .id = 41, .useIntegrated = false, .magnetOffset = moduleMagnetOffset4},
 };
 
 } // namespace drive
diff --git a/src/main/cpp/subsystems/drive/DriveSubsystem.cpp b/src/main/cpp/subsystems/drive/DriveSubsystem.cpp
index c462b7d..46fc106 100644
--- a/src/main/cpp/subsystems/drive/DriveSubsystem.cpp
+++ b/src/main/cpp/subsystems/drive/DriveSubsystem.cpp
@@ -3,8 +3,146 @@
 // the WPILib BSD license file in the root directory of this project.
 #pragma once
 #include "DriveSubsystem.h"
+#include "DriveConstants.h"
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "rmb/controller/LogitechGamepad.h"
+#include "rmb/drive/SwerveDrive.h"
+#include "rmb/drive/SwerveModule.h"
+#include "rmb/motorcontrol/AngularVelocityController.h"
+#include "rmb/motorcontrol/Talon/TalonFXPositionController.h"
+#include "rmb/motorcontrol/Talon/TalonFXVelocityController.h"
+#include "rmb/sensors/gyro.h"
+#include "units/angular_velocity.h"
+#include "units/velocity.h"
+#include <algorithm>
+#include <array>
+#include <memory>
+
+DriveSubsystem::DriveSubsystem(std::shared_ptr<rmb::Gyro> gyro) {
+    std::array<rmb::SwerveModule, 4> modules = {
+        rmb::SwerveModule(
+            rmb::asLinear(std::make_unique<rmb::TalonFXVelocityController>(
+            constants::drive::velocityControllerCreateInfo1), 
+            constants::drive::wheelCircumference/1_tr), 
+            std::make_unique<rmb::TalonFXPositionController>(
+            constants::drive::positionControllerCreateInfo1),
+            frc::Translation2d(
+            constants::drive::robotDimX/2.0, -constants::drive::robotDimY/2.0),
+            true),
+
+        rmb::SwerveModule(
+            rmb::asLinear(std::make_unique<rmb::TalonFXVelocityController>(
+            constants::drive::velocityControllerCreateInfo2), 
+            constants::drive::wheelCircumference/1_tr), 
+            std::make_unique<rmb::TalonFXPositionController>(
+            constants::drive::positionControllerCreateInfo2), 
+            frc::Translation2d(
+            constants::drive::robotDimX/2.0, constants::drive::robotDimY/2.0),
+            true),
+
+        rmb::SwerveModule(
+            rmb::asLinear(std::make_unique<rmb::TalonFXVelocityController>(
+            constants::drive::velocityControllerCreateInfo3), 
+            constants::drive::wheelCircumference/1_tr), 
+            std::make_unique<rmb::TalonFXPositionController>(
+            constants::drive::positionControllerCreateInfo3), 
+            frc::Translation2d(
+            -constants::drive::robotDimX/2.0, constants::drive::robotDimY/2.0),
+            true),
+
+        rmb::SwerveModule(
+            rmb::asLinear(std::make_unique<rmb::TalonFXVelocityController>(
+            constants::drive::velocityControllerCreateInfo4), 
+            constants::drive::wheelCircumference/1_tr), 
+            std::make_unique<rmb::TalonFXPositionController>(
+            constants::drive::positionControllerCreateInfo4), 
+            frc::Translation2d(
+            -constants::drive::robotDimX/2.0, -constants::drive::robotDimY/2.0),
+            true)
+            };
+
+    drive = std::make_unique<rmb::SwerveDrive<4>>(
+        std::move(modules), 
+        gyro, frc::HolonomicDriveController(
+        frc::PIDController(0.0f, 0.0f, 0.0f), 
+        frc::PIDController(0.0f, 0.0f, 0.0f), 
+        frc::ProfiledPIDController<units::radian>(
+        1.0f, 0.0f, 0.0f, 
+        frc::TrapezoidProfile<units::radian>::Constraints(
+        6.28_rad_per_s, 0.2_tr/1_s/1_s))), 
+        constants::drive::maxModuleSpeed); 
+
+    drive->resetPose();
+
+    std::cout<<"drive subsytem up"<<std::endl; 
+
+    nt::NetworkTableInstance ntInstance = nt::NetworkTableInstance::GetDefault();
+    std::shared_ptr<nt::NetworkTable> table =
+    ntInstance.GetTable("drivesubsystem");
+
+    anglePublisher = table->GetDoubleTopic("angle").Publish();
+    targetChassisSpeedsPublisher =
+    table->GetDoubleArrayTopic("targetChassis").Publish();
+    chassisSpeedsPublisher = table->GetDoubleArrayTopic("chassis").Publish();
+
+}
+
+void DriveSubsystem::driveTeleop(const rmb::LogitechGamepad &gamepad){
+    units::meters_per_second_t maxSpeed = 1_mps; 
+    units::turns_per_second_t maxRotation  = 1_tps;
+    // std::cout<<"yspeed controller" << gamepad.GetLeftX()<<std::endl; 
+    // std::cout<<"xspeed controller"<<gamepad.GetLeftY()<<std::endl; 
+    std::cout<<"rotation controller"<<gamepad.GetRightX()<<std::endl; 
+    units::meters_per_second_t xSpeed = -maxSpeed*gamepad.GetLeftY(); 
+    units::meters_per_second_t ySpeed = maxSpeed*gamepad.GetLeftX(); 
+    units::turns_per_second_t rotation = maxRotation*gamepad.GetRightX(); 
+
+    if(std::abs(gamepad.GetLeftX())<0.05){
+        ySpeed = 0_mps; 
+    }
+    if(std::abs(gamepad.GetLeftY())<0.05){
+        xSpeed = 0_mps; 
+    }
+    if(std::abs(gamepad.GetRightX())<0.05){
+        rotation = 0_tps; 
+    }
+    drive -> driveCartesian(xSpeed,
+                            ySpeed, 
+                            rotation, 
+                            true); 
+}
+
+void DriveSubsystem::driveTeleop(double x, double y, double z){
+    drive -> driveCartesian(x, y, x, false); 
+}
+
+
+frc2::CommandPtr DriveSubsystem::driveTeleopCommand(const rmb::LogitechGamepad &gamepad){
+    return frc2::RunCommand([&]()
+    {   drive->resetPose(); 
+        driveTeleop(gamepad);}, {this}).ToPtr(); 
+}
+
+frc2::CommandPtr DriveSubsystem::driveTeleopCommand(double x, double y, double z){
+    return frc2::RunCommand([&]()
+    {drive->driveCartesian(x, y, z, true);}, {this}).ToPtr();
+}
+
+
+frc2::CommandPtr DriveSubsystem::reset(){
+    return frc2::InstantCommand([this](){drive ->resetPose();}, {this}).ToPtr(); 
+}
 
-DriveSubsystem::DriveSubsystem() = default;
 
 // This method will be called once per scheduler run
-void DriveSubsystem::Periodic() {}
+void DriveSubsystem::Periodic() {
+
+    drive->updateNTDebugInfo(false);
+}
diff --git a/src/main/cpp/subsystems/drive/DriveSubsystem.h b/src/main/cpp/subsystems/drive/DriveSubsystem.h
index 3914706..889fa7a 100644
--- a/src/main/cpp/subsystems/drive/DriveSubsystem.h
+++ b/src/main/cpp/subsystems/drive/DriveSubsystem.h
@@ -4,18 +4,43 @@
 
 #pragma once
 
+#include "frc2/command/CommandPtr.h"
+#include "networktables/DoubleArrayTopic.h"
+#include "networktables/DoubleTopic.h"
+#include "rmb/drive/SwerveDrive.h"
+#include "rmb/sensors/gyro.h"
 #include <frc2/command/SubsystemBase.h>
+#include "rmb/controller/LogitechGamepad.h"
 
 class DriveSubsystem : public frc2::SubsystemBase {
  public:
-  DriveSubsystem();
-
+  enum brakeMode {
+    MOTOR_BRAKE = 0, 
+    ANGLE_BRAKE
+  }; 
+  DriveSubsystem(std::shared_ptr<rmb::Gyro> gyro);
   /**
    * Will be called periodically whenever the CommandScheduler runs.
    */
   void Periodic() override;
 
+  void driveTeleop(const rmb::LogitechGamepad &gamepad); 
+  void driveTeleop(double x , double y, double z); 
+
+  frc2::CommandPtr driveTeleopCommand(const rmb::LogitechGamepad &gamepad); 
+  frc2::CommandPtr driveTeleopCommand(double x , double y, double z); 
+
+  void brake(brakeMode mode); 
+
+  void motorStop(); 
+
+  frc2::CommandPtr reset(); 
+
  private:
-  // Components (e.g. motor controllers and sensors) should generally be
-  // declared private and exposed only through public methods.
+
+ std::unique_ptr<rmb::SwerveDrive<4>> drive; 
+ nt::DoublePublisher anglePublisher; 
+ nt::DoubleArrayPublisher targetChassisSpeedsPublisher; 
+ nt::DoubleArrayPublisher chassisSpeedsPublisher; 
+
 };
diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.1.1.json
rename to vendordeps/PathplannerLib-2025.2.1.json
index 6322388..71e25f3 100644
--- a/vendordeps/PathplannerLib-2025.1.1.json
+++ b/vendordeps/PathplannerLib-2025.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.1.1.json",
+    "fileName": "PathplannerLib-2025.2.1.json",
     "name": "PathplannerLib",
-    "version": "2025.1.1",
+    "version": "2025.2.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.1.1"
+            "version": "2025.2.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.1.1",
+            "version": "2025.2.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.2.0.json
similarity index 85%
rename from vendordeps/Phoenix6-frc2025-latest.json
rename to vendordeps/Phoenix6-25.2.0.json
index 7f4bd2e..38747fb 100644
--- a/vendordeps/Phoenix6-frc2025-latest.json
+++ b/vendordeps/Phoenix6-25.2.0.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-frc2025-latest.json",
+    "fileName": "Phoenix6-25.2.0.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.2.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.2.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.0",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib-2025.0.1.json
similarity index 86%
rename from vendordeps/REVLib.json
rename to vendordeps/REVLib-2025.0.1.json
index 2c39628..c998054 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib-2025.0.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib.json",
+    "fileName": "REVLib-2025.0.1.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json
similarity index 89%
rename from vendordeps/Studica-2025.0.0.json
rename to vendordeps/Studica-2025.0.1.json
index ddb0e44..5010be0 100644
--- a/vendordeps/Studica-2025.0.0.json
+++ b/vendordeps/Studica-2025.0.1.json
@@ -1,13 +1,13 @@
 {
-    "fileName": "Studica-2025.0.0.json",
+    "fileName": "Studica-2025.0.1.json",
     "name": "Studica",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
     "frcYear": "2025",
     "mavenUrls": [
         "https://dev.studica.com/maven/release/2025/"
     ],
-    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
     "cppDependencies": [
         {
             "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
             "libName": "Studica",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         },
         {
             "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
             "libName": "StudicaDriver",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "javaDependencies": [
         {
             "artifactId": "Studica-java",
             "groupId": "com.studica.frc",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
@@ -65,7 +65,7 @@
                 "osxuniversal",
                 "windowsx86-64"
             ],
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ]
 }
\ No newline at end of file