hopper fixes

Author: sophiep29

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

@@ -26,16 +26,19 @@ public class RobotContainer {
   private Kicker kicker = new Kicker();
   private CommandXboxController controller = new CommandXboxController(1);
   private SwerveDriveState driveState = new SwerveDriveState();
+  public static boolean isExtended = false;
 
   private Command shootGroup =
       Commands.parallel(
               new KickerCommand(kicker, KickerCommand.Position.INTAKE),
-              new HopperCommand(hopper, HopperCommand.Position.SHOOT_RETRACT),
+              new HopperCommand(hopper, HopperCommand.Position.SHOOT_RETRACT, isExtended),
               new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE))
           .finallyDo(
               interrupt ->
                   CommandScheduler.getInstance()
-                      .schedule(new HopperCommand(hopper, HopperCommand.Position.SHOOT_EXTEND)));
+                      .schedule(
+                          new HopperCommand(
+                              hopper, HopperCommand.Position.SHOOT_EXTEND, isExtended)));
 
   public RobotContainer() {
     configureBindings();
@@ -52,8 +55,8 @@ private void configureBindings() {
             controller::getLeftY,
             controller::getRightX));
     // Flywheel
-//    flywheel.setDefaultCommand(
-//        new FlywheelCommand(flywheel, FlywheelCommand.Position.DEFAULT_SHOT, drivetrain));
+    //    flywheel.setDefaultCommand(
+    //        new FlywheelCommand(flywheel, FlywheelCommand.Position.DEFAULT_SHOT, drivetrain));
 
     /* Controls */
     // Y = Shoot Toggle
@@ -66,10 +69,13 @@ private void configureBindings() {
     // Right Stick Down = Extend/Retract Hopper
     controller
         .rightStick()
-        .and(() -> !shootGroup.isScheduled())
+        // .and(() -> !shootGroup.isScheduled())
         .onTrue(
-            new HopperCommand(hopper, HopperCommand.Position.EXTEND_RETRACT)
-                .alongWith(Commands.runOnce(() -> System.out.println("Hopper Pressed"))));
+            Commands.runEnd(
+                    () -> hopper.move(Hopper.getRotation(isExtended)), () -> hopper.stop(), hopper)
+                .withDeadline(Commands.waitSeconds(2))
+                .andThen(Commands.runOnce(() -> isExtended = !isExtended)));
+
     //    // Right Trigger = Climb Retract
     //    controller
     //        .rightTrigger()

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

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
 import frc.robot.config.HopperConfig;
 import frc.robot.subsystems.Hopper;
 
@@ -15,11 +16,12 @@ public static enum Position {
 
   private Hopper subsystem;
   private HopperCommand.Position pose;
-  private static boolean isExtended = false;
+  private boolean isExtended;
 
-  public HopperCommand(Hopper subsystem, HopperCommand.Position pose) {
+  public HopperCommand(Hopper subsystem, HopperCommand.Position pose, boolean isExtended) {
     this.pose = pose;
     this.subsystem = subsystem;
+    this.isExtended = isExtended;
 
     addRequirements(subsystem);
   }
@@ -31,13 +33,15 @@ public void initialize() {
       // (Needs check) If hopper already extended, retract, and if hopper retracted, extend
       case EXTEND_RETRACT:
         if (isExtended) {
-          //          System.out.println("Hopper: Retracting");
+          System.out.println("Hopper: Retracting");
           subsystem.move(HopperConfig.HOPPER_RETRACT_ROTATION);
-          isExtended = false;
+          RobotContainer.isExtended = false;
+          System.out.println(RobotContainer.isExtended);
         } else {
-          //          System.out.println("Hopper: Extending");
+          System.out.println("Hopper: Extending");
           subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION);
-          isExtended = true;
+          RobotContainer.isExtended = true;
+          System.out.println(RobotContainer.isExtended);
         }
         break;
 

src/main/java/frc/robot/config/HopperConfig.java

@@ -8,7 +8,7 @@ public class HopperConfig {
   // Set
   public static final double HOPPER_MAX_CRUISE_VELOCITY = 3000;
   public static final double HOPPER_TARGET_ACCELERATION = 500;
-  public static final double HOPPER_P = 2;
+  public static final double HOPPER_P = 1.5;
   public static final double HOPPER_I = 0;
   public static final double HOPPER_D = 0;
   public static final double HOPPER_S = 0;
@@ -22,6 +22,6 @@ public class HopperConfig {
   public static final double HOPPER_TOLERANCE = 0.2;
 
   // Set
-  public static final double HOPPER_EXTEND_ROTATION = 3.65;
-  public static final double HOPPER_RETRACT_ROTATION = 0.0;
+  public static final double HOPPER_EXTEND_ROTATION = 4.1;
+  public static final double HOPPER_RETRACT_ROTATION = 0.1;
 }

src/main/java/frc/robot/subsystems/Hopper.java

@@ -33,6 +33,9 @@ public Hopper() {
     hopperConfig.Slot0.kP = HopperConfig.HOPPER_P;
     hopperConfig.Slot0.kI = HopperConfig.HOPPER_I;
     hopperConfig.Slot0.kD = HopperConfig.HOPPER_D;
+    hopperConfig.Slot0.kS = HopperConfig.HOPPER_S;
+    hopperConfig.Slot0.kV = HopperConfig.HOPPER_V;
+    hopperConfig.Slot0.kA = HopperConfig.HOPPER_A;
 
     hopperConfig.Slot1.kP = HopperConfig.SLOW_HOPPER_P;
     hopperConfig.Slot1.kI = HopperConfig.SLOW_HOPPER_I;
@@ -56,12 +59,12 @@ public void slowMove(double rotation) {
   }
 
   public void extendDirectional(double speed) {
-    speed = Math.abs(-speed);
+    speed = Math.abs(speed);
     hopper.setControl(new DutyCycleOut(speed));
   }
 
   public void retractDirectional(double speed) {
-    speed = Math.abs(speed);
+    speed = Math.abs(-speed);
     hopper.setControl(new DutyCycleOut(speed));
   }
 
@@ -78,7 +81,7 @@ public boolean atPosition() {
         <= HopperConfig.HOPPER_TOLERANCE);
   }
 
-  public boolean extended() {
+  public boolean isExtended1() {
     return (Math.abs(hopper.getPosition().getValueAsDouble() - HopperConfig.HOPPER_EXTEND_ROTATION)
         <= HopperConfig.HOPPER_TOLERANCE);
   }
@@ -87,4 +90,28 @@ public boolean retracted() {
     return (Math.abs(hopper.getPosition().getValueAsDouble() - HopperConfig.HOPPER_RETRACT_ROTATION)
         <= HopperConfig.HOPPER_TOLERANCE);
   }
+
+  private boolean extended = false;
+
+  public boolean isExtended() {
+    return this.extended;
+  }
+
+  public static double getRotation(boolean isExtended) {
+    if (isExtended) {
+      return HopperConfig.HOPPER_RETRACT_ROTATION;
+    } else {
+      return HopperConfig.HOPPER_EXTEND_ROTATION;
+    }
+  }
+
+  public void toggleExtend() {
+    if (extended) {
+      move(HopperConfig.HOPPER_RETRACT_ROTATION);
+      extended = false;
+    } else {
+      move(HopperConfig.HOPPER_EXTEND_ROTATION);
+      extended = true;
+    }
+  }
 }