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.resetMotionProfile();
         robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION);
@@ -114,7 +113,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

@@ -34,6 +34,7 @@
 import com.stuypulse.robot.subsystems.wing.*;
 import com.stuypulse.robot.util.*;
 import com.stuypulse.robot.util.BootlegXbox;
+import com.stuypulse.robot.util.Limelight.DataType;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.VideoMode.PixelFormat;
@@ -135,10 +136,13 @@ private void configureDriverBindings() {
         driver.getTopButton()
             .onTrue(new ManagerValidateState())
             .onTrue(new ManagerChooseScoreNode())
-            .whileTrue(new RobotAlignThenScoreCubes());
+            .whileTrue(Manager.getInstance().getGamePiece().isCube() ? new RobotAlignThenScoreCubes() : new RobotAlignThenScoreCones());
 
         // swerve
-        driver.getLeftButton().whileTrue(new SwerveDriveAlignThenBalance());
+        // driver.getLeftButton().whileTrue(new SwerveDriveAlignThenBalance());
+
+        driver.getLeftButton().onTrue(new InstantCommand( () -> Vision.getInstance().setPipeline(DataType.TAPE)));
+        driver.getRightButton().onTrue(new InstantCommand( () -> Vision.getInstance().setPipeline(DataType.APRIL_TAG)));
 
         // odometry
         driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180)));
@@ -147,7 +151,7 @@ private void configureDriverBindings() {
         driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90)));
 
         // plant
-        driver.getRightButton().onTrue(new PlantEngage());
+        // driver.getRightButton().onTrue(new PlantEngage());
         driver.getRightBumper().onTrue(new PlantDisengage());
 
         new Trigger(intake::hasGamePiece)
@@ -213,10 +217,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/RobotAlignThenScoreCones.java

@@ -0,0 +1,123 @@
+/************************ PROJECT JIM *************************/
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/
+/* This work is licensed under the terms of the MIT license.  */
+/**************************************************************/
+
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+import com.stuypulse.robot.subsystems.LEDController;
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
+import com.stuypulse.robot.constants.Settings.Alignment.Translation;
+import com.stuypulse.robot.subsystems.Manager;
+import com.stuypulse.robot.subsystems.intake.*;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.Vision;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.robot.util.LEDColor;
+import com.stuypulse.robot.util.Limelight.DataType;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class RobotAlignThenScoreCones extends CommandBase {
+
+    // Subsystems
+    private final SwerveDrive swerve;
+    private final Intake intake;
+
+    private final Vision vision;
+
+    // Holonomic control
+    private final HolonomicController controller;
+    private final BStream aligned;
+
+    // Logging
+    private final FieldObject2d targetPose2d;
+
+    public RobotAlignThenScoreCones(){
+        this.swerve = SwerveDrive.getInstance();
+        this.intake = Intake.getInstance();
+        this.vision = Vision.getInstance();
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P,Translation.I,Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D));
+
+        SmartDashboard.putData("Alignment/Controller", controller);
+
+        aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME));
+
+        targetPose2d = Odometry.getInstance().getField().getObject("Target Pose");
+        addRequirements(swerve, intake);
+    }
+
+    private boolean isAligned() {
+        if (Manager.getInstance().getGamePiece().isCone()) {
+            return controller.isDone(
+                Alignment.ALIGNED_CONE_THRESHOLD_X.get(),
+                Alignment.ALIGNED_CONE_THRESHOLD_Y.get(),
+                Alignment.ALIGNED_CONE_THRESHOLD_ANGLE.get());
+        } else {
+            return controller.isDone(
+                Alignment.ALIGNED_CUBE_THRESHOLD_X.get(),
+                Alignment.ALIGNED_CUBE_THRESHOLD_Y.get(),
+                Alignment.ALIGNED_CUBE_THRESHOLD_ANGLE.get());
+        }
+    }
+
+    @Override
+    public void initialize() {
+        intake.enableBreak();
+        Odometry.USE_VISION_ANGLE.set(true);
+
+        vision.setPipeline(DataType.TAPE);
+
+        LEDController.getInstance().setColor(LEDColor.BLUE, 694000000);
+    }
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = Odometry.getInstance().getPose();
+        Pose2d targetPose = new Pose2d(
+            currentPose.getTranslation().getX() + vision.getDistance() * Math.cos(vision.getAngle()),
+            currentPose.getTranslation().getY() + vision.getDistance() * Math.sin(vision.getAngle()),
+            Rotation2d.fromDegrees(0)
+        );
+        targetPose2d.setPose(targetPose);
+
+        controller.update(targetPose, currentPose);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return isAligned();
+    }
+
+    public CommandBase scoreCone() {
+        return this.andThen(new RobotScore());
+    }
+
+    public void end(boolean interupted) {
+        intake.enableBreak();
+        Odometry.USE_VISION_ANGLE.set(false);
+        swerve.stop();
+        intake.stop();
+        targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
+
+        vision.setPipeline(DataType.APRIL_TAG);
+
+        LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0);
+    }
+
+}

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/commands/auton/BCThreePieceWLowWire.java

@@ -152,7 +152,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
     public BCThreePieceWLowWire() {
 
         var paths = SwerveDriveFollowTrajectory.getSeparatedPaths(
-            PathPlanner.loadPathGroup("3 Piece W Low - Blue - Bumpside", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS),
+            PathPlanner.loadPathGroup("BC 3 Piece W Low Bump", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS),
             "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away"
         );
 

src/main/java/com/stuypulse/robot/commands/auton/BCTwoPieceDockWire.java

@@ -101,7 +101,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
 
     public BCTwoPieceDockWire() {
         var paths = SwerveDriveFollowTrajectory.getSeparatedPaths(
-            PathPlanner.loadPathGroup("2 Piece Dock Bumpside", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS),
+            PathPlanner.loadPathGroup("BC 2 Piece Dock Bump", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS),
             "Intake Piece", "Score Piece", "Dock"
         );
 

src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java

@@ -177,7 +177,7 @@ public TwoPieceDock() {
 
             new LEDSet(LEDColor.RED),
 
-            arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(10.0)),
+            arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(15.0)),
 
             new ParallelCommandGroup(
                 new SwerveDriveFollowTrajectory(

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java

@@ -41,7 +41,7 @@ public SwerveDriveToPose(Supplier<Pose2d> targetPoses){
         this.targetPoses = targetPoses;
 
         controller = new HolonomicController(
-            new PIDController(Translation.P,Translation.I,Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
             new PIDController(Translation.P, Translation.I, Translation.D),
             new AnglePIDController(Rotation.P, Rotation.I, Rotation.D));
 

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -25,6 +25,8 @@ public interface Field {
     double WIDTH = 16.54;
     double HEIGHT = 8.02;
 
+    double MID_PEG_HEIGHT = Units.inchesToMeters(34);
+    double HIGH_PEG_HEIGHT = Units.inchesToMeters(46);
     // intake offset from center to the right
     double INTAKE_OFFSET_RIGHT = Units.inchesToMeters(1.625);
 

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -14,8 +14,8 @@ public interface Gamepad {
     }
 
     public interface Intake {
-        int FRONT_MOTOR_PORT = 30;
-        int BACK_MOTOR_PORT = 31;
+        int FRONT_MOTOR_PORT = 31;
+        int BACK_MOTOR_PORT = 30;
         int FRONT_SENSOR = 0;
         int BACK_SENSOR = 1;
     }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -74,12 +74,15 @@ public interface Vision {
         double MAX_USE_ANGLE = 50;
 
         public interface Limelight {
+            String FIRST_LIMELIGHT = "limelight-back";
+
             String [] LIMELIGHTS = {
                 "limelight-back",
                 "limelight-front"
             };
             int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805};
             Pose3d [] POSITIONS = new Pose3d[] {
+                // new Pose3d(new Translation3d(0,0,28.5), new Rotation3d(0, 0, 0))
                 new Pose3d(new Translation3d(0.1, 0, 1.29032), new Rotation3d(0, Math.toRadians(-30), Math.PI)),
                 new Pose3d(new Translation3d(0.1, 0, 1.29032), new Rotation3d(0, Math.toRadians(-30), 0))
             };
@@ -237,7 +240,7 @@ public interface Wrist {
                     MOI,
                     RADIUS);
 
-            Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482).plus(Rotation2d.fromDegrees(120));
+            Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482).plus(Rotation2d.fromDegrees(0));
 
             SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0);
             SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0);

src/main/java/com/stuypulse/robot/subsystems/Manager.java

@@ -15,7 +15,9 @@
 import com.stuypulse.robot.constants.Field.ScoreXPoses;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.vision.Vision;
 import com.stuypulse.robot.util.ArmState;
+import com.stuypulse.robot.util.Limelight;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -310,6 +312,7 @@ public void setGridNode(int gridNode) {
 
     @Override
     public void periodic() {
+
         Arm.getInstance().getVisualizer().setIntakingPiece(gamePiece);
 
         SmartDashboard.putString("Manager/Game Piece", gamePiece.name());

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

@@ -416,4 +416,4 @@ public final void periodic() {
     }
 
     public void periodicallyCalled() {}
-}
+}

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

@@ -119,20 +119,23 @@ public double getWristVelocityRadiansPerSecond() {
     }
 
     // private boolean isShoulderStalling() {
-    //     double appliedShoulderVoltage =
-    //         Math.max(
-    //             shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(),
-    //             shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(),
-    //         );
-
-    //     return shoulderEncoder.getVelocity() < Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts > Shoulder.STALLING_VOLTAGE.doubleValue() ||
-    //             wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue();
+    // double appliedShoulderVoltage =
+    // Math.max(
+    // shoulderRight.getAppliedOutput() * shoulderRight.getBusVoltage(),
+    // shoulderLeft.getAppliedOutput() * shoulderLeft.getBusVoltage(),
+    // );
+
+    // return shoulderEncoder.getVelocity() <
+    // Shoulder.STALLING_VELOCITY.doubleValue() && shoulderVolts >
+    // Shoulder.STALLING_VOLTAGE.doubleValue() ||
+    // wrist.getOutputCurrent() > Shoulder.STALLING_CURRENT.doubleValue();
     // }
 
     // private boolean isWristStalling() {
-    //     return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() && wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() ||
-    //             shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() ||
-    //             shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue();
+    // return wristEncoder.getVelocity() < Wrist.STALLING_VELOCITY.doubleValue() &&
+    // wristVolts > Wrist.STALLING_VOLTAGE.doubleValue() ||
+    // shoulderLeft.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue() ||
+    // shoulderRight.getOutputCurrent() > Wrist.STALLING_CURRENT.doubleValue();
     // }
 
     @Override
@@ -146,11 +149,11 @@ public void periodicallyCalled() {
         SmartDashboard.putNumber("Arm/Wrist/Current (amps)", wrist.getOutputCurrent());
 
         // if (wristIsStalling()) {
-        //     setWristVoltageImpl(WRIST);
+        // setWristVoltageImpl(WRIST);
         // }
 
         // if (armIsStalling()) {
-        //     shoulderVolts = 0;
+        // shoulderVolts = 0;
         // }
 
         // runShoulder(shoulderVolts);

src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java

@@ -82,15 +82,15 @@ public void acquire() {
         acquiring = true;
         switch (Manager.getInstance().getGamePiece()) {
             case CUBE:
-                frontMotor.set(Acquire.CUBE_FRONT.doubleValue());
-                backMotor.set(Acquire.CUBE_BACK.doubleValue());
+                frontMotor.set(-Acquire.CUBE_FRONT.doubleValue());
+                backMotor.set(-Acquire.CUBE_BACK.doubleValue());
                 break;
             case CONE_TIP_UP:
                 break;
             case CONE_TIP_OUT:
             case CONE_TIP_IN:
-                frontMotor.set(Acquire.CONE_FRONT.doubleValue());
-                backMotor.set(-Acquire.CONE_BACK.doubleValue());
+                frontMotor.set(-Acquire.CONE_FRONT.doubleValue());
+                backMotor.set(Acquire.CONE_BACK.doubleValue());
                 break;
             default:
                 break;
@@ -102,17 +102,17 @@ public void deacquire() {
         acquiring = false;
         switch (Manager.getInstance().getGamePiece()) {
             case CUBE:
-                frontMotor.set(-Deacquire.CUBE_FRONT.doubleValue());
-                backMotor.set(-Deacquire.CUBE_BACK.doubleValue());
+                frontMotor.set(+Deacquire.CUBE_FRONT.doubleValue());
+                backMotor.set(+Deacquire.CUBE_BACK.doubleValue());
                 break;
             case CONE_TIP_UP:
-                frontMotor.set(+Deacquire.CONE_UP_FRONT.doubleValue());
-                backMotor.set(-Deacquire.CONE_UP_BACK.doubleValue());
+                frontMotor.set(-Deacquire.CONE_UP_FRONT.doubleValue());
+                backMotor.set(+Deacquire.CONE_UP_BACK.doubleValue());
                 break;
             case CONE_TIP_OUT:
             case CONE_TIP_IN:
-                frontMotor.set(-Deacquire.CONE_FRONT.doubleValue());
-                backMotor.set(Deacquire.CONE_BACK.doubleValue());
+                frontMotor.set(Deacquire.CONE_FRONT.doubleValue());
+                backMotor.set(-Deacquire.CONE_BACK.doubleValue());
                 break;
             default:
                 break;