adjusted arm high control, fixed intakeing, maybe zero yaw on auto?

FIRSTTeam102 · Mar 28, 2024 · 8f3bf9d · 8f3bf9d
1 parent e63e371
commit 8f3bf9d
Show file tree

Hide file tree

Showing 7 changed files with 24 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -33,12 +33,13 @@
 import frc.robot.commands.vision.AprilTagVision;
 import frc.robot.commands.vision.GamePieceVision;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
@@ -114,15 +115,22 @@ public RobotContainer() {
 			.withSize(4, 1).withPosition(0, 5);
 		driveTab.add("alerts", Alert.getAlertsSendable())
 			.withSize(5, 4).withPosition(4, 5).withWidget(Alert.widgetName);
-		driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.12:5800/stream.mjpg"))
-			.withProperties(Map.of("show crosshair", false, "show controls", false))
-			.withWidget(BuiltInWidgets.kCameraStream)
-			.withSize(11, 5)
-			.withPosition(0, 0);
+		// driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.12:5800/stream.mjpg"))
+		// .withProperties(Map.of("show crosshair", false, "show controls", false))
+		// .withWidget(BuiltInWidgets.kCameraStream)
+		// .withSize(11, 5)
+		// .withPosition(0, 0);
+		var camera = CameraServer.startAutomaticCapture();
+		camera.setResolution(160, 120);
+		camera.setFPS(15);
+		camera.setPixelFormat(PixelFormat.kMJPEG);
+		driveTab.add("camera", camera).withWidget(BuiltInWidgets.kCameraStream).withProperties(Map.of("comp", 50))
+			.withPosition(0, 0).withSize(11, 5);
 
 		Command shuffleboardAutoOptions = Commands.parallel(
 			Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0),
-			Commands.runOnce(() -> intake.resetNoteDetection(noteStartEntry.getBoolean(false))));
+			Commands.runOnce(() -> intake.resetNoteDetection(noteStartEntry.getBoolean(false))),
+			Commands.runOnce(() -> swerve.gyroIO.setYaw(swerve.getPose().getRotation().getDegrees())));
 
 		// named commands must be registered before any paths are created
 		NamedCommands.registerCommand("Options", shuffleboardAutoOptions);

diff --git a/src/main/java/frc/robot/commands/arm/SetArmPosition.java b/src/main/java/frc/robot/commands/arm/SetArmPosition.java
@@ -38,7 +38,7 @@ public void execute() {
 	@Override
 	public void end(boolean interrupted) {
 		// only stop if interrupted. Otherwise, continue adjusting arm position just move on to another command
-		if (interrupted) {
+		if (interrupted || targetPosition_deg >= 65) {
 			arm.stop();
 		}
 	}

diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.intake;
 
+import frc.robot.constants.IntakeConstants;
 import frc.robot.constants.LightsConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Intake;
@@ -24,6 +25,7 @@ private IntakeWithArm(Intake intake, Arm arm) {
 	public void initialize() {
 		if (!intake.isHoldingNote()) {
 			arm.setPosition(-2);
+			intake.setMotorSpeed(IntakeConstants.intakeSpeed);
 			Lights.setStatus(LightsConstants.Mode.Intaking);
 		}
 	}

diff --git a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java
@@ -54,10 +54,10 @@ public void execute() {
 	// Called once the command ends or is interrupted.
 	@Override
 	public void end(boolean interrupted) {
-		if (interrupted) {
+		if (interrupted)
 			shooter.stop();
+		if (interrupted || targetPosition.armAngle_deg() >= 65)
 			arm.stop();
-		}
 	}
 
 	// Returns true when the command should end.

diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -11,8 +11,8 @@ public final class ArmConstants {
 	public static final double kI = 0;
 	public static final double kD = .00000165;
 
-	public static final double kPHigh = 0.000067;
-	public static final double kDHigh = 0.00000225;
+	public static final double kPHigh = 0.000047;
+	public static final double kDHigh = 0.00000725;
 
 	public static final double maxOutput = .5;
 	public static final double minOutput = -maxOutput;

diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java
@@ -16,7 +16,7 @@ public record ScoringPosition(double armAngle_deg, double shooterSpeed_rpm) {};
 		scoringMap.put(3.227, new ScoringPosition(19, 3450));
 	}
 
-	public static final ScoringPosition ampPosition = new ScoringPosition(87, 1750);
+	public static final ScoringPosition ampPosition = new ScoringPosition(90, 1750);
 	public static final ScoringPosition subwooferPosition = new ScoringPosition(-1.5, 3250);
 	public static final ScoringPosition carryPosition = new ScoringPosition(40, 0);
 	public static final ScoringPosition lowCarryPosition = new ScoringPosition(4, 0);

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -151,6 +151,7 @@ private void updateInputs(ArmIOInputs inputs) {
 	}
 
 	public void setPosition(double position_deg, int pidSlot) {
+		Logger.recordOutput("Arm/pidSlot", pidSlot);
 		targetPosition_deg = position_deg;
 		pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, pidSlot,
 			feedforwardController.calculate(Units.degreesToRadians(targetPosition_deg), 0));