Stator limits, error fixes

Iron-Panthers · Sep 21, 2023 · b53df54 · b53df54
1 parent 71931a9
commit b53df54
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 23 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -17,17 +17,15 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.Constants.Drive.Dims;
-import frc.util.NodeSelectorUtility;
-import frc.util.NodeSelectorUtility.Height;
-import frc.util.NodeSelectorUtility.NodeSelection;
-import frc.util.NodeSelectorUtility.NodeType;
 import frc.robot.commands.ScoreCommand.ScoreStep;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
 import frc.robot.subsystems.NetworkWatchdogSubsystem.IPv4;
 import frc.robot.subsystems.RGBSubsystem.RGBColor;
 import frc.robot.subsystems.VisionSubsystem.TagCountDeviation;
 import frc.robot.subsystems.VisionSubsystem.UnitDeviationParams;
 import frc.util.CAN;
+import frc.util.NodeSelectorUtility.Height;
+import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.pathing.FieldObstructionMap;
 import java.nio.file.Path;
 import java.util.List;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -22,7 +22,6 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants;
 import frc.robot.Constants.Config;
 import frc.robot.Constants.Drive;
 import frc.robot.commands.AlignGamepieceCommand;
@@ -408,7 +407,7 @@ private void setupAutonomousCommands() {
 
     final List<ScoreStep> drivingCubeOuttake =
         List.of(
-            new ScoreStep(ElevatorState(35, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
+            new ScoreStep(ElevatorState(35.0, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
             new ScoreStep(IntakeMode.OUTTAKE));
     final boolean[] intakeLow = {false};
     final Map<String, Command> eventMap =

diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java
@@ -14,21 +14,18 @@
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class GroundPickupCommand extends SequentialCommandGroup {
   /** Creates a new GroundPickupCommand. */
-  public GroundPickupCommand(
-    ElevatorSubsystem elevatorSubsystem,
-    IntakeSubsystem intakeSubsystem
-  ) {
+  public GroundPickupCommand(ElevatorSubsystem elevatorSubsystem, IntakeSubsystem intakeSubsystem) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-    new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE)
-      .deadlineWith(
         new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE)
-        .alongWith(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.HANDOFF)))
-      .andThen(
-        new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED)
-        .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD))));
-        //hold???
-
+            .deadlineWith(
+                new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.HANDOFF))
+            .andThen(
+                new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED)
+                    .alongWith(
+                        new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD))));
+    // hold???
+
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -28,6 +28,7 @@ public class ElevatorSubsystem extends SubsystemBase {
   private double targetHeight;
   private double currentAngle;
   private double targetAngle;
+  private double statorCurrentLimit;
 
   private PIDController heightController;
   private PIDController wristController;
@@ -59,6 +60,7 @@ public ElevatorSubsystem() {
     targetHeight = 0.0;
     currentAngle = 0.0;
     targetAngle = 0.0;
+    statorCurrentLimit = 10.0;
 
     rightMotor.configFactoryDefault();
     leftMotor.configFactoryDefault();
@@ -72,7 +74,7 @@ public ElevatorSubsystem() {
     leftMotor.setNeutralMode(NeutralMode.Brake);
     wristMotor.setNeutralMode(NeutralMode.Brake);
 
-    filter = LinearFilter.movingAverage(35);
+    filter = LinearFilter.movingAverage(30);
 
     tab.addDouble("Motor Position", () -> canCoder.getAbsolutePosition());
   }
@@ -117,11 +119,16 @@ public void setTargetAngle(double targetAngle) {
   public void periodic() {
     currentHeight = getHeight();
     currentAngle = getCurrentAngleDegrees();
-    double motorPower = heightController.calculate(currentHeight, targetHeight);
-    rightMotor.set(TalonFXControlMode.PercentOutput, motorPower);
+
+    if (filter.calculate(rightMotor.getStatorCurrent()) < statorCurrentLimit) {
+      double motorPower = heightController.calculate(currentHeight, targetHeight);
+      rightMotor.set(TalonFXControlMode.PercentOutput, motorPower);
+    } else {
+      rightMotor.set(TalonFXControlMode.PercentOutput, 0);
+    }
+
     wristMotor.set(
         TalonFXControlMode.PercentOutput,
-        MathUtil.clamp(
-            wristController.calculate(currentAngle, targetAngle), -0.25, 0.25));
+        MathUtil.clamp(wristController.calculate(currentAngle, targetAngle), -0.25, 0.25));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -42,6 +42,7 @@ public void intakePeriodic(IntakeMode mode) {
         intakeMotor.set(TalonFXControlMode.PercentOutput, -1);
       case HOLD:
       case OFF:
+        intakeMotor.set(TalonFXControlMode.PercentOutput, 0);
       default:
         intakeMotor.set(TalonFXControlMode.PercentOutput, 0);
     }