From fc3cb155946a71e13754495b8eff20202ffa4dfc Mon Sep 17 00:00:00 2001 From: Sha-dos <77746688+Sha-dos@users.noreply.github.com> Date: Thu, 16 Nov 2023 19:18:22 -0600 Subject: [PATCH] slower mode Co-authored-by: Andy Killorin <37423245+speedy6451@users.noreply.github.com> --- .../java/com/team2502/robot2023/Constants.java | 2 +- .../com/team2502/robot2023/RobotContainer.java | 2 +- .../robot2023/commands/DriveCommand.java | 16 +++++++++++++--- 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team2502/robot2023/Constants.java b/src/main/java/com/team2502/robot2023/Constants.java index d061e6c..7b43ab4 100644 --- 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 index 07078fb..78146c2 100644 --- 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 index 1b66267..9d00b9c 100644 --- 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,8 @@ private enum Drivetype { private boolean slowmode = false; private boolean prevTog = false; + private boolean slower = false; + private final SendableChooser typeEntry = new SendableChooser<>(); private final SendableChooser controllerEntry = new SendableChooser<>(); @@ -70,6 +73,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 +91,7 @@ public void execute() { prevTog = tog; SmartDashboard.putBoolean("iosajioj", slowmode); + slower = SmartDashboard.getBoolean("slower", false); ChassisSpeeds speeds; Translation2d centerOfRotation; @@ -140,9 +146,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);