Merge branch 'main' of https://github.com/redshiftrobotics/crescendo-…

…2024

src/main/java/frc/robot/RobotContainer.java

@@ -62,7 +62,7 @@ public class RobotContainer {
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
-        autoChooser.addOption("Example Auto", Autos.exampleAuto(drivetrain));
+        autoChooser.addOption("Testing Auto", Autos.testingAuto(drivetrain));
 
         setUpDriveController();
         configureBindings();
@@ -74,8 +74,8 @@ public void setUpDriveController() {
         final HIDType genericHIDType = genericHID.getType();
 
         SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-        SmartDashboard.putString("Bot Name", Constants.currentBot.name());
-        
+        SmartDashboard.putString("Bot Name", Constants.currentBot.toString());
+
         if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
             final CommandJoystick driverJoystick = new CommandJoystick(genericHID.getPort());
             SwerveDriveJoystickControl control = new SwerveDriveJoystickControl(drivetrain, driverJoystick);

src/main/java/frc/robot/commands/Autos.java

@@ -10,7 +10,7 @@
 
 public final class Autos {
     /** Example static factory for an autonomous command. */
-    public static Command exampleAuto(SwerveDrivetrain drivetrain) {
+    public static Command testingAuto(SwerveDrivetrain drivetrain) {
         return Commands.sequence(
             new AutoDriveTo(drivetrain, new Translation2d(1, 0))
             // new WaitCommand(1),

src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java

@@ -1,43 +1,35 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.RobotMovementConstants;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.utils.OptionButton;
+import frc.robot.utils.OptionButton.ActivationMode;
 
 // Here is the documentation for the xbox controller code:
 // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.html
 
 /**
  * This is the default command for the drivetrain, allowing for remote operation with xbox controller
  */
-public class SwerveDriveXboxControl extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final CommandXboxController controller;
-    private final PIDController robotAnglePID;
-
+public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
+    private final OptionButton preciseModeButton;
+    private final OptionButton boostModeButton;
     /**
      * Creates a new SwerveDriveXboxControl Command.
      *
      * @param drivetrain           The drivetrain of the robot
      * @param driverXboxController The xbox controller used to control drivetrain
      */
     public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
-        this.drivetrain = drivetrain;
-        this.controller = driverXboxController;
-        this.robotAnglePID = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
-        robotAnglePID.enableContinuousInput(0, 2*Math.PI);
-
+        super(drivetrain, driverXboxController);
         // Create and configure buttons
         // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
-
+        preciseModeButton = new OptionButton(driverXboxController, 8, ActivationMode.HOLD);
+        boostModeButton = new OptionButton(driverXboxController, 1, ActivationMode.HOLD);
         // Tell the command schedular we are using the drivetrain
         addRequirements(drivetrain);
     }
@@ -57,31 +49,22 @@ public void initialize() {
      */
     @Override
     public void execute() {
+        final CommandXboxController xboxController = (CommandXboxController) controller;
 
-        double leftX = applyJoystickDeadzone(controller.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
-        double leftY = applyJoystickDeadzone(controller.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
+        double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
+        double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
 
-        double rightX = -applyJoystickDeadzone(controller.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
+        double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
 
-    /* OLD ROTATION CODE
-        double targetAngle = Math.atan2(rightX, rightY);
-    
-        final double currentAngle = drivetrain.getHeading().getRadians();
-        final double turnSpeed = robotAnglePID.calculate(currentAngle, targetAngle);
+        int speedLevel = 1
+        - preciseModeButton.getStateAsInt()
+        + ;
 
         final ChassisSpeeds speeds = new ChassisSpeeds(
-                leftX * RobotMovementConstants.maxSpeed,
-                leftY * RobotMovementConstants.maxSpeed,
-                turnSpeed);
-    */
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            leftX * RobotMovementConstants.maxSpeed,
-            leftY * RobotMovementConstants.maxSpeed,
-            rightX * RobotMovementConstants.maxTurnSpeed);
-
+            leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+            leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
         drivetrain.setDesiredState(speeds, false);
-
     }
 
     /**