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/.github/.gitattributes b/.gitattributes similarity index 100% rename from .github/.gitattributes rename to .gitattributes 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 diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4dd1b15c..4f227243 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 @@ -371,32 +371,39 @@ 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 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_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 + public static final double RESOLUTION_WIDTH_PIX = 640; // unused + 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 + 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/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 06c2e603..e5d8aa51 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; @@ -33,7 +31,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .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 +41,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .minus(Rotation2d.fromRadians(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/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java new file mode 100644 index 00000000..aa78fe17 --- /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.fromRadians(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.fromRadians(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/commands/IntakeNEO.java b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java index 5360739c..2a9b4722 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java @@ -10,42 +10,30 @@ 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(SmartDashboard.getNumber("Intake RPM", speed)); + intake.motorSetIntake(.5); // Fast intake speed for initial intake intake.resetCurrentLimit(); - index=0; } // 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(); - } 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(); } @@ -63,6 +49,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/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/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 6bd70b3c..f2835fb9 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(10, 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,7 +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("Intake target RPM", 0); + // SmartDashboard.putNumber("Vortex volts", 0); } public boolean intakeIsOverTemp() { @@ -141,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()); @@ -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()); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index e172a7ce..068924aa 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,17 +29,31 @@ 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 + // key is distance (meters), value is angle (rads) + shooterMap.put(0.0, 0.0); } @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() { botPose = LimelightHelpers.getBotPose3d(SHOOTER_LL_NAME); @@ -49,7 +67,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); } @@ -61,21 +78,18 @@ 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; } } - public double getDistanceToNoteMeters() { Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE) .plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME))); @@ -89,16 +103,64 @@ 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()); } + // 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_DEG_PER_SEC) { // degrees per second + 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() { + 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() { + 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();