differentiate cube and cone stator limit, haltdrivecommand bind

Iron-Panthers · Nov 11, 2023 · e609e7b · e609e7b
1 parent 8e417cd
commit e609e7b
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 17 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -207,7 +207,7 @@ public static final class Setpoints {
       public static final ElevatorState GROUND_INTAKE_CONE =
           new ElevatorState(MIN_EXTENSION_INCHES, 75);
       public static final ElevatorState GROUND_INTAKE_CUBE =
-          new ElevatorState(MIN_EXTENSION_INCHES, 81.4);
+          new ElevatorState(MIN_EXTENSION_INCHES, 85);
       public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5);
       public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5);
       public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5);
@@ -255,6 +255,9 @@ public static final class Intake {
     public static final double HOLD_CONE_PERCENT = 0.1;
     public static final double HOLD_CUBE_PERCENT = -0.1;
 
+    public static final double CONE_STATOR_LIMIT = 75;
+    public static final double CUBE_STATOR_LIMIT = 65;
+
     public static final class Ports {
       public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor");
       public static final int CONE_TOF_PORT = 0;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -219,6 +219,8 @@ private void configureButtonBindings() {
 
     anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
 
+    anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
+
     jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem));
 
     jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem));

diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java
@@ -4,6 +4,7 @@
 import frc.robot.Constants;
 import frc.robot.Constants.Elevator;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem.Modes;
 import frc.util.Util;
 
 import java.util.function.BooleanSupplier;
@@ -20,6 +21,11 @@ public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanS
     addRequirements(this.elevatorSubsystem);
   }
 
+  @Override
+  public void initialize() {
+    elevatorSubsystem.setMode(Modes.POSITION_CONTROL);
+  }
+
   @Override
   public void execute() {
     if (isCube.getAsBoolean()) {
@@ -28,16 +34,4 @@ public void execute() {
       elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE);
     }
   }
-
-  @Override
-  public boolean isFinished() {
-    return Util.epsilonEquals(
-              elevatorSubsystem.getCurrentAngleDegrees(),
-              elevatorSubsystem.getTargetAngle(),
-              Elevator.ANGLE_EPSILON)
-          && Util.epsilonEquals(
-              elevatorSubsystem.getCurrentExtensionInches(),
-              elevatorSubsystem.getTargetExtension(),
-              Elevator.EXTENSION_EPSILON);
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -20,7 +20,6 @@ public class IntakeSubsystem extends SubsystemBase {
   private Modes currentIntakeMode;
   private LinearFilter filter;
   private double filterOutput;
-  private double statorCurrentLimit;
   private boolean isCube;
   // private final TimeOfFlight coneToF, cubeToF;
 
@@ -40,8 +39,6 @@ public IntakeSubsystem() {
 
     filterOutput = 0.0;
 
-    statorCurrentLimit = 75;
-
     isCube = false;
 
     // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT);
@@ -122,7 +119,9 @@ private double getFilterCalculatedValue() {
   @Override
   public void periodic() {
     filterOutput = filter.calculate(intakeMotor.getStatorCurrent());
-    if (filterOutput >= statorCurrentLimit) {
+    if(isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) {
+      currentIntakeMode = Modes.HOLD;
+    } else if(filterOutput >= Intake.CUBE_STATOR_LIMIT) {
       currentIntakeMode = Modes.HOLD;
     }