Nr rx/vision pipline switching

.vscode/settings.json

@@ -0,0 +1,4 @@
+{
+    "java.configuration.updateBuildConfiguration": "interactive",
+    "java.format.settings.url": "eclipse-formatter.xml"
+}

src/main/java/com/stuypulse/robot/Robot.java

@@ -82,7 +82,6 @@ public void autonomousInit() {
 
 
         robot.arm.setCoast(false, false);
-        robot.arm.setLimp(true, true);
         robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton?
         robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION);
         robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
@@ -113,7 +112,6 @@ public void teleopInit() {
         SmartDashboard.putString("Match State", state.name());
 
         robot.arm.setCoast(false, false);
-        robot.arm.setLimp(false, false);
         robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION);
         robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
         robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -213,10 +213,6 @@ private void configureOperatorBindings() {
         operator.getBottomButton()
             .onTrue(new ManagerSetGamePiece(GamePiece.CONE_TIP_OUT));
 
-        operator.getRightBumper()
-            .onTrue(arm.runOnce(arm::enableLimp))
-            .onFalse(arm.runOnce(arm::disableLimp));
-
         // arm to neutral
         operator.getDPadRight().onTrue(new IntakeAcquire())
             .onFalse(new IntakeStop());

src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java

@@ -86,8 +86,6 @@ public void execute() {
         var targetState = trajectory.getStates().get(currentIndex);
         arm.setTargetState(targetState);
 
-        arm.setLimp(targetState.isWristLimp(), false);
-
         double currentShoulderTolerance = (targetState.getShoulderTolerance().orElse(shoulderTolerance)).doubleValue();
         double currentWristTolerance = (targetState.getWristTolerance().orElse(wristTolerance)).doubleValue();
 

src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java

@@ -11,13 +11,12 @@
 import com.stuypulse.stuylib.control.feedback.PIDController;
 import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
 import com.stuypulse.stuylib.math.Angle;
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile;
-import com.stuypulse.stuylib.streams.booleans.BStream;
-import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
 import com.stuypulse.stuylib.streams.filters.MotionProfile;
 
+import java.util.Optional;
+
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.Arm.Shoulder;
 import com.stuypulse.robot.constants.Settings.Arm.Wrist;
@@ -38,17 +37,15 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-import java.util.Optional;
-
-
 /**
-  * Double jointed arm controlled by two motion profiled PID controllers.
-  *
-  * Available control "modes":
-  * - setpoint control (PID+FF controllers are used) (shoulder is not allowed above maximum shoulder angle)
-  * - limp mode (controller output is overriden to be zero)
-  * - voltage override ("force" feeds a voltage to the motor)
-  */
+ * Double jointed arm controlled by two motion profiled PID controllers.
+ *
+ * Available control "modes":
+ * - setpoint control (PID+FF controllers are used) (shoulder is not allowed
+ * above maximum shoulder angle)
+ * - limp mode (controller output is overriden to be zero)
+ * - voltage override ("force" feeds a voltage to the motor)
+ */
 public abstract class Arm extends SubsystemBase {
 
     // Singleton
@@ -71,23 +68,17 @@ public static Arm getInstance() {
     private final SmartNumber shoulderTargetDegrees;
     private final SmartNumber wristTargetDegrees;
 
+    // Voltage overrides (used when present)
+    private Optional<Double> wristVoltageOverride;
+    private Optional<Double> shoulderVoltageOverride;
+
     // controllers for each joint
     private final Controller shoulderController;
     private final AngleController wristController;
 
     // Mechanism2d visualizer
     private final ArmVisualizer armVisualizer;
 
-    // Limp mode (forces a joint to receive zero voltage)
-    private SmartBoolean wristLimp;
-    private SmartBoolean shoulderLimp;
-
-    // Voltage overrides (used when present)
-    private Optional<Double> wristVoltageOverride;
-    private Optional<Double> shoulderVoltageOverride;
-
-    private BStream wristEnabled;
-
     private SmartNumber shoulderVelocityFeedbackDebounce;
     private SmartNumber shoulderVelocityFeedbackCutoff;
 
@@ -122,78 +113,67 @@ public double doubleValue() {
 
         @Override
         public float floatValue() {
-            return (float)doubleValue();
+            return (float) doubleValue();
         }
 
         @Override
         public int intValue() {
-            return (int)doubleValue();
+            return (int) doubleValue();
         }
 
         @Override
         public long longValue() {
-            return (long)doubleValue();
+            return (long) doubleValue();
         }
     }
 
     protected Arm() {
+        // These are the setpoints for the joints relative to the "horizontal" (like the
+        // unit circle) -- keep both
         shoulderTargetDegrees = new SmartNumber("Arm/Shoulder/Target Angle (deg)", -90);
         wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +90);
 
+        // These numbers are used for disabling/enabling wrist control while the
+        // shoulder is moving -- they are no longer necessary. Remove them
         shoulderVelocityFeedbackDebounce = new SmartNumber(
-            "Arm/Wrist/Feedback Enabled Debounce",
-            Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());
+                "Arm/Wrist/Feedback Enabled Debounce",
+                Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());
 
         shoulderVelocityFeedbackCutoff = new SmartNumber(
-            "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)",
-            Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
-
-        wristEnabled = BStream.create(this::isWristFeedbackEnabled)
-            .filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce));
+                "Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)",
+                Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
 
         shoulderMaxVelocity = new SmartNumber(
-            "Arm/Shoulder/Max Velocity",
-            Shoulder.TELEOP_MAX_VELOCITY.doubleValue());
+                "Arm/Shoulder/Max Velocity",
+                Shoulder.TELEOP_MAX_VELOCITY.doubleValue());
         shoulderMaxAcceleration = new SmartNumber(
-            "Arm/Shoulder/Max Acceleration",
-            Shoulder.TELEOP_MAX_ACCELERATION.doubleValue());
+                "Arm/Shoulder/Max Acceleration",
+                Shoulder.TELEOP_MAX_ACCELERATION.doubleValue());
 
         wristMaxVelocity = new SmartNumber(
-            "Arm/Wrist/Max Velocity",
-            Wrist.TELEOP_MAX_VELOCITY.doubleValue());
+                "Arm/Wrist/Max Velocity",
+                Wrist.TELEOP_MAX_VELOCITY.doubleValue());
         wristMaxAcceleration = new SmartNumber(
-            "Arm/Wrist/Max Acceleration",
-            Wrist.TELEOP_MAX_ACCELERATION.doubleValue());
-
-        shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, Shoulder.Feedforward.kA).position()
-            .add(new ArmEncoderFeedforward(new GamePiecekG()))
-            .add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs))
-            .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD))
-            .setSetpointFilter(
-                new MotionProfile(
-                    shoulderMaxVelocity.filtered(Math::toRadians).number(),
-                    shoulderMaxAcceleration.filtered(Math::toRadians).number()))
-            .setOutputFilter(x -> {
-                if (isShoulderLimp()) return 0;
-                return shoulderVoltageOverride.orElse(x);
-            })
-            ;
+                "Arm/Wrist/Max Acceleration",
+                Wrist.TELEOP_MAX_ACCELERATION.doubleValue());
+
+        shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV,
+                Shoulder.Feedforward.kA).position()
+                .add(new ArmEncoderFeedforward(new GamePiecekG()))
+                .add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs))
+                .add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD))
+                .setSetpointFilter(
+                        new MotionProfile(
+                                shoulderMaxVelocity.filtered(Math::toRadians).number(),
+                                shoulderMaxAcceleration.filtered(Math::toRadians).number()));
 
         wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle()
-            .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG))
-            .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)
-                .setOutputFilter(x -> wristEnabled.get() ? x : 0))
-            .setSetpointFilter(
-                new AMotionProfile(
-                    wristMaxVelocity.filtered(Math::toRadians).number(),
-                    wristMaxAcceleration.filtered(Math::toRadians).number()))
-            .setOutputFilter(x -> {
-                if (isWristLimp()) return 0;
-                return wristVoltageOverride.orElse(x);
-            });
-
-        wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false);
-        shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false);
+                .add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG))
+                .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)
+                        .setSetpointFilter(
+                                new AMotionProfile(
+                                        wristMaxVelocity.filtered(Math::toRadians).number(),
+                                        wristMaxAcceleration.filtered(Math::toRadians).number())));
 
         wristVoltageOverride = Optional.empty();
         shoulderVoltageOverride = Optional.empty();
@@ -214,17 +194,9 @@ public void disableGamePieceGravityCompensation() {
     }
 
     // Arm Control Overrides
-
-    private final boolean isWristLimp() {
-        return wristLimp.get();
-    }
-
-    private final boolean isShoulderLimp() {
-        return shoulderLimp.get();
-    }
-
     private final boolean isWristFeedbackEnabled() {
-        return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units.degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue());
+        return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units
+                .degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue());
     }
 
     // Set kinematic constraints
@@ -239,7 +211,6 @@ public final void setWristConstraints(Number velocity, Number acceleration) {
         wristMaxAcceleration.set(acceleration);
     }
 
-
     // Read target State
     public final Rotation2d getShoulderTargetAngle() {
         return Rotation2d.fromDegrees(shoulderTargetDegrees.get());
@@ -305,6 +276,7 @@ public final boolean isAtTargetState(double shoulderEpsilonDegrees, double wrist
 
     // Read angle measurements
     public abstract Rotation2d getShoulderAngle();
+
     protected abstract Rotation2d getRelativeWristAngle();
 
     public final Rotation2d getWristAngle() {
@@ -316,6 +288,7 @@ public final ArmState getState() {
     }
 
     public abstract double getShoulderVelocityRadiansPerSecond();
+
     public abstract double getWristVelocityRadiansPerSecond();
 
     // Set a voltage override
@@ -329,34 +302,23 @@ public void setWristVoltage(double voltage) {
 
     // Feed a voltage to the hardware layer
     protected abstract void setShoulderVoltageImpl(double voltage);
+
     protected abstract void setWristVoltageImpl(double voltage);
 
     // set coast / brake mode
     public void setCoast(boolean wristCoast, boolean shoulderCoast) {}
 
-    // set if the ligaments are "limp" (zero voltage)
-    public final void setLimp(boolean wristLimp, boolean shoulderLimp) {
-        this.wristLimp.set(wristLimp);
-        this.shoulderLimp.set(shoulderLimp);
-    }
-
-    public final void enableLimp() {
-        setLimp(true, true);
-    }
-    public final void disableLimp() {
-        setLimp(false, false);
-    }
-
     // Arm Visualizer
     public final ArmVisualizer getVisualizer() {
         return armVisualizer;
     }
+
 
     @Override
     public final void periodic() {
         // Validate shoulder and wrist target states
         Rotation2d shoulderTarget = getShoulderTargetAngle();
-        Rotation2d wristTarget = getWristTargetAngle();
+        // Rotation2d wristTarget = getWristTargetAngle();
 
         double normalizedDeg = shoulderTarget.minus(Rotation2d.fromDegrees(-90)).getDegrees();
 
@@ -366,46 +328,50 @@ public final void periodic() {
             setShoulderTargetAngle(Rotation2d.fromDegrees(180 - Shoulder.MAX_SHOULDER_ANGLE.get()));
         }
 
-
         // Run control loops on validated target angles
         shoulderController.update(
-            getWrappedShoulderAngle(getShoulderTargetAngle()),
-            getWrappedShoulderAngle(getShoulderAngle()));
+                getWrappedShoulderAngle(getShoulderTargetAngle()),
+                getWrappedShoulderAngle(getShoulderAngle()));
 
         SmartDashboard.putNumber("Arm/Shoulder/Wrapped Angle", getWrappedShoulderAngle(getShoulderAngle()));
-        SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", getWrappedShoulderAngle(getShoulderTargetAngle()));
+        SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle",
+                getWrappedShoulderAngle(getShoulderTargetAngle()));
 
         wristController.update(
-            Angle.fromRotation2d(getWristTargetAngle()),
-            Angle.fromRotation2d(getWristAngle()));
+                Angle.fromRotation2d(getWristTargetAngle()),
+                Angle.fromRotation2d(getWristAngle()));
 
         setWristVoltageImpl(wristController.getOutput());
         setShoulderVoltageImpl(shoulderController.getOutput());
 
-        armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees());
+        armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()),
+                wristController.getSetpoint().toDegrees());
         armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees());
         armVisualizer.setFieldArm(Odometry.getInstance().getPose(), getState());
 
         SmartDashboard.putNumber("Arm/Shoulder/Angle (deg)", getShoulderAngle().getDegrees());
-        SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", Units.radiansToDegrees(shoulderController.getSetpoint()));
+        SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)",
+                Units.radiansToDegrees(shoulderController.getSetpoint()));
         SmartDashboard.putNumber("Arm/Shoulder/Error (deg)", Units.radiansToDegrees(shoulderController.getError()));
         SmartDashboard.putNumber("Arm/Shoulder/Output (V)", shoulderController.getOutput());
-        SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond()));
+        SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)",
+                Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond()));
 
         SmartDashboard.putNumber("Arm/Wrist/Angle (deg)", getWristAngle().getDegrees());
         SmartDashboard.putNumber("Arm/Wrist/Relative Angle (deg)", getRelativeWristAngle().getDegrees());
         SmartDashboard.putNumber("Arm/Wrist/Setpoint (deg)", wristController.getSetpoint().toDegrees());
         SmartDashboard.putNumber("Arm/Wrist/Error (deg)", wristController.getError().toDegrees());
         SmartDashboard.putNumber("Arm/Wrist/Output (V)", wristController.getOutput());
-        SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", Units.radiansToDegrees(getWristVelocityRadiansPerSecond()));
+        SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)",
+                Units.radiansToDegrees(getWristVelocityRadiansPerSecond()));
         SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled Raw", isWristFeedbackEnabled());
-        SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled", wristEnabled.get());
         SmartDashboard.putNumber("Arm/Shoulder/kG", new GamePiecekG().doubleValue());
 
         SmartDashboard.putBoolean("Arm/Shoulder/Game Piece Compensation", pieceGravityCompensation);
 
         periodicallyCalled();
     }
 
-    public void periodicallyCalled() {}
+    public void periodicallyCalled() {
+    }
 }