made a button to increase shuttle speed

grt192 · Mar 22, 2024 · ea82f06 · ea82f06
1 parent cefc35a
commit ea82f06
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -106,7 +106,9 @@ public class RobotContainer {
     private final JoystickButton offsetUpButton = new JoystickButton(switchboard, 7);
     private final JoystickButton offsetDownButton = new JoystickButton(switchboard, 8);
 
-    private final JoystickButton shuttleNotes = new JoystickButton(switchboard, 6);
+    private final JoystickButton toggleClimbModeSwitch = new JoystickButton(switchboard, 6);
+    private final JoystickButton shuttleNotesDefaultSpeed = new JoystickButton(switchboard, 6);
+    private final JoystickButton shuttleNotesIncreaseSpeed = new JoystickButton(switchboard, 2);
 
     private UsbCamera driverCamera;
     private MjpegServer driverCameraServer;
@@ -496,8 +498,12 @@ private void configureBindings() {
             () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0)))
         );
 
-        shuttleNotes.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, 
-            shooterFlywheelSubsystem, swerveSubsystem::getRobotPosition, shooterPivotSubsystem)
+        shuttleNotesDefaultSpeed.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, 
+            shooterFlywheelSubsystem, swerveSubsystem::getRobotPosition, shooterPivotSubsystem, 0.5)
+        );
+
+        shuttleNotesIncreaseSpeed.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, 
+            shooterFlywheelSubsystem, swerveSubsystem::getRobotPosition, shooterPivotSubsystem, 0.7)
         );
 
         /* SWERVE BINDINGS */

diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java
@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
 import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
@@ -15,25 +14,28 @@ public class ShooterFlywheelShuttleCommand extends Command {
 
     private static final double MAX_ROTATION_POWER = 0.3;
     private static final double ERROR_MULTIPLIER = 0.08;
-    private static final double TARGET_ANGLE = Math.toRadians(30);
+    private static final double TARGET_ANGLE = Units.degreesToRadians(70);
 
     private final SwerveSubsystem swerveSubsystem;
     private final ShooterFlywheelSubsystem flywheelSubsystem;
     private Pose2dSupplier poseSupplier; //new Pose2d();
     private final ShooterPivotSubsystem pivotSubsystem;
+    private final double shooterSpeed;
+
 
     private boolean redAlliance = true;
     private double angleOffset;
 
     /** Constructor for Shuttle Command. */
     public ShooterFlywheelShuttleCommand(SwerveSubsystem swerveSubsystem, 
            ShooterFlywheelSubsystem flywheelSubsystem, Pose2dSupplier poseSupplier, 
-           ShooterPivotSubsystem pivotSubsystem) {
+           ShooterPivotSubsystem pivotSubsystem, double speed) {
 
         this.swerveSubsystem = swerveSubsystem;
         this.flywheelSubsystem = flywheelSubsystem;
         this.poseSupplier = poseSupplier;
         this.pivotSubsystem = pivotSubsystem;
+        this.shooterSpeed = speed;
 
         addRequirements(swerveSubsystem, flywheelSubsystem);
     }
@@ -57,7 +59,7 @@ public void initialize() {
 
         double robotX = currentField.getX();
         double robotY = currentField.getY();
-        double robotAngle = currentField.getRotation().getDegrees();
+        double robotAngle = Units.degreesToRadians(currentField.getRotation().getDegrees());
 
         if (redAlliance) {
             angleOffset = getAngle(redAllianceX, redAllianceY, robotX, robotY) - robotAngle;
@@ -70,16 +72,15 @@ public void initialize() {
 
     /** Is robot in center line. */
     public boolean isFinished() {
-
-        return (poseSupplier.getPose2d().getX() < 326.365 + 70 || poseSupplier.getPose2d().getX() > 326.36 - 70);
+        return (poseSupplier.getPose2d().getX() < Units.inchesToMeters(326.365 + 70) || poseSupplier.getPose2d().getX() > Units.inchesToMeters(326.36 - 70));
     }
 
     /** What the command has to execute. */
     public void execute() {
 
         swerveSubsystem.setRobotRelativeDrivePowers(0, 0, -MathUtil.clamp(
             angleOffset * ERROR_MULTIPLIER, -MAX_ROTATION_POWER, MAX_ROTATION_POWER)); //CHANGE ANGULAR POWER
-        flywheelSubsystem.setShooterMotorSpeed(ShooterConstants.FLYWHEEL_SHUTTLE_SPEED);
+        flywheelSubsystem.setShooterMotorSpeed(shooterSpeed);
         pivotSubsystem.setAngle(TARGET_ANGLE);
 
     }