more consistent auto stow, faster wrist

Iron-Panthers · Nov 12, 2023 · 8a5e9f5 · 8a5e9f5
1 parent f10a25e
commit 8a5e9f5
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -256,7 +256,8 @@ public static final class Intake {
     public static final double HOLD_CUBE_PERCENT = -0.2;
 
     public static final double CONE_STATOR_LIMIT = 75;
-    public static final double CUBE_STATOR_LIMIT = 65;
+    public static final double CUBE_STATOR_LIMIT = 69;
+    public static final double GROUND_CUBE_STATOR_LIMIT = 60;
 
     public static final class Ports {
       public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor");

diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java
@@ -23,7 +23,7 @@ public GroundPickupCommand(
     // Add your commands in the addCommands() call, e.g.
 
     addCommands(
-        new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCone)
+        new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCone, true)
             .deadlineWith(new GroundIntakeElevatorCommand(elevatorSubsystem, isCone))
             .andThen(
                 new ElevatorPositionCommand(elevatorSubsystem, () -> Elevator.Setpoints.STOWED)));

diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java
@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants.Intake;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.Modes;
 import java.util.function.BooleanSupplier;
@@ -13,16 +14,22 @@ public class IntakeModeCommand extends CommandBase {
   private IntakeSubsystem intakeSubsystem;
   private Modes mode;
   private BooleanSupplier isCone;
+  private boolean isGroundIntake;
 
   /** Creates a new IntakeCommand. */
-  public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone) {
+  public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone, boolean isGroundIntake) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.intakeSubsystem = intakeSubsystem;
     this.mode = mode;
     this.isCone = isCone;
+    this.isGroundIntake = isGroundIntake;
     addRequirements(intakeSubsystem);
   }
 
+  public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone) {
+    this(intakeSubsystem, mode, isCone, false);
+  }
+
   public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.intakeSubsystem = intakeSubsystem;
@@ -53,8 +60,14 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (intakeSubsystem.getMode() == Modes.HOLD || intakeSubsystem.getMode() == Modes.OFF) {
-      return true;
+    if(!isGroundIntake) {
+      if (intakeSubsystem.getMode() == Modes.HOLD || intakeSubsystem.getMode() == Modes.OFF) {
+        return true;
+      }
+    } else {
+      if(intakeSubsystem.getFilterOutput() > Intake.GROUND_CUBE_STATOR_LIMIT) {
+        return true;
+      }
     }
     return false;
   }

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -292,7 +292,7 @@ private void positionDrivePeriodic() {
     } else {
       wristMotor.set(
           TalonFXControlMode.PercentOutput,
-          MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.5, 0.5));
+          MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -1, 1));
     }
   }
 

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -112,6 +112,10 @@ public void setIsCone(boolean isCone) {
     this.isCone = isCone;
   }
 
+  public double getFilterOutput() {
+    return filterOutput;
+  }
+
   private double getFilterCalculatedValue() {
     return filter.calculate(intakeMotor.getStatorCurrent());
   }