add drive subsystem as requirement and add max velocities

team401 · Nov 13, 2024 · e2aafe3 · e2aafe3
1 parent 4dccf94
commit e2aafe3
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java b/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java
@@ -13,12 +13,18 @@ public class DriveWithJoysticks extends Command {
     private DriveTemplate drive;
     private CommandJoystick leftJoystick;
     private CommandJoystick rightJoystick;
+    private double maxLinearVelocity;
+    private double maxAngularVelocity;
 
     public DriveWithJoysticks(
-            DriveTemplate drive, CommandJoystick leftJoystick, CommandJoystick rightJoystick) {
+            DriveTemplate drive, CommandJoystick leftJoystick, CommandJoystick rightJoystick, double maxLinearVelocity, double maxAngularVelocity) {
         this.drive = drive;
         this.leftJoystick = leftJoystick;
         this.rightJoystick = rightJoystick;
+        this.maxLinearVelocity = maxLinearVelocity;
+        this.maxAngularVelocity = maxAngularVelocity;
+
+        addRequirements(this.drive);
     }
 
     @Override
@@ -28,7 +34,7 @@ public void execute() {
         double omega = Deadband.oneAxisDeadband(rightJoystick.getX(), 0.1);
         omega = Math.copySign(omega * omega, omega);
 
-        drive.setGoalSpeeds(new ChassisSpeeds(linearSpeeds.getX(), linearSpeeds.getY(), omega), true);
+        drive.setGoalSpeeds(new ChassisSpeeds(linearSpeeds.getX() * maxLinearVelocity, linearSpeeds.getY() * maxLinearVelocity, omega * maxAngularVelocity), true);
     }
 
     public Translation2d getLinearVelocity(double x, double y) {