diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index fd5bf811..4f227243 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_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 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 a4efdcce..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; 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/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..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,21 +29,30 @@ 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() { @@ -101,4 +114,53 @@ 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_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