cleaned up more code

FRC5190 · Mar 19, 2024 · ed62566 · ed62566
1 parent 7c5cd69
commit ed62566
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 2 deletions.
diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java
@@ -168,7 +168,7 @@ private void setupTeleopControls() {
 
     operator_controller_.a().onTrue(new ArmPID(arm_, 2));
 
-    operator_controller_.y().onTrue(new ArmPID(arm_, 25));
+    operator_controller_.y().onTrue(superstructure_.setPosition(Superstructure.Position.SUBWOOFER));
 
     operator_controller_.x().onTrue(new ArmPID(arm_, 55));
 

diff --git a/src/main/java/org/ghrobotics/frc2024/ShootingPosition.java b/src/main/java/org/ghrobotics/frc2024/ShootingPosition.java
@@ -18,6 +18,12 @@ public double distanceToSpeaker(Pose2d robotPose, Pose2d speakerPose) {
   public double regressionFormula(double x) {
     double y = (10.0286 * Math.log((9.48057 * x) - 8.57204)) + 8.226559;
 
+    if (y > 50) {
+      y = 2;
+    } else if (y < 0) {
+      y = 2;
+    }
+
     return y;
   }
 

diff --git a/src/main/java/org/ghrobotics/frc2024/Superstructure.java b/src/main/java/org/ghrobotics/frc2024/Superstructure.java
@@ -47,6 +47,8 @@ public void periodic() {
 
     SmartDashboard.putNumber("Shooting Angle", armShootingAngle);
 
+    SmartDashboard.putString("Superstructure State", state);
+
     // Checks output current to see if note has intaked or not (current > 35 means intaked)
     if (intake_.getLeftOutputCurrent() > 35) {
       LimelightHelpers.setLEDMode_ForceOn("limelight");
@@ -59,7 +61,7 @@ public Command setPosition(Position pos) {
       new InstantCommand(() -> this.state = pos.posname),
       new ParallelCommandGroup(
         new ArmPID(arm_, pos.angle)
-      ).withTimeout(2)
+      ).withTimeout(3.5)
     );
   }
 

diff --git a/src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java b/src/main/java/org/ghrobotics/frc2024/auto/AutoSelector.java
@@ -17,6 +17,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -34,6 +35,9 @@ public class AutoSelector {
   private final Shooter shooter_;
   private final Feeder feeder_;
 
+  // Sendable chooser
+  private final SendableChooser<Command> routine_chooser_;
+
   // Different Paths
   PathPlannerPath middle_middle_intake_path = PathPlannerPath.fromPathFile("middle_middle_intake");
   PathPlannerPath middle_middle_shoot_path = PathPlannerPath.fromPathFile("middle_middle_shoot");
@@ -60,6 +64,9 @@ public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstr
     shooter_ = shooter;
     feeder_ = feeder;
 
+    routine_chooser_ = AutoBuilder.buildAutoChooser();
+    routine_chooser_.setDefaultOption("Four Note Auto", fourNoteFull());
+
     stop_all_motor = new ParallelCommandGroup(
       new InstantCommand(() -> intake_.stopMotor()),
       new InstantCommand(() -> feeder_.stopMotor()),
@@ -90,6 +97,10 @@ public AutoSelector(Drive drive, RobotState robot_state, Superstructure superstr
     );
   }
 
+  public Command getSelectedRoutine() {
+    return routine_chooser_.getSelected();
+  }
+
   public Command fourNoteFull() {
     return new SequentialCommandGroup(
       fourNotePt1().withTimeout(5.0),