Made Romi DriveTrain

src/main/java/frc/team5115/RobotContainer.java

@@ -1,16 +1,18 @@
 public class RobotContainer {
     private final int number;
+    private final Drive drive;
     public RobotContainer(){
         switch (Constants.currentMode) {
             case REAL:
-                number = 5;
+                drive = new Drive(new DriveIORomi());
                 break;
             case SIM:
-                number 20;
+                drive = new Drive(new DriveIO(){});
                 break;
             default:
-                number = 10;
+                drive = new Drive(new DriveIO(){});
                 break;
         }
+        drive.setDefaultCommand(drive.drive());
     }
 }

src/main/java/frc/team5115/drive/Drive.java

@@ -1,10 +1,36 @@
 public class Drive extends SubsystemBase {
 
-    public Drive{
 
-    }
     private final DriveIOInputsAutoLogged inputs = new DriveIOInputsAutoLogged();
     private final DriveIO io;
-
+
+    public Drive(DriveIO io){
+        this.io = io;  
+    }
+    private tankDrive(double lateral, double rotational) {
+        double l_Speed = lateral + rotational;
+        double r_Speed = lateral - rotational;
+        if (Math.abs(l_Speed) > 1) {
+            r_Speed = r_Speed/l_Speed * Math.sign(l_Speed);
+            l_Speed = 1;
+        } 
+        if (Math.abs(r_Speed) > 1) {
+            l_Speed = l_Speed/r_Speed * Math.sign(r_Speed);
+            r_Speed = 1;
+        }
+        io.setLeftVoltage(5*l_Speed);
+        io.setRightVoltage(5*r_Speed);
+    }
+    public Command drive(DoubleSupplier lateralSupplier, DoubleSupplier rotationalSupplier) {
+        Commands.run(() -> {
+            tankDrive(lateralSupplier.get(), rotationalSupplier.get());
+        }, this);
+    }
+
+
+    @Override
+    public void periodic() {
+
+    }
 
 }

src/main/java/frc/team5115/drive/DriveIORomi.java

@@ -13,7 +13,8 @@ public class DriveIORomi implements DriveIO {
     private final Encoder m_leftEncoder = new Encoder(4, 5);
     private final Encoder m_rightEncoder = new Encoder(6, 7);
     public DriveIORomi(){
-
+        m_rightMotor.setInverted(true);
+        m_leftMotor.setInverted(false);
     }
     @override
     public void updateInputs (DriveIOInputs inputs) {