Merge pull request #16 from Team-2502/rotini

slower mode
Team-2502 · Nov 17, 2023 · 229c83d · 229c83d
2 parents ff114d1 + adfa490
commit 229c83d
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 5 deletions.
diff --git a/src/main/java/com/team2502/robot2023/Constants.java b/src/main/java/com/team2502/robot2023/Constants.java
@@ -148,7 +148,7 @@ public static final class Leds {
         public static final class Arm {
             public static final boolean NT_TUNE = false;
 
-            public static final double ELEVATOR_LIM_TOP = 85;
+            public static final double ELEVATOR_LIM_TOP = 90;
             public static final double ELEVATOR_LIM_BOTTOM = 0;
 
             public static final double ELEVATOR_P = 0.5;

diff --git a/src/main/java/com/team2502/robot2023/RobotContainer.java b/src/main/java/com/team2502/robot2023/RobotContainer.java
@@ -54,7 +54,7 @@ public class RobotContainer {
 
     protected final LightstripSubsystem LIGHTSTRIP = new LightstripSubsystem();
 
-    protected final PhotonVisionSubsystem VISION = new PhotonVisionSubsystem(DRIVETRAIN);
+    //protected final PhotonVisionSubsystem VISION = new PhotonVisionSubsystem(DRIVETRAIN);
 
     public RobotContainer() {
         DRIVETRAIN.setDefaultCommand(new DriveCommand(DRIVETRAIN, JOYSTICK_DRIVE_LEFT, JOYSTICK_DRIVE_RIGHT));

diff --git a/src/main/java/com/team2502/robot2023/commands/DriveCommand.java b/src/main/java/com/team2502/robot2023/commands/DriveCommand.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class DriveCommand extends CommandBase {
     private final DrivetrainSubsystem drivetrain;
@@ -46,6 +47,7 @@ private enum Drivetype {
     private boolean slowmode =  false;
     private boolean prevTog = false;
 
+    private boolean slower = false;
     private final SendableChooser<Drivetype> typeEntry = new SendableChooser<>();
     private final SendableChooser<DriveController> controllerEntry = new SendableChooser<>();
 
@@ -70,6 +72,8 @@ public DriveCommand(DrivetrainSubsystem drivetrain, Joystick joystickDriveLeft,
         controllerEntry.addOption("Xbox", DriveController.Xbox);
         SmartDashboard.putData("Drive Controller", controllerEntry);
 
+        SmartDashboard.putBoolean("slower", slower);
+
         addRequirements(drivetrain);
     }
 
@@ -86,6 +90,7 @@ public void execute() {
         prevTog = tog;
 
         SmartDashboard.putBoolean("iosajioj", slowmode);
+        slower = SmartDashboard.getBoolean("slower", false);
 
         ChassisSpeeds speeds;
         Translation2d centerOfRotation;
@@ -140,9 +145,13 @@ public void execute() {
                     break;
                 case FieldOrientedTwistRetDead:
                     speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
-                            Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getY(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-                            -Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getX(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL),
-                            -Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.04, rightJoystick.getZ(), 0.55 * 0.6) * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
+                            Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getY(), 0.45 * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL))
+                            * (slower ? 0.12 : 1),
+                            -Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getX(), 0.45 * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL))
+                            * (slower ? 0.12 : 1),
+                            -Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.04, rightJoystick.getZ(), 0.55)
+                            * (slower ? 0.35 : 1)
+                            * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT),
                             Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset));
                     centerOfRotation = new Translation2d(0, 0);
                     drivetrain.setSpeeds(speeds, centerOfRotation);