From 8f3bf9d077d11168e5d2aeae2318e7b208d04b90 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Wed, 27 Mar 2024 20:18:35 -0400 Subject: [PATCH] adjusted arm high control, fixed intakeing, maybe zero yaw on auto? --- src/main/java/frc/robot/RobotContainer.java | 22 +++++++++++++------ .../robot/commands/arm/SetArmPosition.java | 2 +- .../robot/commands/intake/IntakeWithArm.java | 2 ++ .../commands/scoring/SetScoringPosition.java | 4 ++-- .../frc/robot/constants/ArmConstants.java | 4 ++-- .../frc/robot/constants/ScoringConstants.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 1 + 7 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ef78cb..8963b71 100644 --- 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 index dc50558..3ae3bbd 100644 --- 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 index 5e5d33d..e14a541 100644 --- 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 index 64f7b75..b58372d 100644 --- 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 index 8da5914..7ce6f05 100644 --- 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 index 996a50d..ebe8b94 100644 --- 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 index 3205726..d7f1da8 100644 --- 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));