testing changes shuttling

grt192 · Apr 6, 2024 · b6d6d42 · b6d6d42
1 parent 3101a31
commit b6d6d42
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -534,6 +534,11 @@ private void configureBindings() {
                     mechController.setRumble(RumbleType.kBothRumble, 0);
                 }
             }
+            if (shooterFlywheelSubsystem.atSpeed()) {
+                mechController.setRumble(RumbleType.kBothRumble, .4);
+            } else {
+                mechController.setRumble(RumbleType.kBothRumble, 0);
+            }
 
             if (noteInBack 
                 && !intakeRollerSubsystem.getRockwellSensorValue()
@@ -550,7 +555,7 @@ private void configureBindings() {
         }, shooterFlywheelSubsystem
         ));
 
-        dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .6).onlyWhile(dPadRight));
+        dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .65, mechController).onlyWhile(dPadRight));
 
         // intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
         //     intakePosition = MathUtil.clamp(intakePosition, 0, 1);

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,6 +4,8 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.FieldManagementSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
@@ -16,16 +18,17 @@ 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 = Units.degreesToRadians(70);
+    private static final double TARGET_ANGLE = Units.degreesToRadians(42);
 
-    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(323));
-    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(323));
+    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(300));
+    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(300));
 
     private final SwerveSubsystem swerveSubsystem;
     private final ShooterFlywheelSubsystem flywheelSubsystem;
     private final ShooterPivotSubsystem pivotSubsystem;
     private final FieldManagementSubsystem fmsSubsystem;
     private final double shooterSpeed;
+    private XboxController controller;
 
 
     private boolean redAlliance = true;
@@ -34,13 +37,14 @@ public class ShooterFlywheelShuttleCommand extends Command {
     /** Constructor for Shuttle Command. */
     public ShooterFlywheelShuttleCommand(SwerveSubsystem swerveSubsystem, 
            ShooterFlywheelSubsystem flywheelSubsystem, FieldManagementSubsystem fmsSubsystem, 
-           ShooterPivotSubsystem pivotSubsystem, double speed) {
+           ShooterPivotSubsystem pivotSubsystem, double speed, XboxController mechController) {
 
         this.swerveSubsystem = swerveSubsystem;
         this.flywheelSubsystem = flywheelSubsystem;
         this.pivotSubsystem = pivotSubsystem;
         this.fmsSubsystem = fmsSubsystem;
         this.shooterSpeed = speed;
+        this.controller = mechController;
 
         addRequirements(flywheelSubsystem, pivotSubsystem);
     }
@@ -55,6 +59,11 @@ public void initialize() {
     public void execute() {
         flywheelSubsystem.setShooterMotorSpeed(shooterSpeed);
         pivotSubsystem.setAngle(TARGET_ANGLE);
+        if(flywheelSubsystem.atSpeed()) {
+            controller.setRumble(RumbleType.kBothRumble, .4);
+        } else {
+            controller.setRumble(RumbleType.kBothRumble, 0);
+        }
     }
 
     /** Run after command has ended. Turns of shooter and swerve speed.*/

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -114,7 +114,7 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re
                            Units.degreesToRadians(36), 
                            Units.degreesToRadians(31), //4
                            Units.degreesToRadians(28),
-                           Units.degreesToRadians(27),
+                           Units.degreesToRadians(27.5),
                            Units.degreesToRadians(28.5)};
 
         // X = distances, Y = angles in rads