Added armSafe to not let the elevator lift with arm inside the robot

chopshop-166 · Feb 20, 2025 · f3fb67a · f3fb67a
1 parent cf62c06
commit f3fb67a
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java
@@ -9,20 +9,26 @@
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.maps.subsystems.ArmRotateMap;
 import frc.robot.maps.subsystems.ArmRotateMap.ArmRotatePresets;
 import frc.robot.maps.subsystems.ArmRotateMap.Data;
 
 public class ArmRotate extends LoggedSubsystem<Data, ArmRotateMap> {
     final ProfiledPIDController pid;
-    private final double RAISE_SPEED = 0.5;
-    private final double LOWER_SPEED = 0.3;
+    private final double RAISE_SPEED_COEF = 0.5;
+    private final double LOWER_SPEED_COEF = 0.3;
     private final double MANUAL_LOWER_SPEED_COEF = 0.3;
     private final double SLOW_DOWN_COEF = 0.3;
     private final double ZEROING_SPEED = 0.5;
+    private final double SAFE_ANGLE = 0;
     double holdAngle = 0;
 
+    NetworkTableInstance instance = NetworkTableInstance.getDefault();
+    BooleanPublisher armSafePub = instance.getBooleanTopic("Arm/Safe").publish();
+
     ArmRotatePresets preset = ArmRotatePresets.OFF;
 
     public ArmRotate(ArmRotateMap armRotateMap) {
@@ -34,7 +40,7 @@ public Command move(DoubleSupplier rotateSpeed) {
 
         return run(() -> {
             double speed = rotateSpeed.getAsDouble();
-            double speedCoef = RAISE_SPEED;
+            double speedCoef = RAISE_SPEED_COEF;
             if (speed < 0) {
                 speedCoef = MANUAL_LOWER_SPEED_COEF;
             }
@@ -95,6 +101,7 @@ private double getArmAngle() {
     @Override
     public void periodic() {
         super.periodic();
+        armSafePub.set(getData().rotationAbsAngleDegrees >= SAFE_ANGLE);
         if (preset != ArmRotatePresets.OFF) {
             double targetHeight = preset == ArmRotatePresets.HOLD ? holdAngle
                     : getMap().armRotatePreset.getValue(preset);

diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,6 +29,7 @@ public class Elevator extends LoggedSubsystem<Data, ElevatorMap> {
 
     NetworkTableInstance instance = NetworkTableInstance.getDefault();
     DoublePublisher heightPub = instance.getDoubleTopic("Elevator/Height").publish();
+    BooleanSubscriber armSafeSub = instance.getBooleanTopic("Arm/Safe").subscribe(false);
 
     ElevatorPresets level = ElevatorPresets.OFF;
 
@@ -41,13 +43,17 @@ public Command move(DoubleSupplier liftSpeed) {
         return run(() -> {
             double speed = liftSpeed.getAsDouble();
             double speedCoef = RAISE_SPEED;
-            if (speed < 0) {
-                speedCoef = MANUAL_LOWER_SPEED_COEF;
-            }
-            if (Math.abs(speed) > 0) {
-                level = ElevatorPresets.OFF;
-                getData().motor.setpoint = limits(speed * speedCoef);
-            } else if (level == ElevatorPresets.OFF) {
+            if (armSafeSub.getAsBoolean()) {
+                if (speed < 0) {
+                    speedCoef = MANUAL_LOWER_SPEED_COEF;
+                }
+                if (Math.abs(speed) > 0) {
+                    level = ElevatorPresets.OFF;
+                    getData().motor.setpoint = limits(speed * speedCoef);
+                } else if (level == ElevatorPresets.OFF) {
+                    getData().motor.setpoint = 0.0;
+                }
+            } else {
                 getData().motor.setpoint = 0.0;
             }
 
@@ -58,7 +64,11 @@ public Command zero() {
         return startSafe(() -> {
             getMap().motor.resetValidators();
             level = ElevatorPresets.OFF;
-            getData().motor.setpoint = ZEROING_SPEED;
+            if (armSafeSub.getAsBoolean()) {
+                getData().motor.setpoint = ZEROING_SPEED;
+            } else {
+                getData().motor.setpoint = 0.0;
+            }
         }).until(() -> getMap().motor.validate()).andThen(resetCmd());
     }
 
@@ -74,11 +84,15 @@ public Command moveToZero() {
     public Command moveTo(ElevatorPresets level) {
         PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal);
         return runOnce(() -> {
-            this.level = level;
-            pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
+                this.level = level;
+                pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
+
         }).andThen(run(() -> {
             Logger.recordOutput("PID at goal", pid.atGoal());
-        })).until(setPointPersistenceCheck).withName("Move to set height");
+        })).until(() -> {
+            return setPointPersistenceCheck.getAsBoolean() && armSafeSub.getAsBoolean();
+        }).withName("Move to set height");
+
     }
 
     public Command hold() {