From 8bc791e95975f4898ea2e121b8cdfa2b847bdd8f Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Fri, 24 May 2024 16:47:39 -0700 Subject: [PATCH 01/14] All the final intake changes --- .DataLogTool/datalogtool.json | 2 +- src/main/java/org/carlmontrobotics/commands/IntakeNEO.java | 6 +++--- .../org/carlmontrobotics/subsystems/IntakeShooter.java | 7 ++----- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json index 95f2eef4..43f54f60 100644 --- a/.DataLogTool/datalogtool.json +++ b/.DataLogTool/datalogtool.json @@ -1,6 +1,6 @@ { "download": { "localDir": "C:\\Users\\Deep Blue\\Downloads", - "serverTeam": "199" + "serverTeam": "10.1.99.2" } } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java index 5360739c..c986920b 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java @@ -29,7 +29,7 @@ public void initialize() { // if (intake.intakeDetectsNote()) { // return; // } - intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed)); + intake.motorSetIntake(.5); intake.resetCurrentLimit(); index=0; @@ -39,10 +39,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed)); // Intake Led if((intake.intakeDetectsNote())) { timer.start(); + intake.motorSetIntake(.1); } else { timer.stop(); timer.reset(); @@ -63,6 +63,6 @@ public void end(boolean interrupted) { public boolean isFinished() { // return intake.intakeDetectsNote() && timer.get()>0.1; // || //timer.hasElapsed(MAX_SECONDS_OVERLOAD); - return intake.intakeDetectsNote(); + return intake.outtakeDetectsNote(); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 6bd70b3c..c24ae121 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -27,7 +27,7 @@ public class IntakeShooter extends SubsystemBase { private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO); // private final CANSparkMax outakeMotor = // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550); - private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(10, MotorType.kBrushless); + private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(OUTAKE_PORT, MotorType.kBrushless); private final RelativeEncoder outtakeEncoder = outtakeMotorVortex.getEncoder(); private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); private final SparkPIDController pidControllerOutake = outtakeMotorVortex.getPIDController(); @@ -72,6 +72,7 @@ public IntakeShooter() { outtakeDistanceSensor.setRangingMode(RangingMode.Short, 24); outtakeMotorVortex.setSmartCurrentLimit(60); SmartDashboard.putNumber("Intake target RPM", 0); + SmartDashboard.putNumber("Vortex volts", 0); } public boolean intakeIsOverTemp() { @@ -174,10 +175,6 @@ else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) // outakeMotor.set(SmartDashboard.getNumber("intake volts", 0)); // count++; - - // double volts = SmartDashboard.getNumber("Vortex volts", 0); - // outakeMotorVortex.set(volts); - // setMaxOutake(); SmartDashboard.putNumber("Intake amps", intakeMotor.getOutputCurrent()); From c9e0959a78a3bd942157d33dd7cc4ebc2b6c6e64 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 27 May 2024 17:35:08 -0700 Subject: [PATCH 02/14] Use wpilib java formatting style. --- .vscode/settings.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.vscode/settings.json b/.vscode/settings.json index 56e1a4ae..358fc76d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -30,6 +30,7 @@ "java.test.defaultConfig": "WPIlibUnitTests", "wpilib.skipTests": true, "java.debug.settings.onBuildFailureProceed": true, + "java.format.settings.url": "https://raw.githubusercontent.com/wpilibsuite/styleguide/main/ide/eclipse-java-google-style.xml", "editor.formatOnSave": true, "editor.formatOnSaveMode": "modificationsIfAvailable" } \ No newline at end of file From c8252a053ec4d9a0482593b592f5101ac8e73b81 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 27 May 2024 18:14:37 -0700 Subject: [PATCH 03/14] Move .gitattributes to top-level folder where it belongs. --- .github/.gitattributes => .gitattributes | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/.gitattributes => .gitattributes (100%) diff --git a/.github/.gitattributes b/.gitattributes similarity index 100% rename from .github/.gitattributes rename to .gitattributes From 9e563a1c3aaed38a3b9a9c9a90dbe6f25de60a01 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sun, 9 Jun 2024 16:56:02 -0700 Subject: [PATCH 04/14] testing --- .../java/org/carlmontrobotics/Constants.java | 25 +++++++++---------- .../subsystems/Limelight.java | 9 ++++--- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4dd1b15c..57ff9f06 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -371,32 +371,31 @@ public static final class Autoc { 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the // angular constraints have no effect. } - // #endregion } public static final class Limelightc { public static final String INTAKE_LL_NAME = "limelight-intake"; public static final String SHOOTER_LL_NAME = "limelight-shooter"; - public static final double ERROR_TOLERANCE_RAD = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; - public static final double RESOLUTION_WIDTH_PIX = 640; - public static final double MOUNT_ANGLE_DEG_SHOOTER = 42.5; - public static final double MOUNT_ANGLE_DEG_INTAKE = -22; // 23.228 - public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(11.5); // 16.6 - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); // 16.6 - public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; - public static final double NOTE_HEIGHT = Units.inchesToMeters(0); + public static final double ERROR_TOLERANCE_RAD = 0.1; // unused + public static final double HORIZONTAL_FOV_DEG = 0; // unused + public static final double RESOLUTION_WIDTH_PIX = 640; // unused + public static final double MOUNT_ANGLE_DEG_SHOOTER = 10; + public static final double MOUNT_ANGLE_DEG_INTAKE = -22; + public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); + public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused + public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused public static final double MIN_MOVEMENT_METERSPSEC = 0.5; public static final double MIN_MOVEMENT_RADSPSEC = 0.5; public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); - public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(19.5); + public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); - public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); + public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused public static final class Apriltag { public static final int RED_SPEAKER_CENTER_TAG_ID = 4; public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125 + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index e172a7ce..12b85217 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -34,8 +34,12 @@ public void periodic() { getDistanceToSpeakerMeters(); getCurrentPose(); getDistanceToNoteMeters(); - } + SmartDashboard.putBoolean("see apriltag", LimelightHelpers.getTV(SHOOTER_LL_NAME)); + SmartDashboard.putNumber("distance to speaker", getDistanceToSpeakerMeters()); + SmartDashboard.putNumber("rotation to align", getRotateAngleDeg()); + SmartDashboard.putNumber("arm angle based on position", getArmAngleToShootSpeakerRad()); + } public void updateBotPose3d() { botPose = LimelightHelpers.getBotPose3d(SHOOTER_LL_NAME); @@ -49,7 +53,6 @@ public Pose2d getCurrentPose() { return estimatedPos; } - public double getTXDeg(String limelightName) { return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTX(INTAKE_LL_NAME) : -LimelightHelpers.getTY(SHOOTER_LL_NAME); } @@ -75,7 +78,6 @@ public double getDistanceToSpeakerMeters() { } } - public double getDistanceToNoteMeters() { Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE) .plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME))); @@ -100,5 +102,4 @@ public double getRotateAngleDeg() { double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); } - } \ No newline at end of file From 20d9600a2c3d9910203bf153d9d7c81a1342df64 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Sun, 9 Jun 2024 17:45:39 -0700 Subject: [PATCH 05/14] worksession --- src/main/java/org/carlmontrobotics/RobotContainer.java | 4 ++-- src/main/java/org/carlmontrobotics/subsystems/Limelight.java | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index aa4bbc5e..8e98a362 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -192,7 +192,7 @@ private void setBindingsDriver() { .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), new IntakeNEO(intakeShooter))); new JoystickButton(driverController, Driver.rotateFieldRelative0Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); + .onTrue(new AlignToNote(drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(270), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative180Deg) @@ -201,7 +201,7 @@ private void setBindingsDriver() { .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); } private void setBindingsManipulatorNEO() { - new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter)); + new JoystickButton(manipulatorController, EJECT_BUTTON).whileTrue(new AlignToNote(drivetrain)); // test new JoystickButton(manipulatorController, Button.kB.value).whileTrue(new RampMaxRPM(intakeShooter)); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 12b85217..8ccc9c05 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -35,8 +35,9 @@ public void periodic() { getCurrentPose(); getDistanceToNoteMeters(); - SmartDashboard.putBoolean("see apriltag", LimelightHelpers.getTV(SHOOTER_LL_NAME)); - SmartDashboard.putNumber("distance to speaker", getDistanceToSpeakerMeters()); + SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME)); + SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); + SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME)); SmartDashboard.putNumber("rotation to align", getRotateAngleDeg()); SmartDashboard.putNumber("arm angle based on position", getArmAngleToShootSpeakerRad()); } From e8a613bc561f7a4ae885827cae589bea6921a45b Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sun, 9 Jun 2024 22:49:39 -0700 Subject: [PATCH 06/14] updated constants --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- .../org/carlmontrobotics/commands/AlignToApriltag.java | 4 ++-- .../org/carlmontrobotics/subsystems/Limelight.java | 10 ++++------ 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 57ff9f06..471cba73 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -380,8 +380,8 @@ public static final class Limelightc { public static final double ERROR_TOLERANCE_RAD = 0.1; // unused public static final double HORIZONTAL_FOV_DEG = 0; // unused public static final double RESOLUTION_WIDTH_PIX = 640; // unused - public static final double MOUNT_ANGLE_DEG_SHOOTER = 10; - public static final double MOUNT_ANGLE_DEG_INTAKE = -22; + public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; + public static final double MOUNT_ANGLE_DEG_INTAKE = -29; public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 06c2e603..33f63556 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -33,7 +33,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .minus(Rotation2d.fromDegrees(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); @@ -43,7 +43,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .minus(Rotation2d.fromDegrees(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 8ccc9c05..f7124b35 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -35,11 +35,11 @@ public void periodic() { getCurrentPose(); getDistanceToNoteMeters(); + // intake limelight testing SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME)); SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME)); - SmartDashboard.putNumber("rotation to align", getRotateAngleDeg()); - SmartDashboard.putNumber("arm angle based on position", getArmAngleToShootSpeakerRad()); + SmartDashboard.putNumber("rotation to align", getRotateAngleRad()); } public void updateBotPose3d() { @@ -65,14 +65,12 @@ public double getTYDeg(String limelightName) { public double getDistanceToSpeakerMeters() { if (LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID || LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) { - // TODO: change MOUNT_ANGLE_DEG_SHOOTER Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER) .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); //because limelight is mounted horizontally double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) / angleToGoal.getTan(); // SmartDashboard.putNumber("limelight distance", distance); return distance; } - else { // SmartDashboard.putNumber("limelight distance", -1); return -1; @@ -92,13 +90,13 @@ public double getDistanceToNoteMeters() { } } - public double getArmAngleToShootSpeakerRad(){ + public double getArmAngleToShootSpeakerRad() { double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS; double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH; return END_EFFECTOR_BASE_ANGLE_RADS - Math.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters); } - public double getRotateAngleDeg() { + public double getRotateAngleRad() { double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); From 997ab275b40395fa61aa3ab3ad3b0c687c9eea50 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Mon, 10 Jun 2024 10:05:46 -0700 Subject: [PATCH 07/14] updated limelighthelpers --- .../subsystems/LimelightHelpers.java | 227 +++++++++++++++++- 1 file changed, 220 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java index a9da99ef..2c03899d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.carlmontrobotics.subsystems; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -363,8 +375,58 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -382,7 +444,7 @@ static final String sanitizeName(String name) { return name; } - static Pose3d toPose3D(double[] inData){ + private static Pose3d toPose3D(double[] inData) { if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -394,10 +456,10 @@ static Pose3d toPose3D(double[] inData){ Units.degreesToRadians(inData[5]))); } - static Pose2d toPose2D(double[] inData){ + private static Pose2d toPose2D(double[] inData) { if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + // System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +467,83 @@ static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray, 6); + int tagCount = (int) extractBotPoseEntry(poseArray, 7); + double tagSpan = extractBotPoseEntry(poseArray, 8); + double tagDist = extractBotPoseEntry(poseArray, 9); + double tagArea = extractBotPoseEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +683,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +743,30 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +781,30 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +830,10 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -696,6 +887,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -765,7 +978,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); From f27ed962a95024700892ff6b9598c476a8ced839 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Wed, 12 Jun 2024 09:41:43 -0700 Subject: [PATCH 08/14] incorporated megatag2 into calculations involving shooter limelight --- .../java/org/carlmontrobotics/Constants.java | 8 + .../commands/AimArmSpeaker.java | 5 +- .../commands/AimArmSpeakerMT2.java | 44 ++++ .../commands/AlignToApriltag.java | 2 - .../commands/AlignToApriltagMegatag2.java | 62 +++++ .../org/carlmontrobotics/subsystems/Arm.java | 3 + .../subsystems/Drivetrain.java | 3 + .../subsystems/Limelight.java | 62 ++++- .../subsystems/LimelightHelpers.java | 227 +++++++++++++++++- 9 files changed, 400 insertions(+), 16 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java create mode 100644 src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 471cba73..dd750c4d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -377,6 +377,14 @@ public static final class Limelightc { public static final String INTAKE_LL_NAME = "limelight-intake"; public static final String SHOOTER_LL_NAME = "limelight-shooter"; + public static final int[] VALID_IDS = { 4, 7 }; + + public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + // to represent unreliable heading + public static final int MAX_TRUSTED_ANG_VEL = 720; // maximum trusted angular velocity + public static final double ERROR_TOLERANCE_RAD = 0.1; // unused public static final double HORIZONTAL_FOV_DEG = 0; // unused public static final double RESOLUTION_WIDTH_PIX = 640; // unused diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java index ba2fe607..085ac066 100644 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -6,9 +6,6 @@ import org.carlmontrobotics.subsystems.Arm; import org.carlmontrobotics.subsystems.Limelight; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class AimArmSpeaker extends Command { @@ -41,6 +38,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return arm.armAtSetpoint(); } } diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java new file mode 100644 index 00000000..329c0b6b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Arm; +import org.carlmontrobotics.subsystems.Limelight; +import edu.wpi.first.wpilibj2.command.Command; + +public class AimArmSpeakerMT2 extends Command { + private final Arm arm; + private final Limelight ll; + + /** Creates a new AimOuttakeSpeaker. */ + public AimArmSpeakerMT2(Arm arm, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.arm = arm); + this.ll = ll; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double goal = ll.getOptimizedArmAngleRadsMT2(); + arm.setArmTarget(goal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return arm.armAtSetpoint(); + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 33f63556..19f57c20 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -5,11 +5,9 @@ package org.carlmontrobotics.commands; import static org.carlmontrobotics.Constants.Drivetrainc.*; -import static org.carlmontrobotics.Constants.Limelightc.*; import org.carlmontrobotics.subsystems.Drivetrain; import org.carlmontrobotics.subsystems.Limelight; -import org.carlmontrobotics.subsystems.LimelightHelpers; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java new file mode 100644 index 00000000..f4776fad --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj2.command.Command; + +public class AlignToApriltagMegatag2 extends Command { + + public final TeleopDrive teleopDrive; + public final Drivetrain drivetrain; + private Limelight limelight; + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) { + this.limelight = limelight; + this.drivetrain = drivetrain; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) + .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2())); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + addRequirements(drivetrain); + } + + @Override + public void execute() { + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) + .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2())); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + return false; + // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + // return rotationPID.atSetpoint(); + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 6b2d4ed5..096c7790 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -175,6 +175,9 @@ public void setBooleanDrive(boolean climb) { @Override public void periodic() { + + SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing + babyMode = SmartDashboard.getBoolean("babymode", false); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 970523e1..5dc506a8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -1004,5 +1004,8 @@ public void keepRotateMotorsAtDegrees(int angle) { } } + public double getGyroRate() { + return gyro.getRate(); + } // #endregion } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index f7124b35..7bda03e5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -3,10 +3,12 @@ import static org.carlmontrobotics.Constants.Limelightc.*; import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,6 +20,8 @@ public class Limelight extends SubsystemBase { // private double[] targetPose = null; private Pose3d botPose; + private final InterpolatingDoubleTreeMap shooterMap; + public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; poseEstimator = new SwerveDrivePoseEstimator( @@ -25,21 +29,29 @@ public Limelight(Drivetrain drivetrain) { Rotation2d.fromDegrees(drivetrain.getHeading()), drivetrain.getModulePositions(), new Pose2d()); + + LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS); + + shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing + shooterMap.put(0.0, 0.0); // key is distance (meters), value is angle (rads) } @Override public void periodic() { poseEstimator.update(Rotation2d.fromDegrees(drivetrain.getHeading()), drivetrain.getModulePositions()); updateBotPose3d(); - getDistanceToSpeakerMeters(); - getCurrentPose(); - getDistanceToNoteMeters(); + + updateMT2Odometry(); // intake limelight testing SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME)); SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME)); SmartDashboard.putNumber("rotation to align", getRotateAngleRad()); + + // shooter limelight testing + SmartDashboard.putNumber("distance to speaker (meters)", getDistanceToSpeakerMetersMT2()); + SmartDashboard.putNumber("optimized arm angle", getArmAngleToShootSpeakerRad()); } public void updateBotPose3d() { @@ -101,4 +113,48 @@ public double getRotateAngleRad() { double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); } + + // megatag2 + + public void updateMT2Odometry() { + boolean rejectVisionUpdate = false; + + LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, + poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers + .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); + + if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL) { + rejectVisionUpdate = true; + } + + if (visionPoseEstimate.tagCount == 0) { + rejectVisionUpdate = true; + } + + if (!rejectVisionUpdate) { + poseEstimator + .setVisionMeasurementStdDevs(VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, STD_DEV_HEADING_RADS)); + poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); + } + } + + public double getRotateAngleRadMT2() { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target + + double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the robot and target + double targetY = targetPoseRobotSpace.getY(); // the sideways offset + + double targetOffsetRads = Math.atan2(targetY, targetX); + + return targetOffsetRads; + } + + public double getDistanceToSpeakerMetersMT2() { + return LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME).getX(); + } + + public double getOptimizedArmAngleRadsMT2() { + return shooterMap.get(getDistanceToSpeakerMetersMT2()); + } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java index a9da99ef..2c03899d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.carlmontrobotics.subsystems; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -363,8 +375,58 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -382,7 +444,7 @@ static final String sanitizeName(String name) { return name; } - static Pose3d toPose3D(double[] inData){ + private static Pose3d toPose3D(double[] inData) { if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -394,10 +456,10 @@ static Pose3d toPose3D(double[] inData){ Units.degreesToRadians(inData[5]))); } - static Pose2d toPose2D(double[] inData){ + private static Pose2d toPose2D(double[] inData) { if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + // System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +467,83 @@ static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray, 6); + int tagCount = (int) extractBotPoseEntry(poseArray, 7); + double tagSpan = extractBotPoseEntry(poseArray, 8); + double tagDist = extractBotPoseEntry(poseArray, 9); + double tagArea = extractBotPoseEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +683,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +743,30 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +781,30 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +830,10 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -696,6 +887,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -765,7 +978,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); From ea164bbc205b1e4097acae6610367b3a2463cf69 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Wed, 12 Jun 2024 19:32:34 -0700 Subject: [PATCH 09/14] fixed target angle units --- .../java/org/carlmontrobotics/commands/AlignToApriltag.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 33f63556..a4efdcce 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -33,7 +33,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleRad())); + .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); @@ -43,7 +43,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleRad())); + .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); From 3ad478039850cf91dd0e6e97d108b2eac0c76b77 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Wed, 12 Jun 2024 22:49:40 -0700 Subject: [PATCH 10/14] original controls --- src/main/java/org/carlmontrobotics/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 8e98a362..aa4bbc5e 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -192,7 +192,7 @@ private void setBindingsDriver() { .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), new IntakeNEO(intakeShooter))); new JoystickButton(driverController, Driver.rotateFieldRelative0Deg) - .onTrue(new AlignToNote(drivetrain)); + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(270), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative180Deg) @@ -201,7 +201,7 @@ private void setBindingsDriver() { .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); } private void setBindingsManipulatorNEO() { - new JoystickButton(manipulatorController, EJECT_BUTTON).whileTrue(new AlignToNote(drivetrain)); // test + new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter)); new JoystickButton(manipulatorController, Button.kB.value).whileTrue(new RampMaxRPM(intakeShooter)); From a51458e9948c4eac7f8111004726c470463603d8 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Wed, 12 Jun 2024 23:49:58 -0700 Subject: [PATCH 11/14] changed distance math --- .../java/org/carlmontrobotics/subsystems/Limelight.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index beb335fa..53a3e5b1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -152,7 +152,12 @@ public double getRotateAngleRadMT2() { } public double getDistanceToSpeakerMetersMT2() { - return LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME).getX(); + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); + + double x = targetPoseRobotSpace.getX(); + double y = targetPoseRobotSpace.getY(); + + return Math.sqrt(x * x + y * y); } public double getOptimizedArmAngleRadsMT2() { From 7d21c52ab05093b611c67a5504aee0f70a849f1b Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Thu, 13 Jun 2024 10:23:31 -0700 Subject: [PATCH 12/14] small things --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- .../carlmontrobotics/commands/AlignToApriltagMegatag2.java | 4 ++-- src/main/java/org/carlmontrobotics/subsystems/Limelight.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 58fb2f6d..03b7086c 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -383,7 +383,7 @@ public static final class Limelightc { public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high // to represent unreliable heading - public static final int MAX_TRUSTED_ANG_VEL_DEGSPSEC = 720; // maximum trusted angular velocity + public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity public static final double ERROR_TOLERANCE_RAD = 0.1; // unused public static final double HORIZONTAL_FOV_DEG = 0; // unused diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java index f4776fad..aa78fe17 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java @@ -31,7 +31,7 @@ public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) { rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2())); + .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); @@ -41,7 +41,7 @@ public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2())); + .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 53a3e5b1..068924aa 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -125,7 +125,7 @@ public void updateMT2Odometry() { LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); - if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEGSPSEC) { // degrees per second + if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second rejectVisionUpdate = true; } From fb1a8ab9ffa864d8ac133b33b336da92d01b0fa5 Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 13 Jun 2024 11:42:19 -0700 Subject: [PATCH 13/14] Fixed Spelling, commented out some smartdashboard stuff, and removed unused variables that deal with intake --- .../java/org/carlmontrobotics/Constants.java | 2 +- .../carlmontrobotics/commands/IntakeNEO.java | 20 +++---------------- .../subsystems/IntakeShooter.java | 12 +++++------ 3 files changed, 10 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4dd1b15c..40e68541 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -59,7 +59,7 @@ public static final class Effectorc { public static final double[] kV = { 0.122, 0/* 0.065239, 0.077913 */ }; public static final double[] kA = { 0, 0/* 0.0062809,0.05289 */ }; public static final int INTAKE_PORT = 9; // port - public static final int OUTAKE_PORT = 10; // port + public static final int OUTTAKE_PORT = 10; // port public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java index c986920b..2a9b4722 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java @@ -10,29 +10,21 @@ public class IntakeNEO extends Command { // intake until sees game peice or 4sec has passed - private Timer timer = new Timer(); private final IntakeShooter intake; - double increaseAmount = 0.05; - int index = 0; - public int speed; public IntakeNEO(IntakeShooter intake) { addRequirements(this.intake = intake); - SmartDashboard.putNumber("Intake RPM", speed); - } @Override public void initialize() { - //TODO: Adjust speed or add in an index - timer.reset(); + // TODO: Adjust speed or add in an index; // if (intake.intakeDetectsNote()) { // return; // } - intake.motorSetIntake(.5); + intake.motorSetIntake(.5); // Fast intake speed for initial intake intake.resetCurrentLimit(); - index=0; } @@ -41,11 +33,7 @@ public void initialize() { public void execute() { // Intake Led if((intake.intakeDetectsNote())) { - timer.start(); - intake.motorSetIntake(.1); - } else { - timer.stop(); - timer.reset(); + intake.motorSetIntake(.1); // Slower intake speed triggered after intake ds sees note } } @@ -53,8 +41,6 @@ public void execute() { @Override public void end(boolean interrupted) { intake.stopIntake(); - timer.stop(); - index = 0; //intake.resetCurrentLimit(); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index c24ae121..519471c6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -27,7 +27,8 @@ public class IntakeShooter extends SubsystemBase { private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO); // private final CANSparkMax outakeMotor = // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550); - private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(OUTAKE_PORT, MotorType.kBrushless); + private final CANSparkFlex outtakeMotorVortex = + new CANSparkFlex(OUTTAKE_PORT, MotorType.kBrushless); private final RelativeEncoder outtakeEncoder = outtakeMotorVortex.getEncoder(); private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); private final SparkPIDController pidControllerOutake = outtakeMotorVortex.getPIDController(); @@ -36,7 +37,6 @@ public class IntakeShooter extends SubsystemBase { private Timer intakeTOFTimer = new Timer(); private Timer outtakeTOFTimer = new Timer(); private int count = 0; - private StringLogEntry tofLogEntry; private SimpleMotorFeedforward intakeFeedforward = new SimpleMotorFeedforward(kS[INTAKE], kV[INTAKE], kA[INTAKE]); private final SimpleMotorFeedforward outtakeFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], @@ -61,8 +61,8 @@ public IntakeShooter() { pidControllerIntake.setI(kI[INTAKE]); pidControllerIntake.setD(kD[INTAKE]); SmartDashboard.putData("Intake Shooter", this); - SmartDashboard.putNumber("Intake Ks", kS[INTAKE]); - SmartDashboard.putNumber("Intake Kv", kV[INTAKE]); + // SmartDashboard.putNumber("Intake Ks", kS[INTAKE]); + // SmartDashboard.putNumber("Intake Kv", kV[INTAKE]); intakeEncoder.setAverageDepth(4); intakeEncoder.setMeasurementPeriod(8); // SmartDashboard.putNumber("intake volts", 0); @@ -71,8 +71,8 @@ public IntakeShooter() { intakeDistanceSensor.setRangingMode(RangingMode.Short, 24);// 24 ms is the minimum sample time acc to docs outtakeDistanceSensor.setRangingMode(RangingMode.Short, 24); outtakeMotorVortex.setSmartCurrentLimit(60); - SmartDashboard.putNumber("Intake target RPM", 0); - SmartDashboard.putNumber("Vortex volts", 0); + // SmartDashboard.putNumber("Intake target RPM", 0); + // SmartDashboard.putNumber("Vortex volts", 0); } public boolean intakeIsOverTemp() { From 4633049397d1c04166e41e2ffdb5fc97c706bfe2 Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 13 Jun 2024 11:55:49 -0700 Subject: [PATCH 14/14] commented out more smart dashboard stuff --- .../org/carlmontrobotics/subsystems/IntakeShooter.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 519471c6..f2835fb9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -142,12 +142,12 @@ public void updateValues() { @Override public void periodic() { updateValues(); - double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]); - double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]); + // double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]); + /// double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]); - if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) { - intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV); - } + // if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) { + // intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV); + // } SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote()); SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote()); SmartDashboard.putNumber("intake sample rate", intakeDistanceSensor.getSampleTime());