Small convience changes

Bindings Selector Added
Team2068 · Oct 3, 2023 · e26f6b0 · e26f6b0
2 parents be6ea37 + dfa5cb0
commit e26f6b0
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -7,8 +7,6 @@
 import java.util.HashMap;
 import java.util.List;
 
-import frc.robot.Constants.Paths;
-import frc.robot.commands.AutonBalance;
 import frc.robot.commands.Score;
 import frc.robot.commands.SetClawPosition;
 import frc.robot.commands.SetTelescopePosition;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -77,13 +77,14 @@ public void autonomousPeriodic() {}
   @Override
   public void teleopInit() {
     if (m_autonomousCommand != null) m_autonomousCommand.cancel();
-    m_robotContainer.io.configTesting();
+    m_robotContainer.bindingsSelector.getSelected().run();
     // m_robotContainer.io.configTeleop();
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
   public void testInit() {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -9,7 +9,6 @@
 import frc.robot.commands.AutonBalance;
 import frc.robot.commands.Score;
 import frc.robot.commands.SetClawPosition;
-import frc.robot.commands.SetTelescopePosition;
 import frc.robot.utilities.General;
 import frc.robot.utilities.IO;
 import edu.wpi.first.cameraserver.CameraServer;
@@ -22,15 +21,17 @@
 public class RobotContainer {
 
   final SendableChooser<Command> autonomousSelector = new SendableChooser<>();
+  final SendableChooser<Runnable> bindingsSelector = new  SendableChooser<Runnable>();
 
-  public IO io = new IO();
+  public IO io = new IO(bindingsSelector);
 
   public RobotContainer() {
     Constants.initEventMap(io);
     configureAutonomous();
     io.configGlobal();
 
     SmartDashboard.putData("Auto Selector", autonomousSelector);
+    SmartDashboard.putData("Bindings Selector", bindingsSelector);
     CameraServer.startAutomaticCapture();
   }
 
@@ -86,4 +87,5 @@ public Command getAutonomousCommand() {
     }, io.driveSubsystem);
     return autonomousSelector.getSelected().andThen(postAutonomous);
   }
+
 }
diff --git a/src/main/java/frc/robot/commands/SetTelescopePosition.java b/src/main/java/frc/robot/commands/SetTelescopePosition.java
@@ -41,6 +41,6 @@ public boolean isFinished() {
   @Override
   public void end(boolean interrupted) {
     System.out.println("TELESCOPE HAS REACHED POSITION :DDDD");
-    telescopeSubsystem.stopTelescope();
+    // telescopeSubsystem.stopTelescope();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ClawSubsystem.java b/src/main/java/frc/robot/subsystems/ClawSubsystem.java
@@ -13,7 +13,6 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import com.revrobotics.CANSparkMax.ControlType;
 import frc.robot.Constants.ClawConstants;
 
 public class ClawSubsystem extends SubsystemBase {
@@ -35,7 +34,7 @@ public ClawSubsystem() {
         wristMotor.setSmartCurrentLimit(15);
 
         wristMotor.getEncoder().setPositionConversionFactor(360/31);
-        // TODO: Set wristMotor's on-board encoder to be the absolute encoder[[[==]]]
+        // TODO: Set wristMotor's on-board encoder to be the absolute encoder [[[==]]]
         // wristMotor.getPIDController().setP(0.00001);
         syncWrist();
 

diff --git a/src/main/java/frc/robot/utilities/IO.java b/src/main/java/frc/robot/utilities/IO.java
@@ -5,8 +5,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.ClawConstants;
@@ -34,6 +34,12 @@ public class IO {
 
     public SetArmProfiled armCommand = new SetArmProfiled(73, arm, telescope, photon::rotateMount, true);
 
+    public IO(SendableChooser<Runnable> bindings){
+        bindings.setDefaultOption("", this::configTeleop);
+        bindings.addOption("Testing", this::configTesting);
+        bindings.addOption("Demo", this::configDemo);
+    }
+
     public void configGlobal() {
         photon.camera.setPipelineIndex(1);
         photon.mount.setAngle(PhotonConstants.FORWARD_ANGLE);
@@ -74,24 +80,23 @@ public void configTeleop() {
         driveController.x().onTrue(General.Instant(driveSubsystem::zeroGyro));
     }
 
-    public void configTesting() {
+    public void configTesting(){
         SmartDashboard.putData("Kill LEDs", General.Instant(leds::killLeds));
         PathPlannerServer.startServer(5811);
 
-        // driveController.a().onTrue(General.Instant(claw::syncWrist));
-        // driveController.b().onTrue(General.Instant(() -> claw.setPosition(0)));
-        driveController.x().onTrue(General.Instant(() -> telescope.setPosition(TelescopeConstants.LOW_POSITION)));
-        driveController.y().onTrue(General.Instant(() -> telescope.setPosition(TelescopeConstants.HIGH_POSITION)));
-
-        driveController.leftBumper().onTrue(General.Instant(armCommand::stop));
-        driveController.rightBumper().onTrue(General.Instant(() -> armCommand.setAngle(60)));
+        // driveController.a().onTrue(runSystemsCheck());
+        // driveController.b().onTrue(General.Instant(driveSubsystem::syncEncoders));
+    }
 
-        driveController.leftTrigger().onTrue(General.Instant(telescope::retract));
-        driveController.rightTrigger().onTrue(General.Instant(telescope::extend));
+    public void configDemo() {
+        driveController.a().onTrue(General.Instant(() -> armCommand.setAngle(60)));
+        driveController.b().onTrue(General.Instant(() -> armCommand.setAngle(-60)));
+        driveController.x().onTrue(General.Instant(armCommand::stop));
+        driveController.y().onTrue(General.Instant(driveSubsystem::resetOdometry));
 
-        // driveController.leftTrigger().onTrue(General.Instant(claw::closeClaw, leds::setBlue));
-        // driveController.rightTrigger().onTrue(General.Instant(claw::openClaw, leds::setYellow));
-    } 
+        driveController.leftBumper().onTrue(General.Instant(() -> telescope.setPosition(TelescopeConstants.HIGH_POSITION), leds::setBlue));
+        driveController.rightBumper().onTrue(General.Instant(() -> telescope.setPosition(1), leds::setYellow));
+    }
 
     public Command runSystemsCheck(){
         driveSubsystem.syncEncoders();
@@ -102,7 +107,7 @@ public Command runSystemsCheck(){
 
             // TODO: Extend the drive sys tests to cover moving forward, back, left, right, rotating clock-wise & counter clock-wise
 
-            General.Instant(()->driveSubsystem.drive(new ChassisSpeeds(10,0,0)),
+          General.Instant(()->driveSubsystem.drive(new ChassisSpeeds(10,0,0)),
                           ()->armCommand.setAngle(60)),
           new WaitCommand(0.5),
           General.Instant(()-> armCommand.setAngle(-60)),