gamepiece vision tuning and also climber!

src/main/java/frc/robot/Robot.java

@@ -156,6 +156,8 @@ public void teleopInit() {
 		if (autonomousCommand != null) {
 			autonomousCommand.cancel();
 		}
+
+		robotContainer.arm.setPosition(5);
 	}
 
 	/** This function is called periodically during operator control. */

src/main/java/frc/robot/RobotContainer.java

@@ -1,6 +1,5 @@
 package frc.robot;
 
-import static frc.robot.constants.Constants.tuningMode;
 import static frc.robot.constants.Constants.OperatorConstants.*;
 
 import frc.robot.constants.Constants;
@@ -32,6 +31,7 @@
 import frc.robot.commands.vision.GamePieceVision;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Relay.Value;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -166,7 +166,7 @@ private void configureBindings() {
 		// right bumper -> rotate to speaker apriltag
 		// driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve));
 		// left bumper -> rotate to note
-		driverController.leftBumper().onTrue(new GamePieceVision(vision, swerve));
+		driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve));
 		driverController.a().onTrue(teleopSwerve.toggleFieldRelative());
 		// b -> trap/climb align maybe?
 		driverController.x().whileTrue(new XStance(swerve));
@@ -197,7 +197,7 @@ private void configureBindings() {
 		//
 		// When in tuning mode, create multiple testing options on shuffleboard as well as bind commands to a unique
 		// 'testing' controller
-		if (tuningMode) {
+		if (Constants.tuningMode) {
 			var indexSpeedEntry = Shuffleboard.getTab("Test").add("Index Speed", 0)
 				.withWidget(BuiltInWidgets.kNumberSlider)
 				.withProperties(Map.of("min", -1, "max", 1)).getEntry();
@@ -228,6 +228,10 @@ private void configureBindings() {
 
 			testController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true));
 			testController.rightStick().whileTrue(new ManualArmControl(arm, testController::getLeftY));
+
+			testController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm));
+			testController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm));
+			testController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm));
 		}
 	}
 

src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java

@@ -41,8 +41,9 @@ public SetIntakeSpeed(Intake intake, boolean isIndexing) {
 	public void initialize() {
 		// Run motor only if we are either indexing or don't detect a note. Or, if we are intaking (not indexing) and we
 		// detect a note, don't run
-		if (isIndexing || !intake.detectNote())
-			intake.setMotorSpeed(speed);
+		if (isIndexing || !intake.isHoldingNote())
+			;
+		intake.setMotorSpeed(speed);
 	}
 
 	// Called every time the scheduler runs while the command is scheduled.
@@ -60,6 +61,6 @@ public void end(boolean interrupted) {
 	public boolean isFinished() {
 		// If we are intaking and we detect a note, cancel automatically. If we are indexing, the command will only be
 		// canceled on interrupt
-		return (!isIndexing && intake.detectNote());
+		return (!isIndexing && intake.inputs.noteSensor);
 	}
 }

src/main/java/frc/robot/commands/vision/GamePieceVision.java

@@ -8,6 +8,8 @@
 import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.Vision;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 
@@ -18,6 +20,8 @@ public class GamePieceVision extends Command {
 	private Swerve swerve;
 	private double robotRotate_radps;
 	private boolean isAligned;
+	private PIDController pidController = new PIDController(VisionConstants.gamePieceRotateKp, 0,
+		VisionConstants.gamePieceRotateKd);
 
 	/** Creates a new GamePieceVision. */
 	public GamePieceVision(Vision vision, Swerve swerve) {
@@ -43,19 +47,25 @@ public void execute() {
 		Logger.recordOutput("GamePieceVision/isAligned", isAligned);
 
 		// When we see a ground GamePiece, we will rotate to it
-		if (vision.pieceInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) {
-			robotRotate_radps = VisionConstants.gamePieceRotateKp
-				* vision.pieceInputs.crosshairToTargetErrorX_rad
-				- VisionConstants.gamePieceRotateKd;
-		} else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.pieceInputs.crosshairToTargetErrorX_rad) {
-			robotRotate_radps = VisionConstants.gamePieceRotateKp
-				* vision.pieceInputs.crosshairToTargetErrorX_rad
-				+ VisionConstants.gamePieceRotateKd;
+		// if (vision.pieceInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) {
+		// robotRotate_radps = VisionConstants.gamePieceRotateKp
+		// * vision.pieceInputs.crosshairToTargetErrorX_rad
+		// - VisionConstants.gamePieceRotateKd;
+		// } else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.pieceInputs.crosshairToTargetErrorX_rad) {
+		// robotRotate_radps = VisionConstants.gamePieceRotateKp
+		// * vision.pieceInputs.crosshairToTargetErrorX_rad
+		// + VisionConstants.gamePieceRotateKd;
+		// }
+		// robotRotate_radps *= -1; // Rotate opposite of error
+		if (!MathUtil.isNear(0, vision.pieceInputs.crosshairToTargetErrorX_rad,
+			VisionConstants.crosshairGamePieceBoundRotateX_rad)) {
+			robotRotate_radps = pidController.calculate(vision.pieceInputs.crosshairToTargetErrorX_rad, 0);
+		} else {
+			robotRotate_radps = 0;
 		}
-		robotRotate_radps *= -1; // Rotate opposite of error
-
 		// Generate a continuously updated rotation to GamePiece
 		System.out.println("Swerve --> GamePiece");
+		Logger.recordOutput("Vision/targetRotation", robotRotate_radps);
 		swerve.drive(new Translation2d(0, 0), robotRotate_radps, false); // TELL DRIVE THAT THEY ARE ALWAYS ROBOT ORIENTED
 																																			// WHEN ALIGN TO NOTE
 	}

src/main/java/frc/robot/constants/VisionConstants.java

@@ -5,13 +5,13 @@ public class VisionConstants {
 	public static final double poseError_m = 1; // comparing visionPose to pose
 	public static final double botpose_fieldOffsetX_m = 0.025; // realife offset to pathplanner app
 	/* ObjectDetectionVision */
-	public static final double gamePieceRotateKp = 1.2;
-	public static final double gamePieceRotateKd = 0.34;
+	public static final double gamePieceRotateKp = 1.75;
+	public static final double gamePieceRotateKd = 1.4;
 
 	public static final double AprilTagRotateKp = 1.2;
 	public static final double AprilTagRotateKd = 0.34;
 
-	public static final double crosshairGamePieceBoundRotateX_rad = Math.toRadians(1.0);
+	public static final double crosshairGamePieceBoundRotateX_rad = Math.toRadians(1.5);
 
 	public static final int redSpeakerTag = 4;
 	public static final int blueSpeakerTag = 7;

src/main/java/frc/robot/io/PieceVisionIO.java

@@ -42,7 +42,7 @@ public static class PieceVisionIOInputs {
 	}
 
 	// Network table declarations
-	private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-gp");
+	private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-gpv");
 
 	private NetworkTableEntry pipelineEntry = table.getEntry("pipeline");
 	private NetworkTableEntry tvEntry = table.getEntry("tv");

src/main/java/frc/robot/subsystems/Arm.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import com.revrobotics.AbsoluteEncoder;
@@ -36,6 +37,8 @@ public class Arm extends SubsystemBase {
 
 	private ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA);
 
+	private Relay climberRelay = new Relay(0);
+
 	@Getter
 	@AutoLogOutput
 	private double targetPosition_deg = 0;
@@ -150,6 +153,11 @@ public void setMotorVoltage(double voltage_V) {
 			feedforwardController.calculate(Units.degreesToRadians(inputs.shaftPosition_deg), 0));
 	}
 
+	public void setClimberRelay(Relay.Value value) {
+		climberRelay.set(value);
+		Logger.recordOutput("Arm/relaySetting", value);
+	}
+
 	public void stop() {
 		pidController.setReference(0, ControlType.kDutyCycle, 0,
 			feedforwardController.calculate(Units.degreesToRadians(inputs.shaftPosition_deg), 0));

src/main/java/frc/robot/subsystems/Intake.java

@@ -11,13 +11,24 @@
 import com.revrobotics.CANSparkMax;
 
 import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+import lombok.Getter;
+import lombok.Setter;
+
 public class Intake extends SubsystemBase {
 	private final CANSparkMax motor = new CANSparkMax(motorId, MotorType.kBrushless);
 
 	private final DigitalInput noteSensor = new DigitalInput(sensorId);
 
+	@Getter
+	@Setter
+	@AutoLogOutput
+	private boolean holdingNote = false;
+
+	private boolean cachedNoteSensor = false;
+
 	public Intake() {
 		motor.restoreFactoryDefaults();
 		motor.setIdleMode(IdleMode.kBrake);
@@ -30,6 +41,12 @@ public Intake() {
 	public void periodic() {
 		updateInputs(inputs);
 		Logger.processInputs(getName(), inputs);
+
+		if ((inputs.noteSensor != cachedNoteSensor) && inputs.noteSensor) {
+			holdingNote = !holdingNote;
+		}
+
+		cachedNoteSensor = inputs.noteSensor;
 	}
 
 	public void setMotorSpeed(double speed) {
@@ -56,11 +73,6 @@ public void updateInputs(IntakeIOInputs inputs) {
 		inputs.noteSensor = !noteSensor.get();
 	}
 
-	public boolean detectNote() {
-		return inputs.noteSensor;
-
-	}
-
 	public void stopMotor() {
 		motor.stopMotor();
 	}